diff --git a/Robotito/include/control.h b/Robotito/include/control.h index 1168f0c75d7ecba23ec34c9f8be49f5410078d05..364cf6e07dd40fd71608634946da14d3769584bb 100644 --- a/Robotito/include/control.h +++ b/Robotito/include/control.h @@ -65,6 +65,7 @@ typedef struct { int32_t vel_counter; int32_t odom_counter; + int32_t counter; float target_v; float accum_error; diff --git a/Robotito/src/control/control.c b/Robotito/src/control/control.c index 46891b77e9f5c3a77be8bec180fd279873c82d01..2d1db29b25bc3d479278605eef3f689d4b0f75e0 100644 --- a/Robotito/src/control/control.c +++ b/Robotito/src/control/control.c @@ -8,6 +8,8 @@ static void callback_enc_func(int i_callback, int8_t dir, uint32_t counter) { motors[i_callback].vel_counter+=dir; motors[i_callback].odom_counter+=dir; + motors[i_callback].counter+=dir; + printf("Motor %d tics: %d\n", i_callback, motors[i_callback].counter); } static void getW(sF3dVector_t* v, float phi){ @@ -55,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 @@ -83,13 +84,7 @@ static void callback_motors_pid(){ // 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); - } + } static void callback_odometry(){ diff --git a/Robotito/src/main.c b/Robotito/src/main.c index ab74e754cd0ff87a60aad194f45d8c1a0c17165c..98e0558664dbbb853a62f5682259ca192e0d7fb1 100644 --- a/Robotito/src/main.c +++ b/Robotito/src/main.c @@ -29,7 +29,6 @@ void app_main(void) omni_init(); omni_set_enable(true); while (1) { - omni_drive(0,0,PI/4,0); vTaskDelay( 300000/ portTICK_RATE_MS); } } \ No newline at end of file