Fix integration accumulator in motor control

src/motor-esp32/motor_control.cpp

@@ -37,7 +37,7 @@ static void motor_control_task(void *pvParameters) {
     float actual_speed;
     float error;
     float d_error;
-    //float duty_correction;
+    // float duty_correction;
     float mapped_duty;
 
     xLastWakeTime = xTaskGetTickCount();
@@ -50,35 +50,41 @@ static void motor_control_task(void *pvParameters) {
 
             if (m_ctrl.enabled) {
                 float new_i_error;
+                float old_motor_duty;
                 actual_speed = get_roller_speed((roller_id_t)m_id);
                 error = m_ctrl.desired_speed - actual_speed;
                 new_i_error = m_status[m_id].i_error + (error * MC_ITERATION_TIME);
                 d_error = (error - m_status[m_id].last_error) / MC_ITERATION_TIME;
                 m_status[m_id].last_error = error;
 
+                old_motor_duty = m_status[m_id].duty;
+
                 m_status[m_id].duty = m_param.kp * error + m_param.ki * m_status[m_id].i_error + m_param.kd * d_error;
 
                 // Clip duty correction to the maximum allowed ramp
-                //if (duty_correction > (MAX_DUTY_RAMP * MC_ITERATION_TIME)) {
-                //    duty_correction = (MAX_DUTY_RAMP * MC_ITERATION_TIME);
-                //} else if (duty_correction < -(MAX_DUTY_RAMP * MC_ITERATION_TIME)) {
-                //    duty_correction = -(MAX_DUTY_RAMP * MC_ITERATION_TIME);
-                //}
-
-                // Apply duty correction, and clip
-                //m_status[m_id].duty += duty_correction;
-                
+                if ((m_status[m_id].duty - old_motor_duty) > (MAX_DUTY_RAMP * MC_ITERATION_TIME)) {
+                    m_status[m_id].duty = old_motor_duty + (MAX_DUTY_RAMP * MC_ITERATION_TIME);
+                } else if ((m_status[m_id].duty - old_motor_duty) < (MAX_DUTY_RAMP * MC_ITERATION_TIME)) {
+                    m_status[m_id].duty = old_motor_duty - (MAX_DUTY_RAMP * MC_ITERATION_TIME);
+                }
+
                 // Clip motor duty output if it's saturated
+                // Only add to the integrated error accumulator if we're not saturated (anti-windup), or the change
+                // would bring away from saturation
                 if (m_status[m_id].duty < 0.0) {
                     m_status[m_id].duty = 0.0;
+                    if (new_i_error > m_status[m_id].i_error) {
+                        m_status[m_id].i_error = new_i_error;
+                    }
                 } else if (m_status[m_id].duty > 1.0) {
                     m_status[m_id].duty = 1.0;
+                    if (new_i_error < m_status[m_id].i_error) {
+                        m_status[m_id].i_error = new_i_error;
+                    }
                 } else {
-                    //only add to the integrated error accumulator if we're not saturated (anti-windup)
                     m_status[m_id].i_error = new_i_error;
                 }
 
-
                 // Map duty to fit between min and tri-phase limit for actual motor PWM
                 mapped_duty = 0.33 * (m_status[m_id].duty * (MAX_DUTY - m_param.min_duty) + m_param.min_duty);
                 // This function takes duty as a float percent...

src/motor-esp32/motor_control.h

@@ -12,7 +12,7 @@
 #define MAX_DUTY 0.99 // max duty in each phase - have slightly below 1.0 to give more dead time between phases
 
 #define DEFAULT_KP       0.0
-#define DEFAULT_KI       0.5
+#define DEFAULT_KI       5.0
 #define DEFAULT_KD       0.0
 #define DEFAULT_MIN_DUTY 0.1