Update constants values & discard dif PID stage.

Robotito/include/control.h

@@ -29,7 +29,7 @@
 #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 RAD_PER_COUNT 2*PI/154 // 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
 
@@ -48,7 +48,7 @@
 
 #define KP 0.3      // Proportional component of the PID
 #define KI 0.6      // Integral component of the PID
-#define KD 0.001    // Differential component of the PID
+#define KD 0.0    // Differential component of the PID
 
 typedef struct {
 

Robotito/src/control/control.c

@@ -61,10 +61,8 @@ static void callback_motors_pid(){
         float error = m->target_v - current_v; // compute 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 * rate_error;
 
         if (m->output>MAX_OUTPUT) {
             m->output=MAX_OUTPUT;