Clean 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 );
+    //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,7 @@ 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);
+        //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
 
@@ -145,7 +145,7 @@ 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);
+        //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);
@@ -161,7 +161,7 @@ esp_err_t omni_init(){
         //saving encoder structure
         motors[i].encoder=e;
 
-        printf(" done\r\n");
+        //printf(" done\r\n");
         motors[i].target_v=0;
     }