diff --git a/frtos/CMakeLists.txt b/frtos/CMakeLists.txt index 6aa36ca..3ded301 100644 --- a/frtos/CMakeLists.txt +++ b/frtos/CMakeLists.txt @@ -15,6 +15,7 @@ target_compile_definitions(rtos_car PRIVATE NO_SYS=0 # don't want NO_SYS (generally this would be in your lwipopts.h) LWIP_SOCKET=1 # we need the socket API (generally this would be in your lwipopts.h) PING_USE_SOCKETS=1 + PICO_MAX_SHARED_IRQ_HANDLERS=5 ) target_include_directories(rtos_car PRIVATE ${CMAKE_CURRENT_LIST_DIR}/config diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 6d2cf0e..07b6578 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -3,44 +3,48 @@ #define MOTOR_CONFIG_H // ENA and ENB on the L298N -#define PWM_PIN_RIGHT 1U // chanel B -#define PWM_PIN_LEFT 0U // chanel A +#define PWM_PIN_RIGHT 1U // chanel B +#define PWM_PIN_LEFT 0U // chanel A // IN1, IN2, IN3, IN4 on the L298N -#define DIRECTION_PIN_RIGHT_IN1 11U -#define DIRECTION_PIN_RIGHT_IN2 12U +#define DIRECTION_PIN_RIGHT_IN1 11U +#define DIRECTION_PIN_RIGHT_IN2 12U -#define DIRECTION_PIN_LEFT_IN3 19U -#define DIRECTION_PIN_LEFT_IN4 20U +#define DIRECTION_PIN_LEFT_IN3 19U +#define DIRECTION_PIN_LEFT_IN4 20U -#define DIRECTION_RIGHT_FORWARD (1U << DIRECTION_PIN_RIGHT_IN2) -#define DIRECTION_RIGHT_BACKWARD (1U << DIRECTION_PIN_RIGHT_IN1) -#define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4) -#define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3) +#define DIRECTION_RIGHT_FORWARD (1U << DIRECTION_PIN_RIGHT_IN2) +#define DIRECTION_RIGHT_BACKWARD (1U << DIRECTION_PIN_RIGHT_IN1) +#define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4) +#define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3) -#define DIRECTION_FORWARD (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_FORWARD) -#define DIRECTION_BACKWARD (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_BACKWARD) -#define DIRECTION_LEFT (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_BACKWARD) -#define DIRECTION_RIGHT (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_FORWARD) +#define DIRECTION_FORWARD (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_FORWARD) +#define DIRECTION_BACKWARD (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_BACKWARD) +#define DIRECTION_LEFT (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_BACKWARD) +#define DIRECTION_RIGHT (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_FORWARD) -#define SPEED_PIN_RIGHT 15U -#define SPEED_PIN_LEFT 16U +#define SPEED_PIN_RIGHT 15U +#define SPEED_PIN_LEFT 16U -#define PWM_CLK_DIV 250.f -#define PWM_WRAP 5000U +#define PWM_CLK_DIV 250.f +#define PWM_WRAP 5000U -#define MAX_SPEED 4900U -#define MIN_SPEED 0U // To be changed +#define MAX_SPEED 4900U +#define MIN_SPEED 0U // To be changed + +#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) +#define WHEEL_CONTROL_PRIO (tskIDLE_PRIORITY + 1UL) +#define WHEEL_PID_PRIO (tskIDLE_PRIORITY + 1UL) /*! * @brief Structure for the motor speed parameters - * @param target_speed_cms Target speed in cm/s * @param current_speed_cms Current speed in cm/s * @param distance_cm Distance travelled in cm */ -typedef struct { - float current_cms; - float distance_cm; +typedef struct +{ + float current_cms; + float distance_cm; } motor_speed_t; /*! @@ -49,7 +53,8 @@ typedef struct { * @param pwm_channel PWM channel, either A or B * @param pwm_level PWM level, from 0 to 5000 */ -typedef struct { +typedef struct +{ uint slice_num; uint channel; uint16_t level; @@ -61,7 +66,8 @@ typedef struct { * @param pid_ki Integral gain * @param pid_kd Derivative gain */ -typedef struct { +typedef struct +{ float kp_value; float ki_value; float kd_value; @@ -74,11 +80,19 @@ typedef struct { * @param pwm Motor PWM parameters * @param pid Motor PID parameters */ -typedef struct { +typedef struct +{ motor_speed_t speed; SemaphoreHandle_t sem; motor_pwm_t pwm; motor_pid_t pid; } motor_t; +typedef struct +{ + float starting_distance_cm; + float distance_to_travel_cm; + volatile bool is_running; +} distance_to_stop_t; + #endif /* MOTOR_CONFIG_H */ \ No newline at end of file diff --git a/frtos/motor/CMakeLists.txt b/frtos/motor/CMakeLists.txt index 6d4f305..b469a10 100644 --- a/frtos/motor/CMakeLists.txt +++ b/frtos/motor/CMakeLists.txt @@ -4,12 +4,14 @@ target_link_libraries(motor_test pico_cyw43_arch_lwip_sys_freertos pico_stdlib pico_lwip_iperf + hardware_i2c FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap hardware_pwm ) target_include_directories(motor_test PRIVATE ../config + ../magnetometer ) pico_enable_stdio_usb(motor_test 1) diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index 51389fe..e315635 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -27,47 +27,88 @@ set_wheel_direction(uint32_t direction) gpio_set_mask(direction); } -/*! - * @brief Turn the wheel, must set the priority higher than the motor PID task - * @param direction The direction of the wheel - * @param direction_after The direction of the wheel after turning - * @param speed_after The speed of the wheel after turning - */ void -turn_wheel(uint32_t direction) +turn_left_90() { - set_wheel_speed(0u); - vTaskDelay(pdMS_TO_TICKS(1000)); - float initial_right = g_motor_right.speed.distance_cm; - float initial_left = g_motor_left.speed.distance_cm; - - set_wheel_direction(direction); - set_wheel_speed(3500u); + set_wheel_direction(DIRECTION_RIGHT_FORWARD); + set_wheel_speed(3500u, &g_motor_right); + float initial = g_motor_right.speed.distance_cm; for (;;) { - // gap between wheels = 11.3cm, to turn 90 degrees, need to travel - // 11.3 * pi / 4 = 8.9cm - if (g_motor_left.speed.distance_cm - initial_left >= 6.8f) - { - g_motor_left.pwm.level = 0; - pwm_set_chan_level(g_motor_left.pwm.slice_num, - g_motor_left.pwm.channel, - g_motor_left.pwm.level); - } - - if (g_motor_right.speed.distance_cm - initial_right >= 6.8f) - { - g_motor_right.pwm.level = 0; - pwm_set_chan_level(g_motor_right.pwm.slice_num, - g_motor_right.pwm.channel, - g_motor_right.pwm.level); - } - - if (g_motor_left.pwm.level == 0u && g_motor_right.pwm.level == 0u) + if (g_motor_right.speed.distance_cm - initial >= 17.f) { + set_wheel_speed(0u, &g_motor_right); break; } + vTaskDelay(pdMS_TO_TICKS(5)); } vTaskDelay(pdMS_TO_TICKS(1000)); + g_motor_right.speed.distance_cm = initial; + g_motor_left.speed.distance_cm = initial; +} + +void +turn_right_90() +{ + set_wheel_direction(DIRECTION_LEFT_FORWARD); + set_wheel_speed(3500u, &g_motor_left); + + float initial = g_motor_left.speed.distance_cm; + for (;;) + { + if (g_motor_left.speed.distance_cm - initial >= 17.f) + { + set_wheel_speed(0u, &g_motor_left); + break; + } + vTaskDelay(pdMS_TO_TICKS(5)); + } + vTaskDelay(pdMS_TO_TICKS(1000)); + g_motor_left.speed.distance_cm = initial; + g_motor_right.speed.distance_cm = initial; +} + +void +spin_left_90() +{ + set_wheel_direction(DIRECTION_LEFT); + + set_wheel_speed_synced(3500u); + + float initial = g_motor_left.speed.distance_cm; + for (;;) + { + if (g_motor_left.speed.distance_cm - initial >= 7.5f) + { + set_wheel_speed_synced(0u); + break; + } + vTaskDelay(pdMS_TO_TICKS(5)); + } + vTaskDelay(pdMS_TO_TICKS(1000)); + g_motor_left.speed.distance_cm = initial; + g_motor_right.speed.distance_cm = initial; +} + +void +spin_right_90() +{ + set_wheel_direction(DIRECTION_RIGHT); + + set_wheel_speed_synced(3500u); + + float initial = g_motor_right.speed.distance_cm; + for (;;) + { + if (g_motor_right.speed.distance_cm - initial >= 7.5f) + { + set_wheel_speed_synced(0u); + break; + } + vTaskDelay(pdMS_TO_TICKS(5)); + } + vTaskDelay(pdMS_TO_TICKS(1000)); + g_motor_right.speed.distance_cm = initial; + g_motor_left.speed.distance_cm = initial; } \ No newline at end of file diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index c741ef6..16283f1 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -5,7 +5,7 @@ * @author Richie */ -#include "motor_init.h" +// #include "magnetometer_init.h" /*! * @brief Compute the control signal using PID controller @@ -21,6 +21,8 @@ compute_pid(float *integral, float *prev_error) float error = g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm; + printf("%f,\n", error); + *integral += error; float derivative = error - *prev_error; @@ -34,44 +36,38 @@ compute_pid(float *integral, float *prev_error) return control_signal; } -void -motor_pid_task(__unused void *p_param) +bool +repeating_pid_handler(__unused struct repeating_timer *t) { - float integral = 0.0f; - float prev_error = 0.0f; + static float integral = 0.0f; + static float prev_error = 0.0f; - for (;;) + if ((g_motor_left.pwm.level == 0u) || (g_motor_right.pwm.level == 0u)) { - if (g_motor_left.pwm.level == 0u) - { - g_motor_right.pwm.level = 0; - pwm_set_chan_level(g_motor_right.pwm.slice_num, - g_motor_right.pwm.channel, - g_motor_right.pwm.level); - vTaskDelay(pdMS_TO_TICKS(50)); - continue; - } - - float control_signal = compute_pid(&integral, &prev_error); - - float temp = (float) g_motor_right.pwm.level + control_signal * 0.05f; - - if (temp > MAX_SPEED) - { - temp = MAX_SPEED; - } - - if (temp < MIN_SPEED) - { - temp = MIN_SPEED; - } - - g_motor_right.pwm.level = (uint16_t) temp; - - pwm_set_chan_level(g_motor_right.pwm.slice_num, - g_motor_right.pwm.channel, - g_motor_right.pwm.level); - - vTaskDelay(pdMS_TO_TICKS(50)); + return true; } + + float control_signal = compute_pid(&integral, &prev_error); + + float temp = (float)g_motor_right.pwm.level + control_signal * 0.05f; + + if (temp > MAX_SPEED) + { + temp = MAX_SPEED; + } + + if (temp < MIN_SPEED) + { + temp = MIN_SPEED + 1u; + } + + g_motor_right.pwm.level = (uint16_t)temp; + pwm_set_chan_level(g_motor_right.pwm.slice_num, + g_motor_right.pwm.channel, + g_motor_right.pwm.level); + + printf("speed: %f cm/s\n", g_motor_right.speed.current_cms); + printf("distance: %f cm\n", g_motor_right.speed.distance_cm); + + return true; } \ No newline at end of file diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index d573276..4848f83 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -74,6 +74,26 @@ 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 5000 + */ +void +set_wheel_speed_synced(uint32_t pwm_level) +{ + set_wheel_speed(pwm_level, &g_motor_left); + set_wheel_speed(pwm_level, &g_motor_right); +} /*! * @brief Set the distance to travel before stopping, must be called after @@ -84,34 +104,16 @@ void distance_to_stop(float distance_cm) { float initial = g_motor_right.speed.distance_cm; - printf("initial: %f\n", initial); for (;;) { if (g_motor_right.speed.distance_cm - initial >= distance_cm) { - g_motor_right.pwm.level = 0; - g_motor_left.pwm.level = 0; + set_wheel_speed_synced(0u); break; } - // vTaskDelay(pdMS_TO_TICKS(50)); + vTaskDelay(pdMS_TO_TICKS(10)); } + vTaskDelay(pdMS_TO_TICKS(1000)); + g_motor_right.speed.distance_cm = g_motor_left.speed.distance_cm; } - -/*! - * @brief Set the speed of the wheels - * @param pwm_level The pwm_level of the wheels, from 0 to 5000 - */ -void -set_wheel_speed(uint32_t pwm_level) -{ - g_motor_right.pwm.level = pwm_level; - g_motor_left.pwm.level = pwm_level; - - pwm_set_chan_level(g_motor_right.pwm.slice_num, - g_motor_right.pwm.channel, - g_motor_right.pwm.level); - pwm_set_chan_level(g_motor_left.pwm.slice_num, - g_motor_left.pwm.channel, - g_motor_left.pwm.level); -} \ No newline at end of file diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index 440972b..b7dbe1f 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -4,37 +4,6 @@ #include "motor_direction.h" #include "motor_pid.h" -#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 2UL) -#define WHEEL_CONTROL_PRIO (tskIDLE_PRIORITY + 2UL) -#define WHEEL_PID_PRIO (tskIDLE_PRIORITY + 2UL) - -static void -motor_control_task(__unused void *p_param) -{ - for (;;) - { - set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed(3000u); - distance_to_stop(30); - - set_wheel_direction(DIRECTION_BACKWARD); - set_wheel_speed(3000u); - distance_to_stop(30); - - turn_wheel(DIRECTION_LEFT); - set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed(3000u); - distance_to_stop(30); - - turn_wheel(DIRECTION_RIGHT); - set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed(3000u); - distance_to_stop(30); - - set_wheel_speed(0u); - vTaskDelay(pdMS_TO_TICKS(3000)); - } -} void launch() @@ -49,8 +18,9 @@ launch() irq_set_enabled(IO_IRQ_BANK0, true); -// set_wheel_direction(DIRECTION_FORWARD); -// set_wheel_speed(3000); + // PID timer + struct repeating_timer pid_timer; + add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer); // Left wheel // @@ -72,14 +42,6 @@ launch() WHEEL_SPEED_PRIO, &h_monitor_right_wheel_speed_task_handle); - TaskHandle_t h_motor_pid_right_task_handle = NULL; - xTaskCreate(motor_pid_task, - "motor_pid_task", - configMINIMAL_STACK_SIZE, - (void *)&g_motor_right, - WHEEL_PID_PRIO, - &h_motor_pid_right_task_handle); - // control task TaskHandle_t h_motor_turning_task_handle = NULL; xTaskCreate(motor_control_task, diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c index 3cbd440..7c52131 100644 --- a/frtos/rtos_car.c +++ b/frtos/rtos_car.c @@ -14,46 +14,90 @@ #include "motor_speed.h" #include "motor_direction.h" +#include "motor_pid.h" #include "line_sensor.h" #include "ultrasonic_sensor.h" -#define READ_LEFT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) -#define READ_RIGHT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) +#define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) +#define READ_RIGHT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) +#define READ_BARCODE_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) -#define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) -#define READ_RIGHT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) -#define READ_BARCODE_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) +#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 3UL) -#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 3UL) - -#define DISTANCE_TASK_PRIORITY (tskIDLE_PRIORITY + 4UL) +#define DISTANCE_TASK_PRIORITY (tskIDLE_PRIORITY + 4UL) /* Common Car State Structure (TODO: TBC)*/ -//static car_state_t g_car_state; +// static car_state_t g_car_state; + +static void +motor_control_task(__unused void *p_param) +{ + static bool left = false; + static bool right = false; + + for (;;) + { + state_t state = gpio_get(LEFT_SENSOR_PIN); + printf("state: %d\n", state); + + if (state == 1) + { + if (!left) + { + spin_left_90(); + left = true; + } + else if (!right) + { + spin_right_90(); + spin_right_90(); + right = true; + } + else + { + spin_right_90(); + left = false; + right = false; + } + } + else + { + set_wheel_direction(DIRECTION_FORWARD); + set_wheel_speed_synced(3500u); + } + vTaskDelay(pdMS_TO_TICKS(5)); + } +} void launch() { +// // isr to detect left line sensor +// gpio_set_irq_enabled(LEFT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true); +// gpio_add_raw_irq_handler(LEFT_SENSOR_PIN, h_line_sensor_handler); - struct repeating_timer g_left_sensor_timer; - add_repeating_timer_ms(LINE_SENSOR_READ_DELAY, - h_left_sensor_timer_handler, - NULL, - &g_left_sensor_timer); + // 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, h_wheel_sensor_isr_handler); - struct repeating_timer g_right_sensor_timer; - add_repeating_timer_ms(LINE_SENSOR_READ_DELAY, - h_right_sensor_timer_handler, - NULL, - &g_right_sensor_timer); + // 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, h_wheel_sensor_isr_handler); - struct repeating_timer g_barcode_sensor_timer; - add_repeating_timer_ms(LINE_SENSOR_READ_DELAY, - h_barcode_sensor_timer_handler, - NULL, - &g_barcode_sensor_timer); + irq_set_enabled(IO_IRQ_BANK0, true); + +// // line sensor timer +// struct repeating_timer g_left_sensor_timer; +// add_repeating_timer_ms(LINE_SENSOR_READ_DELAY, +// h_left_sensor_timer_handler, +// NULL, +// &g_left_sensor_timer); + + // PID timer + struct repeating_timer pid_timer; + add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer); TaskHandle_t h_monitor_left_sensor_task; xTaskCreate(monitor_left_sensor_task, @@ -63,55 +107,53 @@ launch() READ_LEFT_SENSOR_PRIO, &h_monitor_left_sensor_task); - TaskHandle_t h_monitor_right_sensor_task; - xTaskCreate(monitor_right_sensor_task, - "Monitor Right Sensor Task", + // Left wheel + // + TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; + xTaskCreate(monitor_wheel_speed_task, + "monitor_left_wheel_speed_task", configMINIMAL_STACK_SIZE, - NULL, - READ_RIGHT_SENSOR_PRIO, - &h_monitor_right_sensor_task); - - TaskHandle_t h_monitor_barcode_sensor_task; - xTaskCreate(monitor_barcode_sensor_task, - "Monitor Barcode Sensor Task", - configMINIMAL_STACK_SIZE, - NULL, - READ_BARCODE_SENSOR_PRIO, - &h_monitor_barcode_sensor_task); + (void *)&g_motor_left, + WHEEL_SPEED_PRIO, + &h_monitor_left_wheel_speed_task_handle); - TaskHandle_t h_monitor_direction_task; - xTaskCreate(monitor_direction_task, - "Monitor Direction Task", + // Right wheel + // + TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL; + xTaskCreate(monitor_wheel_speed_task, + "monitor_wheel_speed_task", + configMINIMAL_STACK_SIZE, + (void *)&g_motor_right, + WHEEL_SPEED_PRIO, + &h_monitor_right_wheel_speed_task_handle); + + // control task + TaskHandle_t h_motor_turning_task_handle = NULL; + xTaskCreate(motor_control_task, + "motor_turning_task", configMINIMAL_STACK_SIZE, NULL, - DIRECTION_TASK_PRIORITY, - &h_monitor_direction_task); - - TaskHandle_t h_monitor_distance_task; - xTaskCreate(distance_task, - "Monitor Distance Task", - configMINIMAL_STACK_SIZE, - NULL, - DISTANCE_TASK_PRIORITY, - &h_monitor_distance_task); + WHEEL_CONTROL_PRIO, + &h_motor_turning_task_handle); vTaskStartScheduler(); } int -main (void) +main(void) { stdio_usb_init(); - motor_init(); + sleep_ms(4000); + printf("Test started!\n"); - set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD); + motor_init(); line_sensor_setup(); - init_ultrasonic(); + //init_ultrasonic(); - initialize_car_state(); // TODO: Could be common functionality, To confirm + //initialize_car_state(); // TODO: Could be common functionality, To confirm // during Integration launch();