diff --git a/Robotito/platformio.ini b/Robotito/platformio.ini
index c312d732d1b2b13c2c472bdeee6031885a98f61f..3ddd37b9967fd06f7680f9c5eeb7098e5102fc15 100644
--- a/Robotito/platformio.ini
+++ b/Robotito/platformio.ini
@@ -12,3 +12,4 @@
platform = espressif32
board = esp32thing
framework = espidf
+monitor_speed = 115200
\ No newline at end of file
diff --git a/Robotito/src/control/control.c b/Robotito/src/control/control.c
index 09219e61ef6a65b358a9abc44152d3d10bc2fb60..9176329aef2a5840c185e004e390e91b8edff504 100644
--- a/Robotito/src/control/control.c
+++ b/Robotito/src/control/control.c
@@ -24,7 +24,7 @@ static void getW(sF3dVector_t* v, float phi){
m.x3 = sin(pi_3 + phi);
m.y3 = -cos(pi_3 + phi);
m.z3 = ROBOT_RADIUS;
-
+ printf("En getW previo a MatMul: Vx= %.15f \t Vy= %.15f \t Vz= %.15f \n",v->x,v->y,v->z );
vector_mul(m, v);
}
@@ -55,7 +55,7 @@ static void callback_motors_pid(){
float current_v = m->vel_counter / OMNI_PID_CTRL_TIMER; // compute curren motor vel in tics/s
m->vel_counter = 0; // reset counter
- //printf("motor %i, tics: %i, current_v: %f\n", i, m->counter, current_v);
+ 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
@@ -74,10 +74,20 @@ static void callback_motors_pid(){
}
m->prev_error = error; // save prev error
+ //if (i==1)
+ //{
+ //printf("Para el Motor %d tenemos: E_vel=%.15f \t E_ac=%.15f \t E_dot=%.15f \n",i,error,accum_error,(error - m->prev_error));
+ //}
+
}
+
// set motors speeds
for (int i=0; i<NMOTORS; i++) {
+ //if (i==1)
+ //{
+ // printf("Velocidad del motor %d es %.15f \n", i,motors[i].output );
+ //}
motor_set_speed(motors[i].driver, motors[i].output);
}
}
@@ -204,9 +214,8 @@ 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;
-
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;
diff --git a/Robotito/src/main.c b/Robotito/src/main.c
index 1e9ecd8cb64f958a1aab1e21d96aedb38d1e806f..c0879c132665708f8df56565b4a80633b05802fb 100644
--- a/Robotito/src/main.c
+++ b/Robotito/src/main.c
@@ -28,47 +28,9 @@ void app_main(void)
printf("Configuring Initial Parameters of mcpwm...\n");
omni_init();
omni_set_enable(true);
- int vel;
+ int vel =2000;
while (1) {
- for (size_t i = 100; 98 < i; i--)
- {
- vel = i;
- printf("---------------------------------------------------------------------\n");
- printf("Busco vel=%d\n",vel);
- printf("---------------------------------------------------------------------\n");
- omni_drive(vel,0,0,0);
- vTaskDelay( 1000/ portTICK_RATE_MS);
- }
- omni_drive(0,0,0,0);
- vTaskDelay(1000/ portTICK_RATE_MS);
- for (size_t i = 100; 98 < i; i--)
- {
- vel = i;
- printf("---------------------------------------------------------------------\n");
- printf("Busco vel=%d\n",vel);
- printf("---------------------------------------------------------------------\n");
- omni_drive(0,vel,0,0);
- vTaskDelay( 1000/ portTICK_RATE_MS);
- }
- for (size_t i = 100; 98 < i; i--)
- {
- vel = i;
- printf("---------------------------------------------------------------------\n");
- printf("Busco vel=%d\n",vel);
- printf("---------------------------------------------------------------------\n");
- omni_drive(-vel,0,0,0);
- vTaskDelay( 1000/ portTICK_RATE_MS);
- }
- omni_drive(0,0,0,0);
- vTaskDelay(1000/ portTICK_RATE_MS);
- for (size_t i = 100; 98 < i; i--)
- {
- vel = i;
- printf("---------------------------------------------------------------------\n");
- printf("Busco vel=%d\n",vel);
- printf("---------------------------------------------------------------------\n");
- omni_drive(0,-vel,0,0);
- vTaskDelay( 1000/ portTICK_RATE_MS);
- }
+ omni_drive(100,0,500,0);
+ vTaskDelay( 300000/ portTICK_RATE_MS);
}
}
\ No newline at end of file
diff --git a/Robotito/src/motors/motors.c b/Robotito/src/motors/motors.c
index f55eba1483dcff2d0e0918f3783eb7d22185f87b..cd4514976fdc95e5acfa8151c888fa7ab63c2b51 100644
--- a/Robotito/src/motors/motors.c
+++ b/Robotito/src/motors/motors.c
@@ -47,6 +47,7 @@ esp_err_t motor_install(int8_t a, int8_t b, mcpwm_unit_t pwm_unit, mcpwm_io_sign
*/
void motor_forward(mcpwm_unit_t mcpwm_num, mcpwm_timer_t timer_num , float duty_cycle)
{
+ //printf("\t Motor speed: %.3f\n",duty_cycle);
mcpwm_set_signal_low(mcpwm_num, timer_num, MCPWM_OPR_B);
mcpwm_set_duty(mcpwm_num, timer_num, MCPWM_OPR_A, duty_cycle);
mcpwm_set_duty_type(mcpwm_num, timer_num, MCPWM_OPR_A, MCPWM_DUTY_MODE_0); //call this each time, if operator was previously in low/high state
@@ -57,6 +58,7 @@ void motor_forward(mcpwm_unit_t mcpwm_num, mcpwm_timer_t timer_num , float duty_
*/
void motor_backward(mcpwm_unit_t mcpwm_num, mcpwm_timer_t timer_num , float duty_cycle)
{
+ //printf("\t Motor speed: %.3f\n",duty_cycle);
mcpwm_set_signal_low(mcpwm_num, timer_num, MCPWM_OPR_A);
mcpwm_set_duty(mcpwm_num, timer_num, MCPWM_OPR_B, duty_cycle);
mcpwm_set_duty_type(mcpwm_num, timer_num, MCPWM_OPR_B, MCPWM_DUTY_MODE_0); //call this each time, if operator was previously in low/high state