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++;
}
