diff --git a/include/motor.h b/include/motor.h index d80bf04..d1275dd 100644 --- a/include/motor.h +++ b/include/motor.h @@ -41,6 +41,10 @@ private: unsigned long _stallStartTime = 0; void (*_stallCallback)(float current) = nullptr; + // ADC zero-current offset (calibrated at startup) + int _adcOffsetRight = 0; + int _adcOffsetLeft = 0; + // Pingpong state bool _pingpongActive = false; int _pingpongBaseSpeed = 50; @@ -55,6 +59,7 @@ private: void applyMotorState(); float readCurrentSense(int pin); + void calibrateCurrentOffset(); void checkStall(); void updatePingpong(); int applyRandomness(int baseValue, int randomPercent); diff --git a/src/motor.cpp b/src/motor.cpp index 7906768..ca2d886 100644 --- a/src/motor.cpp +++ b/src/motor.cpp @@ -3,6 +3,9 @@ // Set to true to enable current sensing (requires R_IS and L_IS connected) #define CURRENT_SENSING_ENABLED true +// Number of samples for offset calibration +#define OFFSET_CALIBRATION_SAMPLES 16 + MotorController motor; void MotorController::begin() { @@ -28,6 +31,9 @@ void MotorController::begin() { analogSetAttenuation(ADC_11db); // Full range 0-3.3V pinMode(R_IS_PIN, INPUT); pinMode(L_IS_PIN, INPUT); + + // Calibrate zero-current offset (ESP32 ADC has inherent offset) + calibrateCurrentOffset(); Serial.println("Current sensing enabled"); #endif @@ -148,6 +154,11 @@ float MotorController::readCurrentSense(int pin) { // Read ADC value (12-bit, 0-4095) int adcValue = analogRead(pin); + // Subtract zero-current offset (calibrated at startup) + int offset = (pin == R_IS_PIN) ? _adcOffsetRight : _adcOffsetLeft; + adcValue = adcValue - offset; + if (adcValue < 0) adcValue = 0; // Clamp to zero + // Convert to voltage (0-3.3V) float voltage = (adcValue / ADC_MAX) * ADC_VREF; @@ -166,6 +177,26 @@ float MotorController::readCurrentSense(int pin) { #endif } +void MotorController::calibrateCurrentOffset() { +#if CURRENT_SENSING_ENABLED + // Sample ADC multiple times and average to get stable offset + long sumRight = 0; + long sumLeft = 0; + + for (int i = 0; i < OFFSET_CALIBRATION_SAMPLES; i++) { + sumRight += analogRead(R_IS_PIN); + sumLeft += analogRead(L_IS_PIN); + delay(5); // Small delay between samples + } + + _adcOffsetRight = sumRight / OFFSET_CALIBRATION_SAMPLES; + _adcOffsetLeft = sumLeft / OFFSET_CALIBRATION_SAMPLES; + + Serial.printf("Current sense offset calibrated: R=%d L=%d (ADC counts)\n", + _adcOffsetRight, _adcOffsetLeft); +#endif +} + // Pingpong mode implementation void MotorController::startPingpong(int speed, int timeMs, int speedRandomPercent, int timeRandomPercent, bool useStallReturn) { _pingpongBaseSpeed = constrain(speed, 0, 100);