diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index 8107d29..6389daa 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -19,8 +19,6 @@ * the same function. * if the motor direction is not set, it will not move. * @param direction The direction of the left and right wheels - * @param left_speed The speed of the left motor, from 0.0 to 1.0 - * @param right_speed The speed of the right motor, from 0.0 to 1.0 */ void set_wheel_direction(uint32_t direction) @@ -43,6 +41,13 @@ revert_wheel_direction() gpio_set_mask(reverted_direction & DIRECTION_MASK); } +/*! + * @brief Check if the current direction is within the range of the target + * @param current_direction current yaw + * @param target_direction target yaw + * @param range acceptable range + * @return true if the current direction is within the range of the target + */ bool check_direction(float current_direction, float target_direction, float range) { @@ -64,8 +69,13 @@ check_direction(float current_direction, float target_direction, float range) return false; } +/*! + * @brief Spin the car to a certain yaw + * @param target_yaw The target yaw to spin to + * @param pp_car_struct The car struct pointer + */ void -spin_to_yaw(float target_yaw, car_struct_t *car_struct) +spin_to_yaw(float target_yaw, car_struct_t *pp_car_struct) { updateDirection(); float initial_yaw = g_direction.yaw; @@ -83,9 +93,9 @@ spin_to_yaw(float target_yaw, car_struct_t *car_struct) set_wheel_direction(DIRECTION_LEFT); } - set_wheel_speed_synced(80u, car_struct); + set_wheel_speed_synced(80u, pp_car_struct); - car_struct->p_pid->use_pid = false; + pp_car_struct->p_pid->use_pid = false; for (;;) { @@ -93,12 +103,12 @@ spin_to_yaw(float target_yaw, car_struct_t *car_struct) if (check_direction(g_direction.yaw, target_yaw, 1)) { set_wheel_direction(DIRECTION_MASK); - set_wheel_speed_synced(0u, car_struct); + set_wheel_speed_synced(0u, pp_car_struct); break; } } - car_struct->p_pid->use_pid = true; + pp_car_struct->p_pid->use_pid = true; vTaskDelay(pdMS_TO_TICKS(50)); } diff --git a/frtos/motor/motor_init.h b/frtos/motor/motor_init.h index 3b7aad3..ababd87 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -19,6 +19,13 @@ #include "motor_config.h" +/*! + * @brief Initialize the motor + * @param car_struct The car_struct. Need to have the following fields:\n + * - p_left_motor\n + * - p_right_motor\n + * - p_pid + */ void motor_init(car_struct_t *car_struct) { diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index dd858e6..5cce816 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -8,14 +8,11 @@ #ifndef MOTOR_PID_H #define MOTOR_PID_H -// #include "magnetometer_init.h" - /*! * @brief Compute the control signal using PID controller - * @param target_speed The target speed of the wheel - * @param current_speed The current speed of the wheel * @param integral The integral term of the PID controller * @param prev_error The previous error of the PID controller + * @param car_struct The car_struct pointer * @return The control signal */ float @@ -38,10 +35,15 @@ compute_pid(float *integral, float *prev_error, car_struct_t *car_struct) return control_signal; } +/*! + * @brief Repeating timer handler for the PID controller + * @param ppp_timer The repeating timer + * @return true + */ bool -repeating_pid_handler(struct repeating_timer *t) +repeating_pid_handler(struct repeating_timer *ppp_timer) { - car_struct_t *car_strut = (car_struct_t *)t->user_data; + car_struct_t *car_strut = (car_struct_t *)ppp_timer->user_data; static float integral = 0.0f; static float prev_error = 0.0f; diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index 881bb24..e19e19a 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -38,14 +38,14 @@ h_wheel_sensor_isr_handler(void) /*! * @brief Task to monitor and control the speed of the wheel - * @param pvParameters motor_speed_t struct pointer + * @param ppp_motor motor_speed_t struct pointer * @see motor_speed_t */ void -monitor_wheel_speed_task(void *pvParameters) +monitor_wheel_speed_task(void *ppp_motor) { volatile motor_t *p_motor = NULL; - p_motor = (motor_t *)pvParameters; + p_motor = (motor_t *)ppp_motor; uint64_t curr_time = 0u; uint64_t prev_time = 0u; @@ -76,30 +76,35 @@ monitor_wheel_speed_task(void *pvParameters) } } -void -set_wheel_speed(uint32_t pwm_level, motor_t * motor) -{ - motor->pwm.level = pwm_level; - - pwm_set_chan_level(motor->pwm.slice_num, - motor->pwm.channel, - motor->pwm.level); -} - /*! * @brief Set the speed of the wheels * @param pwm_level The pwm_level of the wheels, from 0 to 99 + * @param p_motor The motor to set the speed */ void -set_wheel_speed_synced(uint32_t pwm_level, car_struct_t *car_strut) +set_wheel_speed(uint32_t pwm_level, motor_t *p_motor) { if (pwm_level > MAX_PWM_LEVEL) { pwm_level = MAX_PWM_LEVEL; } - set_wheel_speed(pwm_level, car_strut->p_left_motor); - set_wheel_speed(pwm_level, car_strut->p_right_motor); + p_motor->pwm.level = pwm_level; + + pwm_set_chan_level( + p_motor->pwm.slice_num, p_motor->pwm.channel, p_motor->pwm.level); +} + +/*! + * @brief Set the speed of the wheels + * @param pwm_level The pwm_level of the wheels, from 0 to 99 + * @param pp_car_strut The car struct pointer + */ +void +set_wheel_speed_synced(uint32_t pwm_level, car_struct_t *pp_car_strut) +{ + set_wheel_speed(pwm_level, pp_car_strut->p_left_motor); + set_wheel_speed(pwm_level, pp_car_strut->p_right_motor); } ///*! diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index 7a45243..ab1e096 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -22,8 +22,13 @@ motor_control_task(void *params) } } +/*! + * @brief init the tasks for the motor + * @param pp_car_struct The car struct + * @param p_isr_handler The isr handler + */ void -launch(car_struct_t *car_struct, void *isr_handler) +motor_tasks_init(car_struct_t *pp_car_struct, void *p_isr_handler) { // Left wheel // @@ -31,7 +36,7 @@ launch(car_struct_t *car_struct, void *isr_handler) xTaskCreate(monitor_wheel_speed_task, "monitor_left_wheel_speed_task", configMINIMAL_STACK_SIZE, - (void *)car_struct->p_left_motor, + (void *)pp_car_struct->p_left_motor, WHEEL_SPEED_PRIO, &h_monitor_left_wheel_speed_task_handle); @@ -41,7 +46,7 @@ launch(car_struct_t *car_struct, void *isr_handler) xTaskCreate(monitor_wheel_speed_task, "monitor_wheel_speed_task", configMINIMAL_STACK_SIZE, - (void *)car_struct->p_right_motor, + (void *)pp_car_struct->p_right_motor, WHEEL_SPEED_PRIO, &h_monitor_right_wheel_speed_task_handle); @@ -50,17 +55,17 @@ launch(car_struct_t *car_struct, void *isr_handler) xTaskCreate(motor_control_task, "motor_turning_task", configMINIMAL_STACK_SIZE, - (void *)car_struct, + (void *)pp_car_struct, WHEEL_CONTROL_PRIO, &h_motor_turning_task_handle); // isr to detect right motor slot gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true); - gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, isr_handler); + gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, p_isr_handler); // isr to detect left motor slot gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true); - gpio_add_raw_irq_handler(SPEED_PIN_LEFT, isr_handler); + gpio_add_raw_irq_handler(SPEED_PIN_LEFT, p_isr_handler); irq_set_enabled(IO_IRQ_BANK0, true); } @@ -73,17 +78,17 @@ main(void) sleep_ms(4000); printf("Test started!\n"); - motor_t g_motor_right; - motor_t g_motor_left; - motor_pid_t g_pid; + motor_t motor_right; + motor_t motor_left; + motor_pid_t pid; - car_struct_t car_struct = { .p_right_motor = &g_motor_right, - .p_left_motor = &g_motor_left, - .p_pid = &g_pid }; + car_struct_t car_struct = { .p_right_motor = &motor_right, + .p_left_motor = &motor_left, + .p_pid = &pid }; motor_init(&car_struct); - launch(&car_struct, &h_wheel_sensor_isr_handler); + motor_tasks_init(&car_struct, &h_wheel_sensor_isr_handler); // PID timer struct repeating_timer pid_timer;