FOR_EACH(i, controllers)
{
err->ve[i] = sym_limit(setp->ve[i], deg2rad(tsfloat_get(&angles_max)))
+ deg2rad(tsfloat_get(&biases[i])) - pos->ve[i];
ctrl->ve[i] = pid_control(&controllers[i], err->ve[i], speed->ve[i], dt);
}
static float speed_func(float angle)
{
float _speed = tsfloat_get(&speed);
return sym_limit(tsfloat_get(&speed_slope) * angle, _speed);
}
