Tide up code

Robotito/src/control/control.c

@@ -26,7 +26,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);
 }
 
@@ -57,7 +57,6 @@ static void callback_motors_pid(){
 
         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
 
@@ -76,20 +75,11 @@ 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);
     }
 }
@@ -118,22 +108,14 @@ static void callback_odometry(){
     o->phi      += v->z * OMNI_ODOM_CTRL_TIMER;
     o->phi_dot   = v->z;
     
-    //char* Txdata = (char*) malloc(100);
-    //sprintf (Txdata, "Hello world index \r\n");
-    //uart_write_bytes(UART_NUM_2, Txdata, strlen(Txdata));
-    //float_buffer = o;
-    float_buffer[0] = PI;
+
+    float_buffer[0] = o->x;
     float_buffer[1] = o->x_dot;
     float_buffer[2] = o->y;
     float_buffer[3] = o->y_dot;
     float_buffer[4] = o->phi;
     float_buffer[5] = o->phi_dot;
-    uart_write_bytes(UART_NUM_2, &float_buffer[0], sizeof(float));
-    // uart_write_bytes(UART_NUM_2, &float_buffer[1], sizeof(float));
-    // uart_write_bytes(UART_NUM_2, &float_buffer[2], sizeof(float));
-    // uart_write_bytes(UART_NUM_2, &float_buffer[3], sizeof(float));
-    // uart_write_bytes(UART_NUM_2, &float_buffer[4], sizeof(float));
-    // uart_write_bytes(UART_NUM_2, &float_buffer[5], sizeof(float));
+    uart_write_bytes(UART_NUM_2, float_buffer, sizeof(float)*6 );
     free(v);
 }
 
@@ -159,8 +141,6 @@ esp_err_t omni_init(){
         mcpwm_io_signals_t pwm_b = default_pwm_subunit_io[2*i+1];
         mcpwm_timer_t timer_unit = default_pwm_subunit_timer[i];
 
-        //printf("Setting motor %d pins:%d,%d enc:%d,%d", i, pin1, pin2, encA, encB);
-
         motor_h_t *m;
         motor_install(pin1,pin2, MOTOR_PWM_UNIT, pwm_a, pwm_b, timer_unit, &m);
 
@@ -233,7 +213,7 @@ esp_err_t omni_drive(float x_dot ,float y_dot ,float w_dot ,float phi){
     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 */ 
+    /* Here we transform speed from the robot frame in m/s to motor speeds in count/s */ 
     getW(v, phi);
     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;