Movimiento Circular

Robotito/platformio.ini

@@ -12,3 +12,4 @@
 platform = espressif32
 board = esp32thing
 framework = espidf
+monitor_speed = 115200
\ No newline at end of file

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;

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

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