void control_system_task(void *data)
{
(void)data;
for (;;)
{
position_update();
if (smooth_traj_is_paused())
{
// don't move
motors_wrapper_motor_set_duty_cycle(RIGHT_MOTOR, 0);
motors_wrapper_motor_set_duty_cycle(LEFT_MOTOR, 0);
}
else
{
platform_led_toggle(PLATFORM_LED1);
ausbee_cs_manage(&(am.csm_distance));
ausbee_cs_manage(&(am.csm_angle));
control_system_set_motors_ref(am.distance_mm_diff, am.angle_rad_diff);
}
//ausbee_cs_manage(&(am.csm_right_motor));
//ausbee_cs_manage(&(am.csm_left_motor));
vTaskDelay(CONTROL_SYSTEM_PERIOD_S * 1000 / portTICK_RATE_MS);
}
}
// Updating encoder value
void TIM8_UP_TIM13_IRQHandler(void)
{
if (TIM_GetITStatus(TIM8, TIM_IT_Update) == SET) {
#ifdef ENCODERS_HAVE_QUADRATURE
// Specific operations have to be done to read encoder's value has negative
// when moving backward
uint16_t left_counter = TIM1->CNT;
int16_t left_encoder_diff = *(int16_t *)(&left_counter);
uint16_t right_counter = TIM3->CNT;
int16_t right_encoder_diff = -*(int16_t *)(&right_counter);
#else
// Reading encoder value
int32_t left_encoder_diff = TIM3->CNT;
int32_t right_encoder_diff = TIM1->CNT;
// If a motor is moving backward, its encoder value
// is read as a negative number
if (!motors_wrapper_left_motor_is_moving_forward()) {
left_encoder_diff = -left_encoder_diff;
}
if (!motors_wrapper_right_motor_is_moving_forward()) {
right_encoder_diff = -right_encoder_diff;
}
#endif
// Updating position
position_update(left_encoder_diff, right_encoder_diff);
// Resetting encoders value
TIM_SetCounter(TIM1, 0);
TIM_SetCounter(TIM3, 0);
TIM_ClearFlag(TIM8, TIM_FLAG_Update);
}
}
