From ad5cab94109085bbb295772771464f24d9aec83e Mon Sep 17 00:00:00 2001
From: RicardoEA <ricardo.ercoli@fing.edu.uy>
Date: Thu, 26 Jan 2023 21:03:06 -0300
Subject: [PATCH] Tide up code
---
Robotito/src/control/control.c | 30 +++++-------------------------
1 file changed, 5 insertions(+), 25 deletions(-)
diff --git a/Robotito/src/control/control.c b/Robotito/src/control/control.c
index e615f99..0999807 100644
--- a/Robotito/src/control/control.c
+++ b/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;
--
GitLab