diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index 3cb2e65..2948ae6 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -49,7 +49,8 @@ revert_wheel_direction() * @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) +bool +check_direction(float current_direction, float target_direction, float range) { // Normalize directions to be within 0 to 360 degrees current_direction = fmod(current_direction, 360.0f); diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index 277dcf0..24d3b77 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -26,17 +26,15 @@ compute_pid(float *integral, float *prev_error, car_struct_t *car_struct) float derivative = error - *prev_error; - float control_signal - = car_struct->p_pid->kp_value * error - + car_struct->p_pid->ki_value * (*integral) - + car_struct->p_pid->kd_value * derivative; + float control_signal = car_struct->p_pid->kp_value * error + + car_struct->p_pid->ki_value * (*integral) + + car_struct->p_pid->kd_value * derivative; *prev_error = error; return control_signal; } - /*! * @brief Repeating timer handler for the PID controller * @param ppp_timer The repeating timer diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index 43ceb0e..4bb3774 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -22,8 +22,7 @@ h_wheel_sensor_isr_handler(void) gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL); BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(g_left_sem, - &xHigherPriorityTaskWoken); + xSemaphoreGiveFromISR(g_left_sem, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } @@ -32,13 +31,11 @@ h_wheel_sensor_isr_handler(void) gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL); BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(g_right_sem, - &xHigherPriorityTaskWoken); + xSemaphoreGiveFromISR(g_right_sem, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } } - /*! * @brief Task to monitor and control the speed of the wheel * @param ppp_motor motor_speed_t struct pointer @@ -56,15 +53,15 @@ monitor_wheel_speed_task(void *ppp_motor) for (;;) { - if ((xSemaphoreTake(*p_motor->p_sem, pdMS_TO_TICKS(100)) - == pdTRUE) && (*p_motor->use_pid == true)) + if ((xSemaphoreTake(*p_motor->p_sem, pdMS_TO_TICKS(100)) == pdTRUE) + && (*p_motor->use_pid == true)) { curr_time = time_us_64(); elapsed_time = curr_time - prev_time; prev_time = curr_time; p_motor->speed.current_cms - = (float) (SLOT_DISTANCE_CM_MODIFIED / elapsed_time); + = (float)(SLOT_DISTANCE_CM_MODIFIED / elapsed_time); p_motor->speed.distance_cm += SLOT_DISTANCE_CM; } @@ -112,13 +109,14 @@ set_wheel_speed_synced(uint32_t pwm_level, car_struct_t *pp_car_strut) * @param distance_cm distance to travel in cm */ void -distance_to_stop(car_struct_t * pp_car_struct, float distance_cm) +distance_to_stop(car_struct_t *pp_car_struct, float distance_cm) { float initial = pp_car_struct->p_left_motor->speed.distance_cm; for (;;) { - if (pp_car_struct->p_left_motor->speed.distance_cm - initial >= distance_cm) + if (pp_car_struct->p_left_motor->speed.distance_cm - initial + >= distance_cm) { set_wheel_direction(DIRECTION_MASK); set_wheel_speed_synced(0u, pp_car_struct); @@ -127,8 +125,8 @@ distance_to_stop(car_struct_t * pp_car_struct, float distance_cm) vTaskDelay(pdMS_TO_TICKS(10)); } vTaskDelay(pdMS_TO_TICKS(1000)); - pp_car_struct->p_right_motor->speed.distance_cm = - pp_car_struct->p_left_motor->speed.distance_cm; + pp_car_struct->p_right_motor->speed.distance_cm + = pp_car_struct->p_left_motor->speed.distance_cm; } #endif /* MOTOR_SPEED_H */ diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index b5182e4..fc2eb91 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -19,8 +19,6 @@ motor_control_task(void *params) } } - - int main(void) { diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c index 2798bb6..377b313 100644 --- a/frtos/rtos_car.c +++ b/frtos/rtos_car.c @@ -31,14 +31,19 @@ check_collision(void *params) // return ((car_struct->obs->left_sensor_detected << 1) // | (car_struct->obs->right_sensor_detected)) // || car_struct->obs->ultrasonic_detected; - return check_line_touch(car_struct) || - car_struct->obs->ultrasonic_detected; + return check_line_touch(car_struct) || car_struct->obs->ultrasonic_detected; } +/*! + * @brief Task for going straight, shift left or right when there is a line, + * by changing the distance of the right wheel, so the speed of the wheel + * will be changed by the PID function. + * @param p_car_struct + */ void -motor_control_task(void *params) +motor_control_task(void *p_car_struct) { - car_struct_t *car_struct = (car_struct_t *)params; + car_struct_t *car_struct = (car_struct_t *)p_car_struct; set_wheel_direction(DIRECTION_FORWARD); set_wheel_speed_synced(90u, car_struct); @@ -68,6 +73,11 @@ motor_control_task(void *params) } } +/*! + * @brief Go forward until there is an obstacle, reverse the wheel direction, + * travel a certain distance, then stop. + * @param p_car_struct + */ void obs_task(void *params) { @@ -80,10 +90,10 @@ obs_task(void *params) { if (car_struct->obs->ultrasonic_detected) { -// turn(DIRECTION_LEFT, 130u, 90u, car_struct); -// set_wheel_direction(DIRECTION_FORWARD); -// set_wheel_speed_synced(90u, car_struct); -// + // turn(DIRECTION_LEFT, 130u, 90u, car_struct); + // set_wheel_direction(DIRECTION_FORWARD); + // set_wheel_speed_synced(90u, car_struct); + // revert_wheel_direction(); distance_to_stop(car_struct, 20.f); } @@ -91,24 +101,28 @@ obs_task(void *params) } } +/*! + * @brief Task to demonstrate the turning function using magnetometer + * @param params + */ void turn_task(void *params) { - car_struct_t *car_struct = (car_struct_t *)params; + car_struct_t *car_struct = (car_struct_t *)params; - for (;;) - { - set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed_synced(89u, car_struct); + for (;;) + { + set_wheel_direction(DIRECTION_FORWARD); + set_wheel_speed_synced(89u, car_struct); - distance_to_stop(car_struct, 20.f); - vTaskDelay(pdMS_TO_TICKS(1000)); + distance_to_stop(car_struct, 20.f); + vTaskDelay(pdMS_TO_TICKS(1000)); -// turn_to_yaw(DIRECTION_LEFT, 230.f, 80u, car_struct); + // turn_to_yaw(DIRECTION_LEFT, 230.f, 80u, car_struct); - turn(DIRECTION_RIGHT, 80.f, 90u, car_struct); - vTaskDelay(pdMS_TO_TICKS(1000)); - } + turn(DIRECTION_RIGHT, 80.f, 90u, car_struct); + vTaskDelay(pdMS_TO_TICKS(1000)); + } } void @@ -157,11 +171,11 @@ main(void) // Magnetometer magnetometer_init(&car_struct); -// magnetometer_tasks_init(&car_struct); + // magnetometer_tasks_init(&car_struct); // updateDirection(car_struct.p_direction); printf("Magnetometer initialized!\n"); -// sleep_ms(1000); + // sleep_ms(1000); // ultra ultrasonic_init(&car_struct); @@ -180,32 +194,32 @@ main(void) sleep_ms(1000u); - // control task -// TaskHandle_t h_motor_turning_task_handle = NULL; -// xTaskCreate(motor_control_task, -// "motor_turning_task", -// configMINIMAL_STACK_SIZE, -// (void *)&car_struct, -// PRIO, -// &h_motor_turning_task_handle); + // control task + // TaskHandle_t h_motor_turning_task_handle = NULL; + // xTaskCreate(motor_control_task, + // "motor_turning_task", + // configMINIMAL_STACK_SIZE, + // (void *)&car_struct, + // PRIO, + // &h_motor_turning_task_handle); // obs task -// TaskHandle_t h_obs_task_handle = NULL; -// xTaskCreate(obs_task, -// "obs_task", -// configMINIMAL_STACK_SIZE, -// (void *)&car_struct, -// PRIO, -// &h_obs_task_handle); + // TaskHandle_t h_obs_task_handle = NULL; + // xTaskCreate(obs_task, + // "obs_task", + // configMINIMAL_STACK_SIZE, + // (void *)&car_struct, + // PRIO, + // &h_obs_task_handle); // turn task - TaskHandle_t h_turn_task_handle = NULL; - xTaskCreate(turn_task, - "turn_task", - configMINIMAL_STACK_SIZE, - (void *)&car_struct, - PRIO, - &h_turn_task_handle); + TaskHandle_t h_turn_task_handle = NULL; + xTaskCreate(turn_task, + "turn_task", + configMINIMAL_STACK_SIZE, + (void *)&car_struct, + PRIO, + &h_turn_task_handle); // PID timer struct repeating_timer pid_timer; diff --git a/frtos/rtos_car_line.c b/frtos/rtos_car_line.c deleted file mode 100644 index f453482..0000000 --- a/frtos/rtos_car_line.c +++ /dev/null @@ -1,126 +0,0 @@ - -#include "line_sensor_init.h" -#include "ultrasonic_sensor.h" -#include "car_config.h" -#include "motor_init.h" - -/*! - * @brief Check if the car is on the line - * @param params - * @return - */ -uint8_t -check_line_touch(void *params) -{ - car_struct_t *car_struct = (car_struct_t *)params; - - return (car_struct->obs->left_sensor_detected << 1) - | (car_struct->obs->right_sensor_detected); -} - -void -motor_control_task(void *params) -{ - car_struct_t *car_struct = (car_struct_t *)params; - - set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed_synced(90u, car_struct); - - for (;;) - { - uint8_t temp = check_line_touch(car_struct); - switch (temp) - { - default: - break; - case 0b01: // right - car_struct->p_right_motor->speed.distance_cm -= - SLOT_DISTANCE_CM; - break; - case 0b10: //left - car_struct->p_right_motor->speed.distance_cm += - SLOT_DISTANCE_CM; - break; - case 0b11: - revert_wheel_direction(); - distance_to_stop(car_struct, 17.f); - turn(DIRECTION_LEFT, 90u, 90u, car_struct); - set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed_synced(90u, car_struct); - break; - } - - vTaskDelay(pdMS_TO_TICKS(50)); - } -} - -void -h_main_irq_handler(void) -{ - if (gpio_get_irq_event_mask(SPEED_PIN_LEFT) & GPIO_IRQ_EDGE_FALL) - { - gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL); - - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(g_left_sem, &xHigherPriorityTaskWoken); - portYIELD_FROM_ISR(xHigherPriorityTaskWoken); - } - - if (gpio_get_irq_event_mask(SPEED_PIN_RIGHT) & GPIO_IRQ_EDGE_FALL) - { - gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL); - - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(g_right_sem, &xHigherPriorityTaskWoken); - portYIELD_FROM_ISR(xHigherPriorityTaskWoken); - } -} - -int -main(void) -{ - stdio_usb_init(); - - obs_t obs; - - motor_t motor_right; - motor_t motor_left; - motor_pid_t pid; - - direction_t direction; - - car_struct_t car_struct = { .p_right_motor = &motor_right, - .p_left_motor = &motor_left, - .p_pid = &pid, - .obs = &obs, - .p_direction = &direction }; - - // line - line_sensor_init(&car_struct); - line_tasks_init(&car_struct); - printf("Line sensor initialized!\n"); - - // motor - motor_init(&car_struct); - motor_tasks_init(&car_struct, &h_main_irq_handler); - printf("Motor initialized!\n"); - - sleep_ms(1000u); - - // control task - TaskHandle_t h_motor_turning_task_handle = NULL; - xTaskCreate(motor_control_task, - "motor_turning_task", - configMINIMAL_STACK_SIZE, - (void *)&car_struct, - PRIO, - &h_motor_turning_task_handle); - - // PID timer - struct repeating_timer pid_timer; - add_repeating_timer_ms(-50, repeating_pid_handler, &car_struct, &pid_timer); - - vTaskStartScheduler(); - - return (0); -}