// power the motor in order to test them // sequence : left motor forward then backward // right motor forward then backward // nether return void motors_wrapper_test(void) { for (int i = 0; i < 50; ++i) { motors_wrapper_left_motor_set_duty_cycle(i); platform_led_toggle(PLATFORM_LED0); for (volatile int t = 0; t < 1000000; t++); //delay } for (int i = 0; i < 50; ++i) { motors_wrapper_left_motor_set_duty_cycle(-i); platform_led_toggle(PLATFORM_LED1); for (volatile int t = 0; t < 1000000; t++); } for (int i = 0; i < 50; ++i) { motors_wrapper_right_motor_set_duty_cycle(i); platform_led_toggle(PLATFORM_LED2); for (volatile int t = 0; t < 1000000; t++); } for (int i = 0; i < 50; ++i) { motors_wrapper_right_motor_set_duty_cycle(-i); platform_led_toggle(PLATFORM_LED3); for (volatile int t = 0; t < 1000000; t++); } while(1); }
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); } }
// Count one second void TIM2_IRQHandler(void) { if(TIM_GetITStatus(TIM2,TIM_IT_Update) == SET) { elapsed_time++; if (elapsed_time>=88) { //disable_power_relay(); platform_gpio_set(GPIO_RELAIS); platform_led_toggle(PLATFORM_LED5); } TIM_ClearFlag(TIM2, TIM_FLAG_Update); } }
void control_system_task(void *data) { for (;;) { struct control_system *am = (struct control_system *)data; #ifdef USE_DISTANCE_ANGLE_CS ausbee_cs_manage(&(am->csm_distance)); ausbee_cs_manage(&(am->csm_angle)); control_system_set_motors_ref(am, am->distance_mm_diff, am->angle_rad_diff); #endif #ifdef USE_MOTOR_SPEED_CS ausbee_cs_manage(&(am->csm_right_motor)); ausbee_cs_manage(&(am->csm_left_motor)); #endif platform_led_toggle(PLATFORM_LED1); vTaskDelay(CONTROL_SYSTEM_PERIOD_S * 1000 / portTICK_RATE_MS); } }
static void event(void) { platform_led_toggle(PLATFORM_LED0); }