diff --git a/Robotito/include/control.h b/Robotito/include/control.h
index 84bceae1ffbca09839cd6fc563b5331a4e01cc5e..1168f0c75d7ecba23ec34c9f8be49f5410078d05 100644
--- a/Robotito/include/control.h
+++ b/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
diff --git a/Robotito/src/control/control.c b/Robotito/src/control/control.c
index 9176329aef2a5840c185e004e390e91b8edff504..46891b77e9f5c3a77be8bec180fd279873c82d01 100644
--- a/Robotito/src/control/control.c
+++ b/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;
}
diff --git a/Robotito/src/main.c b/Robotito/src/main.c
index c0879c132665708f8df56565b4a80633b05802fb..ab74e754cd0ff87a60aad194f45d8c1a0c17165c 100644
--- a/Robotito/src/main.c
+++ b/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