Newer
Older
/*
* Helper functions
*/
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;
}
static void getW(sF3dVector_t* v, float phi){
float pi_3 = PI/3;
sF3dMatrix_t m;
m.x1 = -sin(phi);
m.y1 = cos(phi);
m.z1 = ROBOT_RADIUS;
m.x2 = -sin(pi_3 - phi);
m.y2 = -cos(pi_3 - phi);
m.z2 = ROBOT_RADIUS;
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);
}
static void getInverseW(sF3dVector_t* v, float phi){
float pi_6 = PI/6;
float dostercios = 2.0/3;
float robot_r_3 = 1.0/(ROBOT_RADIUS*3);
sF3dMatrix_t m;
m.x1 = -sin(phi)*dostercios;
m.y1 = -cos(pi_6 + phi)*dostercios;
m.z1 = cos(phi - pi_6)*dostercios;
m.x2 = cos(phi)*dostercios;
m.y2 = -sin(pi_6 + phi)*dostercios;
m.z2 = sin(phi - pi_6)*dostercios;
m.x3 = robot_r_3;
m.y3 = robot_r_3;
m.z3 = robot_r_3;
vector_mul(m, v);
}
static void callback_motors_pid(){
for (int i=0; i<NMOTORS; i++) {
servo_t *m = &(motors[i]);
float current_v = m->vel_counter / OMNI_PID_CTRL_TIMER; // compute curren motor vel in counts/s
//printf("motor %i, target_v: %f, current_v: %f\n", i, m->target_v, current_v);
float accum_error = m->accum_error + (error * OMNI_PID_CTRL_TIMER); // accumulate error as an integral
float rate_error = (error - m->prev_error)/ OMNI_PID_CTRL_TIMER;
m->output = KP * error;
if (m->output>MAX_OUTPUT) {
m->output=MAX_OUTPUT;
} else if (m->output<-MAX_OUTPUT) {
m->output=-MAX_OUTPUT;
} else {
m->accum_error = accum_error; // only here, for windup protection
}
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));
//}
//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(){
odom_t *o = &(odometry); // get odometry actual info
int32_t tics_motores[NMOTORS];
for (int i=0; i<NMOTORS; i++) {
servo_t *m = &(motors[i]);
tics_motores[i] = m->odom_counter; // get motors odomo counter
m->odom_counter = 0; // reset counter
}
sF3dVector_t *v = calloc(1, sizeof(sF3dVector_t));
v->x = tics_motores[0] * COUNTS_TO_RAD_PER_SEC * WHEEL_RADIUS;
v->y = tics_motores[1] * COUNTS_TO_RAD_PER_SEC * WHEEL_RADIUS;
v->z = tics_motores[2] * COUNTS_TO_RAD_PER_SEC * WHEEL_RADIUS;
o->x += v->x * OMNI_ODOM_CTRL_TIMER;
o->y += v->y * OMNI_ODOM_CTRL_TIMER;
o->phi += v->z * OMNI_ODOM_CTRL_TIMER;
//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[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));
free(v);
}
/*
* Operation functions
*/
esp_err_t omni_init(){
int8_t default_pins[] = MOTOR_PINS;
int8_t default_enc[] = MOTOR_ENC;
mcpwm_io_signals_t default_pwm_subunit_io[] = MOTOR_PWM_SUBUNIT_IO;
mcpwm_timer_t default_pwm_subunit_timer[] = MOTOR_PWM_SUBUNIT_TIMER;
for (int i=0; i<NMOTORS; i++) {
int8_t pin1 = default_pins[2*i];
int8_t pin2 = default_pins[2*i+1];
int8_t encA = default_enc[2*i];
int8_t encB = default_enc[2*i+1];
mcpwm_io_signals_t pwm_a = default_pwm_subunit_io[2*i];
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);
//saving servo structure
motors[i].driver= m;
//encoder
encoder_h_t *e;
encoder_setup(encA,encB,&e);
encoder_register_callback(e, callback_enc_func, i, 1);
//saving encoder structure
motors[i].encoder=e;
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
motors[i].target_v=0;
}
odometry.x = 0;
odometry.x_dot = 0;
odometry.y = 0;
odometry.y_dot = 0;
odometry.phi = 0;
odometry.phi_dot = 0;
// SET TASK TO RUN ISR for motors pid and odometry.
if (tmr_setup(OMNI_PID_NRO_TIMER, 1000*1000*OMNI_PID_CTRL_TIMER, callback_motors_pid) != ESP_OK) {
return ESP_FAIL;
}
if (tmr_setup(OMNI_ODOM_NRO_TIMER, 1000*1000*OMNI_ODOM_CTRL_TIMER, callback_odometry) != ESP_OK) {
return ESP_FAIL;
}
return ESP_OK;
};
esp_err_t omni_set_enable(bool start){
if (start){
if ((tmr_start(OMNI_PID_NRO_TIMER) != ESP_OK)) {
return ESP_FAIL;
}
if ((tmr_start(OMNI_ODOM_NRO_TIMER) != ESP_OK)) {
return ESP_FAIL;
}
for (int i=0; i<NMOTORS; i++) {
motor_start(motors[i].driver);
motors[i].vel_counter = 0;
motors[i].odom_counter = 0;
}
}else{
if ((tmr_stop(OMNI_PID_NRO_TIMER) != ESP_OK)) {
return ESP_FAIL;
}
if ((tmr_stop(OMNI_ODOM_NRO_TIMER) != ESP_OK)) {
return ESP_FAIL;
}
for (int i=0; i<NMOTORS; i++) {
motor_stop(motors[i].driver);
}
}
return ESP_OK;
}
esp_err_t omni_drive(float x_dot ,float y_dot ,float w_dot ,float phi){
sF3dVector_t *v = calloc(1, sizeof(sF3dVector_t));
v->x = x_dot;
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 */
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;
motors[2].target_v = v->z * M_PER_SEC_TO_COUNTS_PER_SEC;