diff --git a/Robotito/include/control.h b/Robotito/include/control.h index b50ac9db54ede946f8e8ade9f044f08a2936b99d..a60c1767c5790e11f44f1a3a3e4843f3c3f3b9c7 100644 --- a/Robotito/include/control.h +++ b/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 { diff --git a/Robotito/src/control/control.c b/Robotito/src/control/control.c index 0999807c9fafd32abc00ec212833dba503557d8c..5aba34fb5b090c6d0bd4b9f4b9e7e2c57a1d40b0 100644 --- a/Robotito/src/control/control.c +++ b/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;