Move to non-integrating PID control system

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();
@@ -49,31 +49,37 @@ static void motor_control_task(void *pvParameters) {
             xQueuePeek(motor_parameters_queue[m_id], &m_param, 0);
 
             if (m_ctrl.enabled) {
-
+                float new_i_error;
                 actual_speed = get_roller_speed((roller_id_t)m_id);
                 error = m_ctrl.desired_speed - actual_speed;
-                m_status[m_id].i_error += error * MC_ITERATION_TIME;
+                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;
 
-                duty_correction = m_param.kp * error + m_param.ki * m_status[m_id].i_error + m_param.kd * d_error;
+                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);
-                }
+                //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;
+                //m_status[m_id].duty += duty_correction;
+                
+                // Clip motor duty output if it's saturated
                 if (m_status[m_id].duty < 0.0) {
                     m_status[m_id].duty = 0.0;
                 } else if (m_status[m_id].duty > 1.0) {
                     m_status[m_id].duty = 1.0;
+                } 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 for actual motor PWM
+
+                // 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...
                 mcpwm_set_duty(MCPWM_UNIT_0, motor_timer[m_id], MCPWM_GEN_A, mapped_duty * 100.0);

src/motor-esp32/motor_control.h

@@ -11,8 +11,8 @@
 
 #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.5
-#define DEFAULT_KI       0.0
+#define DEFAULT_KP       0.0
+#define DEFAULT_KI       0.5
 #define DEFAULT_KD       0.0
 #define DEFAULT_MIN_DUTY 0.1