void robot_cs_update(void* dummy) { double vx_t,vy_t,omegaz_t; double vx_r,vy_r; double alpha; double _ca,_sa; hrobot_vector_t hvec; robot_cs_t *rcs = dummy; // if CS inactivated, just quit if(!rcs->active) return; // if CS was previously inactive, we need a little hack for quadramps if(rcs->reactivated) { int32_t consign; pid_reset(&pid_x); pid_reset(&pid_y); pid_reset(&pid_angle); consign = cs_get_consign(&csm_angle); qramp_angle.previous_var = 0; qramp_angle.previous_out = consign; qramp_angle.previous_in = consign; rcs->reactivated = 0; } // compute control system first level (x,y,a) cs_manage(&csm_x); cs_manage(&csm_y); cs_manage(&csm_angle); // transform output vector from table coords to robot coords vx_t = cs_get_out(&csm_x); vy_t = cs_get_out(&csm_y); omegaz_t = cs_get_out(&csm_angle); hposition_get(rcs->hpm, &hvec); alpha = -hvec.alpha; _ca = cos(alpha); _sa = sin(alpha); vx_r = vx_t*_ca - vy_t*_sa; vy_r = vx_t*_sa + vy_t*_ca; // set second level consigns hrobot_set_motors(rcs->hrs, vx_r, vy_r, omegaz_t); }
// periodical 10ms execution void asserv_rapide_manage(brushless data_moteur) { cs_manage(&position); cs_manage(&speed); // for debug graphs printf_P(PSTR("C%lip%liP%liv%liV%li\n"), position.consign_value, brushless_get_pos((void*)0), position_quadr_derv.pivot + position.consign_value , brushless_get_speed((void*)0), speed.consign_value); //printf_P(PSTR("i%lio%liv%li\n"), speed_pid.prev_in, speed_pid.prev_out, vitesse); }
void motor_cs_update(void* dummy, int32_t m1, int32_t m2, int32_t m3) { uint8_t flags; // set consigns for motors CS cs_set_consign(&csm_motor1, m1); cs_set_consign(&csm_motor2, m2); cs_set_consign(&csm_motor3, m3); // update motors CS cs_manage(&csm_motor1); cs_manage(&csm_motor2); cs_manage(&csm_motor3); return; }
void arm_cs_manage(arm_control_loop_t *loop) { cs_manage(&loop->manager); }
/* called every 5 ms */ static void do_cs(void *dummy) { static uint16_t cpt = 0; static int32_t old_a = 0, old_d = 0; wheel_update_value(); /* robot system, conversion to angle,distance */ if (mainboard.flags & DO_RS) { int16_t a,d; rs_update(&mainboard.rs); /* takes about 0.5 ms */ /* process and store current speed */ a = rs_get_angle(&mainboard.rs); d = rs_get_distance(&mainboard.rs); mainboard.speed_a = a - old_a; mainboard.speed_d = d - old_d; old_a = a; old_d = d; } /* control system */ if (mainboard.flags & DO_CS) { if (mainboard.angle.on) cs_manage(&mainboard.angle.cs); if (mainboard.distance.on) cs_manage(&mainboard.distance.cs); if (mainboard.fessor.on) { cs_manage(&mainboard.fessor.cs); fessor_manage(); } if (mainboard.wheel.on) cs_manage(&mainboard.wheel.cs); if (mainboard.elevator.on) { cs_manage(&mainboard.elevator.cs); elevator_manage(); } } if ((cpt & 1) && (mainboard.flags & DO_POS)) { /* about 1.5ms (worst case without centrifugal force * compensation) */ position_manage(&mainboard.pos); } if (mainboard.flags & DO_BD) { bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs); bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs); } if (mainboard.flags & DO_TIMER) { uint8_t second; /* the robot should stop correctly in the strat, but * in some cases, we must force the stop from an * interrupt */ second = time_get_s(); if (second >= MATCH_TIME + 2) { pwm_ng_set(LEFT_PWM, 0); pwm_ng_set(RIGHT_PWM, 0); printf_P(PSTR("END OF TIME\r\n")); while(1); } } /* brakes */ if (mainboard.flags & DO_POWER) BRAKE_OFF(); else BRAKE_ON(); cpt++; }