void dump_cs(const char *name, struct cs *cs)
{
printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld "
"in=% .5ld out=% .5ld\r\n"),
name, cs_get_consign(cs), cs_get_filtered_consign(cs),
cs_get_error(cs), cs_get_filtered_feedback(cs),
cs_get_out(cs));
}
void dump_cs_debug(const char *name, struct cs *cs)
{
DEBUG(E_USER_CS, "%s cons=% .5ld fcons=% .5ld err=% .5ld "
"in=% .5ld out=% .5ld",
name, cs_get_consign(cs), cs_get_filtered_consign(cs),
cs_get_error(cs), cs_get_filtered_feedback(cs),
cs_get_out(cs));
}
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);
}