Change PID implementation and verify odometry

Use PID exactly as it is defined, implement Ziegler-Nichols to obtain the parameters. Also, odometry is verified with different speeds

Robotito/include/control.h

@@ -26,8 +26,13 @@
 #define WHEEL_RADIUS  WHEEL_DIAMETER/2// in meters
 #define ROBOT_RADIUS 0.0675 // in meters
 
-#define ENC_CPR 12 //counts per revolution
+#define ENC_CPR 12 //counts per revolution -- estas son las cuentas que da el encoder en una vuelta suya, pero como la rueda y el eje del motor tienen una relacion N:1 no es lo mismo. Experimentalmente me dio que la relacion es algo asi como 150:1
 #define MOTOR_REDUCTION 50
+
+#define RAD_PER_COUNT 2*PI/150 // Tendria que ir ENC_CPR*MOTOR_REDUCTION pero se va al carajo
+#define M_PER_SEC_TO_COUNTS_PER_SEC 1/(RAD_PER_COUNT*WHEEL_RADIUS) 
+#define COUNTS_TO_RAD_PER_SEC RAD_PER_COUNT/OMNI_ODOM_CTRL_TIMER
+
 #define TICS_PER_REVOLUTION ENC_CPR*MOTOR_REDUCTION
 #define RAD_PER_TICK 2*PI/TICS_PER_REVOLUTION
 #define M_PER_SEC_TO_TICS_PER_SEC 1/(RAD_PER_TICK*WHEEL_RADIUS)
@@ -40,8 +45,16 @@
 
 #define TICS_TO_RAD_S RAD_PER_TICK/OMNI_ODOM_CTRL_TIMER
 
-#define KP 0.09 //0.09
-#define KI 0.020 //0.05
+/*Procedimiento de Ziegler-Nichols:
+    1- Variar la constante Kp desde 0 hasta registrar oscilaciones oscilaciones estables y consistentes
+        Registramos: Periodo de oscilacion Tu = 0,1666666667 y Ku = 0.26, donde Ku = Kp@ocilaciones
+    2- Suponiendo que trabajremos con un PI entonces se deben fijar:
+        a- Kp = 0.45 * Ku
+        b- Ki = 0.54 * Ku / Tu 
+*/
+
+#define KP 0.15 //
+#define KI 0.08 //0.05
 #define KD 0.00
 #define KF MAX_SPEED_POWER/MAX_SPEED_TICS
 

Robotito/src/control/control.c

@@ -53,17 +53,17 @@ static void callback_motors_pid(){
     for (int i=0; i<NMOTORS; i++) {
         servo_t *m = &(motors[i]);
 
-        float current_v  = m->vel_counter / OMNI_PID_CTRL_TIMER;  // compute curren motor vel in tics/s
+        float current_v  = m->vel_counter / OMNI_PID_CTRL_TIMER;  // compute curren motor vel in counts/s
         m->vel_counter = 0; // reset counter
         printf("motor %i, target_v: %f, current_v: %f\n", i, m->target_v, current_v);
 
         float error = m->target_v - current_v; // compute error
-        float accum_error = m->accum_error+error; // accumulate error
 
-        m->output = KF * m->target_v; // compute PID output value
-        m->output += KP * error;
+        float accum_error = m->accum_error + (error * OMNI_PID_CTRL_TIMER); // accumulate error as an integral
+        float rate_error = (error - m->prev_error)/ OMNI_PID_CTRL_TIMER;
+        m->output = KP * error;
         m->output += KI * accum_error;
-        m->output += KD * (error - m->prev_error);
+        m->output += KD * rate_error;
 
         if (m->output>MAX_OUTPUT) {
             m->output=MAX_OUTPUT;
@@ -103,19 +103,18 @@ static void callback_odometry(){
     }
 
     sF3dVector_t *v = calloc(1, sizeof(sF3dVector_t));
-    v->x = tics_motores[0] * TICS_TO_RAD_S * WHEEL_RADIUS;
-    v->y = tics_motores[1] * TICS_TO_RAD_S * WHEEL_RADIUS;
-    v->z = tics_motores[2] * TICS_TO_RAD_S * WHEEL_RADIUS;
+    v->x = tics_motores[0] * COUNTS_TO_RAD_PER_SEC * WHEEL_RADIUS;
+    v->y = tics_motores[1] * COUNTS_TO_RAD_PER_SEC * WHEEL_RADIUS;
+    v->z = tics_motores[2] * COUNTS_TO_RAD_PER_SEC * WHEEL_RADIUS;
 
     getInverseW(v, o->phi);
 
-    o->x        += v->x;
+    o->x        += v->x * OMNI_ODOM_CTRL_TIMER;
     o->x_dot     = v->x;
-    o->y        += v->y;
+    o->y        += v->y * OMNI_ODOM_CTRL_TIMER;
     o->y_dot     = v->y;
-    o->phi      += v->z;
+    o->phi      += v->z * OMNI_ODOM_CTRL_TIMER;
     o->phi_dot   = v->z;
-
     free(v);
 }
 
@@ -214,17 +213,13 @@ esp_err_t omni_drive(float x_dot ,float y_dot ,float w_dot ,float phi){
     v->x = x_dot;
     v->y = y_dot;
     v->z = w_dot;
+    
+    /* Here we transfor speed from the robot frame in m/s to motor speeds in count/s */ 
     getW(v, phi);
-    printf("Luego de getW VM0= %.15f \t VM1= %.15f \t VM2= %.15f \n",v->x,v->y,v->z );
-    motors[0].target_v = v->x * M_PER_SEC_TO_TICS_PER_SEC;
-    motors[1].target_v = v->y * M_PER_SEC_TO_TICS_PER_SEC;
-    motors[2].target_v = v->z * M_PER_SEC_TO_TICS_PER_SEC;
-
+    motors[0].target_v = v->x * M_PER_SEC_TO_COUNTS_PER_SEC;
+    motors[1].target_v = v->y * M_PER_SEC_TO_COUNTS_PER_SEC;
+    motors[2].target_v = v->z * M_PER_SEC_TO_COUNTS_PER_SEC;
+    
     free(v);
-
-    motors[0].accum_error = 0;
-    motors[1].accum_error = 0;
-    motors[2].accum_error = 0;
-
     return ESP_OK;
 }

Robotito/src/main.c

@@ -28,9 +28,8 @@ void app_main(void)
     printf("Configuring Initial Parameters of mcpwm...\n");
     omni_init();
     omni_set_enable(true);
-    int vel =2000;
     while (1) {       
-        omni_drive(100,0,500,0);
+        omni_drive(0,0,PI/4,0);
         vTaskDelay( 300000/ portTICK_RATE_MS);
     } 
 }
\ No newline at end of file