From 19d651263926fbb64cb1a4ba96f1d947805af3d0 Mon Sep 17 00:00:00 2001 From: soapysoltionss <2200754@sit.singaporetech.edu.sg> Date: Sun, 29 Oct 2023 15:46:20 +0800 Subject: [PATCH 01/23] barcode update --- frtos/config/line_sensor_config.h | 1 + frtos/line_sensor/line_sensor.h | 42 ++++++++++++++++++++++++++ frtos/line_sensor/line_sensor_init.h | 45 ++++++++++++++++++++++++++-- frtos/line_sensor/line_sensor_test.c | 13 ++++++++ frtos/rtos_car.c | 15 ++++++++++ 5 files changed, 114 insertions(+), 2 deletions(-) diff --git a/frtos/config/line_sensor_config.h b/frtos/config/line_sensor_config.h index f9f7c72..68ec354 100644 --- a/frtos/config/line_sensor_config.h +++ b/frtos/config/line_sensor_config.h @@ -7,6 +7,7 @@ #define LEFT_SENSOR_PIN ( 26 ) #define RIGHT_SENSOR_PIN ( 27 ) +#define BARCODE_SENSOR_PIN ( 22 ) /* Map */ diff --git a/frtos/line_sensor/line_sensor.h b/frtos/line_sensor/line_sensor.h index 23c30e0..11c6429 100644 --- a/frtos/line_sensor/line_sensor.h +++ b/frtos/line_sensor/line_sensor.h @@ -6,6 +6,7 @@ #include "line_sensor_init.h" + /** * @brief Monitor the left sensor * @@ -66,7 +67,42 @@ monitor_right_sensor_task(void *params) { } } +/** + * @brief Monitor the barcode sensor + * + * This function will monitor the barcode sensor and send the state to the + * barcode sensor message buffer, used to scan the barcode below the car + * + * @param params + */ +void monitor_barcode_sensor_task(void *params) { + for (;;) { + if (xSemaphoreTake(g_barcode_sensor_sem, portMAX_DELAY) == pdTRUE) { + // Check the flag or receive the message + if (barcode_sensor_triggered == pdTRUE) { + uint32_t barcode_data = 0; + for (int i = 0; i < 9; i++) { + sleep_ms(100); // Wait for a segment of the barcode + + if (gpio_get(BARCODE_SENSOR_PIN)) { + barcode_data |= (1u << i); + } else { + barcode_data &= ~(1u << i); + } + } + + printf("Barcode Data (binary): %09b\n", barcode_data); + + // Send or process the barcode data + xMessageBufferSend(barcode_sensor_msg_buffer, &barcode_data, sizeof(uint32_t), 0); + + // Reset the flag + barcode_sensor_triggered = pdFALSE; + } + } + } +} /** * @brief Monitor the direction and Oritentation of the car * @@ -79,6 +115,7 @@ void monitor_direction_task(__unused void *params) { state_t left_state; state_t right_state; + state_t barcode_state; for (;;) { @@ -92,6 +129,11 @@ monitor_direction_task(__unused void *params) { &right_state, sizeof(state_t), portMAX_DELAY); + + xMessageBufferReceive(barcode_sensor_msg_buffer, + &barcode_state, + sizeof(state_t), + portMAX_DELAY); // g_car_state.current_direction = (left_state << 1) | right_state; diff --git a/frtos/line_sensor/line_sensor_init.h b/frtos/line_sensor/line_sensor_init.h index ecfb4ec..0d45987 100644 --- a/frtos/line_sensor/line_sensor_init.h +++ b/frtos/line_sensor/line_sensor_init.h @@ -23,6 +23,7 @@ static TickType_t lastEdgeTimeLeft = 0; static TickType_t lastEdgeTimeRight = 0; +static TickType_t lastBarcodeTime = 0; typedef enum { // Unused, useful for readability LINE_DETECTED = 0, @@ -53,13 +54,16 @@ typedef struct { // Semaphore SemaphoreHandle_t g_left_sensor_sem = NULL; SemaphoreHandle_t g_right_sensor_sem = NULL; +SemaphoreHandle_t g_barcode_sensor_sem = NULL; // Queue static MessageBufferHandle_t left_sensor_msg_buffer; // Left Sensor Buffer static MessageBufferHandle_t right_sensor_msg_buffer; // Right Sensor Buffer +static MessageBufferHandle_t barcode_sensor_msg_buffer; // Barcode Sensor Buffer static volatile BaseType_t right_sensor_triggered = pdFALSE; static volatile BaseType_t left_sensor_triggered = pdFALSE; +static volatile BaseType_t barcode_sensor_triggered = pdFALSE; // Car State Struct static car_state_t g_car_state; @@ -82,15 +86,17 @@ static inline void line_sensor_setup() { g_left_sensor_sem = xSemaphoreCreateBinary(); g_right_sensor_sem = xSemaphoreCreateBinary(); + g_barcode_sensor_sem = xSemaphoreCreateBinary(); - uint mask = (1 << LEFT_SENSOR_PIN) | (1 << RIGHT_SENSOR_PIN); + uint mask = (1 << LEFT_SENSOR_PIN) | (1 << RIGHT_SENSOR_PIN) | (1 << BARCODE_SENSOR_PIN); - // Initialise 2 GPIO pins and set them to input + // Initialise 3 GPIO pins and set them to input gpio_init_mask(mask); gpio_set_dir_in_masked(mask); left_sensor_msg_buffer = xMessageBufferCreate(30); right_sensor_msg_buffer = xMessageBufferCreate(30); + barcode_sensor_msg_buffer = xMessageBufferCreate(30); } @@ -124,6 +130,22 @@ bool h_right_sensor_timer_handler(repeating_timer_t *repeatingTimer) { return true; } +/** + * @brief Timer Interrupt Handler for the barcode sensor + * + * @param repeatingTimer + * @return True (To keep the timer running) + */ +bool h_barcode_sensor_timer_handler(repeating_timer_t *repeatingTimer) { + + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(g_barcode_sensor_sem, + &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + + return true; +} + void h_line_sensor_handler(void) { BaseType_t xHigherPriorityTaskWoken = pdFALSE; TickType_t currentTicks = xTaskGetTickCount(); @@ -166,6 +188,25 @@ void h_line_sensor_handler(void) { } } + if (gpio_get_irq_event_mask(BARCODE_SENSOR_PIN) & GPIO_IRQ_EDGE_FALL) + { + if ((currentTicks - lastBarcodeTime) >= + pdMS_TO_TICKS(DEBOUNCE_DELAY_MS)) + { + lastBarcodeTime = currentTicks; + gpio_acknowledge_irq(BARCODE_SENSOR_PIN, GPIO_IRQ_EDGE_FALL); + // Set the flag to notify the task + barcode_sensor_triggered = pdTRUE; + xSemaphoreGiveFromISR(g_barcode_sensor_sem, + &xHigherPriorityTaskWoken); + } + else + { + // Reset the timer to the currentTicks if the edge is ignored + lastBarcodeTime = currentTicks; + } + } + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } diff --git a/frtos/line_sensor/line_sensor_test.c b/frtos/line_sensor/line_sensor_test.c index 44750da..054384a 100644 --- a/frtos/line_sensor/line_sensor_test.c +++ b/frtos/line_sensor/line_sensor_test.c @@ -3,6 +3,7 @@ #define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) #define READ_RIGHT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) +#define READ_RIGHT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) #define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 3UL) @@ -17,6 +18,10 @@ launch() gpio_set_irq_enabled(RIGHT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true); gpio_add_raw_irq_handler(RIGHT_SENSOR_PIN, h_line_sensor_handler); + // isr to detect barcode line sensor + gpio_set_irq_enabled(BARCODE_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true); + gpio_add_raw_irq_handler(BARCODE_SENSOR_PIN, h_line_sensor_handler); + irq_set_enabled(IO_IRQ_BANK0, true); // struct repeating_timer g_left_sensor_timer; @@ -47,6 +52,14 @@ launch() 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_RIGHT_SENSOR_PRIO, + &h_monitor_right_sensor_task); + // TaskHandle_t h_monitor_direction_task; // xTaskCreate(monitor_direction_task, // "Monitor Direction Task", diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c index 8084001..50df6a6 100644 --- a/frtos/rtos_car.c +++ b/frtos/rtos_car.c @@ -24,6 +24,7 @@ #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) @@ -48,6 +49,12 @@ launch() NULL, &g_right_sensor_timer); + 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); + TaskHandle_t h_monitor_left_sensor_task; xTaskCreate(monitor_left_sensor_task, "Monitor Left Sensor Task", @@ -63,6 +70,14 @@ launch() 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); TaskHandle_t h_monitor_direction_task; xTaskCreate(monitor_direction_task, From ed516b5ce4be8ad677d65b9e71a9d1ae92e6f947 Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Mon, 30 Oct 2023 23:20:15 +0800 Subject: [PATCH 02/23] add direction control --- frtos/motor/motor_direction.h | 49 +++++++++++++++++++++++++++-------- frtos/motor/motor_test.c | 48 +++++++++++++++++++++++++++++----- 2 files changed, 80 insertions(+), 17 deletions(-) diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index f81273f..51389fe 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -28,19 +28,46 @@ set_wheel_direction(uint32_t direction) } /*! - * @brief Set the speed of the wheels - * @param speed The speed of the wheels, from 0 to 5000 + * @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 -set_wheel_speed(uint32_t speed) +turn_wheel(uint32_t direction) { - g_motor_right.pwm.level = speed; - g_motor_left.pwm.level = speed; + 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; - 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); + set_wheel_direction(direction); + set_wheel_speed(3500u); + + 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) + { + break; + } + } + vTaskDelay(pdMS_TO_TICKS(1000)); } \ No newline at end of file diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index a5f16ff..440972b 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -4,7 +4,37 @@ #include "motor_direction.h" #include "motor_pid.h" -#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) +#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() @@ -19,8 +49,8 @@ launch() irq_set_enabled(IO_IRQ_BANK0, true); - // Set wheel speed - set_wheel_speed(3500); +// set_wheel_direction(DIRECTION_FORWARD); +// set_wheel_speed(3000); // Left wheel // @@ -32,7 +62,6 @@ launch() WHEEL_SPEED_PRIO, &h_monitor_left_wheel_speed_task_handle); - // Right wheel // TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL; @@ -48,9 +77,17 @@ launch() "motor_pid_task", configMINIMAL_STACK_SIZE, (void *)&g_motor_right, - WHEEL_SPEED_PRIO, + WHEEL_PID_PRIO, &h_motor_pid_right_task_handle); + // control task + TaskHandle_t h_motor_turning_task_handle = NULL; + xTaskCreate(motor_control_task, + "motor_turning_task", + configMINIMAL_STACK_SIZE, + NULL, + WHEEL_CONTROL_PRIO, + &h_motor_turning_task_handle); vTaskStartScheduler(); } @@ -64,7 +101,6 @@ main(void) printf("Test started!\n"); motor_init(); - set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD); launch(); From ed3c440ebc75ec94c5ad7f7a0314c07860f48d6f Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Mon, 30 Oct 2023 23:24:58 +0800 Subject: [PATCH 03/23] add direction control --- frtos/config/motor_config.h | 5 +++++ frtos/motor/motor_speed.h | 42 +++++++++++++++++++++++++++++++++++++ 2 files changed, 47 insertions(+) diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 267d672..6d2cf0e 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -18,6 +18,11 @@ #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 SPEED_PIN_RIGHT 15U #define SPEED_PIN_LEFT 16U diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index 520fbee..d573276 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -72,4 +72,46 @@ monitor_wheel_speed_task(void *pvParameters) p_motor->speed.current_cms = 0.f; } } +} + + +/*! + * @brief Set the distance to travel before stopping, must be called after + * setting the speed and direction. + * @param distance_cm distance to travel in cm + */ +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; + break; + } + // vTaskDelay(pdMS_TO_TICKS(50)); + } +} + +/*! + * @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 From 2d0f17a4bc4072aefe7bf00cd70f277cf0c2ee5e Mon Sep 17 00:00:00 2001 From: soapysoltionss <2200754@sit.singaporetech.edu.sg> Date: Tue, 31 Oct 2023 15:45:37 +0800 Subject: [PATCH 04/23] Revert "add direction control" This reverts commit ed3c440ebc75ec94c5ad7f7a0314c07860f48d6f. --- frtos/config/motor_config.h | 5 ----- frtos/motor/motor_speed.h | 42 ------------------------------------- 2 files changed, 47 deletions(-) diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 6d2cf0e..267d672 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -18,11 +18,6 @@ #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 SPEED_PIN_RIGHT 15U #define SPEED_PIN_LEFT 16U diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index d573276..520fbee 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -72,46 +72,4 @@ monitor_wheel_speed_task(void *pvParameters) p_motor->speed.current_cms = 0.f; } } -} - - -/*! - * @brief Set the distance to travel before stopping, must be called after - * setting the speed and direction. - * @param distance_cm distance to travel in cm - */ -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; - break; - } - // vTaskDelay(pdMS_TO_TICKS(50)); - } -} - -/*! - * @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 From 4137d4f350a56af5538d410f4ff6033eeb9ddfca Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Tue, 31 Oct 2023 21:52:09 +0800 Subject: [PATCH 05/23] updated motor with testing codes with line sensor --- frtos/CMakeLists.txt | 1 + frtos/config/motor_config.h | 68 +++++++++------ frtos/motor/CMakeLists.txt | 2 + frtos/motor/motor_direction.h | 107 +++++++++++++++-------- frtos/motor/motor_pid.h | 70 ++++++++-------- frtos/motor/motor_speed.h | 46 +++++----- frtos/motor/motor_test.c | 44 +--------- frtos/rtos_car.c | 154 +++++++++++++++++++++------------- 8 files changed, 276 insertions(+), 216 deletions(-) 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(); From f933d8ea916871d8cdb7a5a5bd1206b1ec1cbeca Mon Sep 17 00:00:00 2001 From: Devoalda Date: Sat, 4 Nov 2023 13:38:47 +0800 Subject: [PATCH 06/23] fix(Magnetometer): Removed Com Filter --- frtos/config/magnetometer_config.h | 6 +- frtos/magnetometer/magnetometer_direction.h | 220 +++++++++++++++----- frtos/magnetometer/magnetometer_init.h | 1 - frtos/magnetometer/magnetometer_test.c | 7 +- frtos/magnetometer/map.h | 128 ++++++++++++ 5 files changed, 303 insertions(+), 59 deletions(-) create mode 100644 frtos/magnetometer/map.h diff --git a/frtos/config/magnetometer_config.h b/frtos/config/magnetometer_config.h index 2996d79..5acb030 100644 --- a/frtos/config/magnetometer_config.h +++ b/frtos/config/magnetometer_config.h @@ -7,14 +7,14 @@ #define DIRECTION_READ_DELAY ( 100 ) -#define ALPHA ( 0.01f ) // Complementary +#define ALPHA ( 0.0f ) // Complementary // Filter Constant // LSM303DLHC temperature compensation coefficients #define SCALE_Z ( 1.0f ) // Scale for Z-axis -#define OFFSET_Z ( 3.0f ) // Offset for Z-axis +#define OFFSET_Z ( 0.0f ) // Offset for Z-axis -#define TEMPERATURE_OFFSET ( 25.0f ) // Reference +#define TEMPERATURE_OFFSET ( 32.0f ) // Reference // temperature for // calibration diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h index c7d7189..b8addee 100644 --- a/frtos/magnetometer/magnetometer_direction.h +++ b/frtos/magnetometer/magnetometer_direction.h @@ -24,6 +24,7 @@ #define MAGNETOMETER_DIRECTION_H #include "magnetometer_init.h" +#include "map.h" /** * @brief Roll Calculation with Accelerometer Data @@ -55,7 +56,7 @@ calculate_pitch(int16_t acceleration[3]) { */ static inline float calculate_yaw_magnetometer(int16_t magnetometer[3]) { - return atan2(magnetometer[1], magnetometer[0]) * (180.0 / M_PI); + return atan2(magnetometer[1], magnetometer[0]) * (180.0f / M_PI); } /** @@ -78,11 +79,11 @@ calculate_yaw_complementary(float yaw_acc, float yaw_mag) { float compensate_magnetometer(float yaw_mag, int16_t temperature) { // Calculate temperature difference from the reference temperature - float delta_temp = (float) (temperature - TEMPERATURE_OFFSET); + uint delta_temp = temperature - TEMPERATURE_OFFSET; // Apply temperature compensation to each axis using macros float compensated_yaw_mag = - yaw_mag - (delta_temp * TEMPERATURE_COEFFICIENT_Z); + yaw_mag - ((float) delta_temp * TEMPERATURE_COEFFICIENT_Z); // Apply scale and offset corrections using macros compensated_yaw_mag = (compensated_yaw_mag - OFFSET_Z) * SCALE_Z; @@ -97,7 +98,12 @@ compensate_magnetometer(float yaw_mag, int16_t temperature) { */ static inline float adjust_yaw(float yaw) { - return (yaw < 0) ? yaw + 360.0f : yaw; + if (yaw < 0) + { + yaw += 360; + } + + return yaw; } /** @@ -106,33 +112,86 @@ adjust_yaw(float yaw) { * the compass direction enum * 45.0 = 360 / 8, used to calculate the compass direction from * the orientation (0 - 7) - * @param yaw Yaw calculated from Complementary Filter + * @param yaw Yaw calculated * @return Compass Direction */ static inline compass_direction_t calculate_compass_direction(float yaw) { - int orientation = (int) ((yaw + 22.5) / 45.0) % 8; // 8 compass directions - switch (orientation) + if (yaw >= 337.5 || yaw < 22.5) { - case 0: - return NORTH; - case 1: - return NORTH_EAST; - case 2: - return EAST; - case 3: - return SOUTH_EAST; - case 4: - return SOUTH; - case 5: - return SOUTH_WEST; - case 6: - return WEST; - case 7: - return NORTH_WEST; - default: - return NORTH; + return NORTH; } + else + { + if (yaw >= 22.5 && yaw < 67.5) + { + return NORTH_EAST; + } + else + { + if (yaw >= 67.5 && yaw < 112.5) + { + return EAST; + } + else + { + if (yaw >= 112.5 && yaw < 157.5) + { + return SOUTH_EAST; + } + else + { + if (yaw >= 157.5 && yaw < 202.5) + { + return SOUTH; + } + else + { + if (yaw >= 202.5 && yaw < 247.5) + { + return SOUTH_WEST; + } + else + { + if (yaw >= 247.5 && yaw < 292.5) + { + return WEST; + } + else + { + if (yaw >= 292.5 && yaw < 337.5) + { + return NORTH_WEST; + } + } + } + } + } + } + } + } +// int orientation = (int) ((yaw + 22.5) / 45.0) % 8; // 8 compass directions +// switch (orientation) +// { +// case 0: +// return NORTH; +// case 1: +// return NORTH_EAST; +// case 2: +// return EAST; +// case 3: +// return SOUTH_EAST; +// case 4: +// return SOUTH; +// case 5: +// return SOUTH_WEST; +// case 6: +// return WEST; +// case 7: +// return NORTH_WEST; +// default: +// return NORTH; +// } } /** @@ -169,18 +228,27 @@ read_direction(int16_t acceleration[3], float pitch = calculate_pitch(acceleration); float yaw_mag = calculate_yaw_magnetometer(magnetometer); + yaw_mag = adjust_yaw(yaw_mag); + // Apply temperature compensation to the magnetometer data - float compensated_mag_yaw = compensate_magnetometer(yaw_mag, - temperature[0]); +// float compensated_mag_yaw = compensate_magnetometer(yaw_mag, +// temperature[0]); +// compensated_mag_yaw = adjust_yaw(compensated_mag_yaw); - float yaw_acc = atan2(acceleration[1], acceleration[0]) * (180.0 / M_PI); - float yaw = calculate_yaw_complementary(yaw_acc, compensated_mag_yaw); +// float yaw_acc = atan2(acceleration[1], acceleration[0]) * (180.0f / M_PI); +// yaw_acc = adjust_yaw(yaw_acc); +// +// float yaw = calculate_yaw_complementary(yaw_acc, yaw_mag); - yaw = adjust_yaw(yaw); +// yaw = adjust_yaw(yaw); +// printf("Yaw: %f\n", yaw); - compass_direction_t compass_direction = calculate_compass_direction(yaw); + compass_direction_t compass_direction = calculate_compass_direction(yaw_mag); - update_orientation_data(roll, pitch, yaw, compass_direction); + update_orientation_data(roll, + pitch, + yaw_mag, + compass_direction); } /** @@ -251,34 +319,80 @@ void print_roll_and_pitch(angle_t roll_angle, angle_t pitch_angle) { } } +void updateDirection() { + int16_t magnetometer[3]; + int16_t accelerometer[3]; + int16_t temperature[1]; + + static int cur_x = 0; + static int cur_y = 0; + + read_magnetometer(magnetometer); + read_accelerometer(accelerometer); + read_temperature(temperature); + + read_direction(accelerometer, magnetometer, temperature); + + // Temperature in degrees Celsius + printf("Temperature: %d\n", temperature[0]); + + print_orientation_data(); + + printf("Direction: "); + + print_direction(g_direction.orientation); + + switch (g_direction.orientation) + { + case NORTH: + cur_y ++; + break; + case EAST: + cur_x ++; + break; + case SOUTH: + cur_y --; + break; + case WEST: + cur_x --; + break; + case NORTH_EAST: + cur_x ++; + cur_y ++; + break; + case SOUTH_EAST: + cur_x ++; + cur_y --; + break; + case SOUTH_WEST: + cur_x --; + cur_y --; + break; + case NORTH_WEST: + cur_x --; + cur_y ++; + break; + } + + // Update the map based on the direction of the car (N, E, S, W) +// update_map(g_direction.orientation, cur_x, cur_y); + +// printf("Current Position: (%d, %d)\n", cur_x, cur_y); +// print_map(); + +// print_roll_and_pitch(g_direction.roll_angle, g_direction.pitch_angle); +} + + void monitor_direction_task(__unused void *params) { for (;;) { if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE) { - int16_t magnetometer[3]; - int16_t accelerometer[3]; - int16_t temperature[1]; - - read_magnetometer(magnetometer); - read_accelerometer(accelerometer); - read_temperature(temperature); - - read_direction(accelerometer, magnetometer, temperature); - - // Temperature in degrees Celsius - printf("Temperature: %d\n", temperature[0]); - - print_orientation_data(); - - printf("Direction: "); - - print_direction(g_direction.orientation); - - print_roll_and_pitch(g_direction.roll_angle, - g_direction.pitch_angle); + updateDirection(); } } } + #endif \ No newline at end of file diff --git a/frtos/magnetometer/magnetometer_init.h b/frtos/magnetometer/magnetometer_init.h index 96ab986..1de1432 100644 --- a/frtos/magnetometer/magnetometer_init.h +++ b/frtos/magnetometer/magnetometer_init.h @@ -32,7 +32,6 @@ SemaphoreHandle_t g_direction_sem = NULL; direction_t g_direction = { .roll = 0, .pitch = 0, -// .heading = 0, .yaw = 0, .orientation = NORTH, .roll_angle = LEFT, diff --git a/frtos/magnetometer/magnetometer_test.c b/frtos/magnetometer/magnetometer_test.c index b509d42..6b137db 100644 --- a/frtos/magnetometer/magnetometer_test.c +++ b/frtos/magnetometer/magnetometer_test.c @@ -2,6 +2,7 @@ #include "magnetometer_init.h" #include "magnetometer_read.h" #include "magnetometer_direction.h" +#include "map.h" #define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 1UL) @@ -30,9 +31,11 @@ main (void) { stdio_usb_init(); -// sleep_ms(2000); + int grid_rows = 10; // Define the number of rows in your grid + int grid_cols = 10; // Define the number of columns in your grid + + car_path_grid = create_grid(grid_rows, grid_cols); -// printf("Test started!\n"); magnetometer_init(); launch(); diff --git a/frtos/magnetometer/map.h b/frtos/magnetometer/map.h new file mode 100644 index 0000000..2737233 --- /dev/null +++ b/frtos/magnetometer/map.h @@ -0,0 +1,128 @@ +// +// Created by junwei on 31/10/23. +// + +#include +#include +#include + +#ifndef TEST_PROJECT_MAP_H +#define TEST_PROJECT_MAP_H + + +// Define the grid structure +typedef struct { + bool **data; // 2D array to represent the grid + int rows; // Number of rows in the grid + int cols; // Number of columns in the grid +} Grid; + +// Global grid to track the car's path +Grid *car_path_grid; + +// Function to create and initialize a grid +Grid *create_grid(int rows, int cols) { + Grid *grid = (Grid *) malloc(sizeof(Grid)); + grid->rows = rows; + grid->cols = cols; + + // Allocate memory for the 2D array + grid->data = (bool **) malloc(rows * sizeof(bool *)); + for (int i = 0; i < rows; i++) { + grid->data[i] = (bool *) malloc(cols * sizeof(bool)); + for (int j = 0; j < cols; j++) { + grid->data[i][j] = false; // Initialize to 'false' (unvisited) + } + } + + return grid; +} + +// Function to mark a cell as visited +void mark_cell(Grid *grid, int row, int col) { + if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols) { + grid->data[row][col] = true; + } +} + +// Function to check if a cell has been visited +bool is_cell_visited(Grid *grid, int row, int col) { + if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols) { + return grid->data[row][col]; + } + return false; // Consider out-of-bounds as unvisited +} + +// Function to destroy the grid and free memory +void destroy_grid(Grid *grid) { + for (int i = 0; i < grid->rows; i++) { + free(grid->data[i]); + } + free(grid->data); + free(grid); +} + +// Function to update the map based on car's current orientation +// Function to update the map based on car's current orientation and position +void update_map(int orientation, int cur_x, int cur_y) { + // Define offsets for different orientations + int offset_x = 0; + int offset_y = 0; + + switch (orientation) { + case NORTH: + offset_y = 1; + break; + case EAST: + offset_x = 1; + break; + case SOUTH: + offset_y = -1; + break; + case WEST: + offset_x = -1; + break; + case NORTH_EAST: + offset_x = 1; + offset_y = 1; + break; + case SOUTH_EAST: + offset_x = 1; + offset_y = -1; + break; + case SOUTH_WEST: + offset_x = -1; + offset_y = -1; + break; + case NORTH_WEST: + offset_x = -1; + offset_y = 1; + break; + } + + // Update the map based on the car's current position and orientation + mark_cell(car_path_grid, cur_x, cur_y); + mark_cell(car_path_grid, cur_x + offset_x, cur_y + offset_y); +} + + +// Function to print the map +void print_map() { + // Invert the map, 0,0 is at the Middle + // Print 1 for visited cells and 0 for unvisited cells + for (int i = car_path_grid->rows - 1; i >= 0; i--) { + for (int j = 0; j < car_path_grid->cols; j++) { + (car_path_grid->data[j][i]) ? printf("1 ") : printf("0 "); +// case false: +// printf("0 "); +// break; +// case true: +// printf("1 "); +// break; +// } + } + printf("\n"); + } +} + +#endif //TEST_PROJECT_MAP_H From 389c4ecd92e013169412ab4906d132fabf6f7b93 Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Mon, 6 Nov 2023 20:45:42 +0800 Subject: [PATCH 07/23] add function to revert wheel direction --- frtos/config/motor_config.h | 2 ++ frtos/motor/motor_direction.h | 20 +++++++++++++++----- 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 07b6578..509f9f9 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -23,6 +23,8 @@ #define DIRECTION_LEFT (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_BACKWARD) #define DIRECTION_RIGHT (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_FORWARD) +#define DIRECTION_MASK (DIRECTION_FORWARD | DIRECTION_BACKWARD) + #define SPEED_PIN_RIGHT 15U #define SPEED_PIN_LEFT 16U diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index e315635..a5ad873 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -19,14 +19,24 @@ void set_wheel_direction(uint32_t direction) { - static const uint32_t mask - = DIRECTION_LEFT_FORWARD | DIRECTION_LEFT_BACKWARD - | DIRECTION_RIGHT_FORWARD | DIRECTION_RIGHT_BACKWARD; - - gpio_put_masked(mask, 0U); + gpio_put_masked(DIRECTION_MASK, 0U); gpio_set_mask(direction); } +/*! + * @brief Set the direction of the wheel to opposite direction using bit mask + */ +void +revert_wheel_direction() +{ + uint32_t current_direction = gpio_get_all(); + + uint32_t reverted_direction = current_direction ^ DIRECTION_MASK; + + gpio_put_masked(DIRECTION_MASK, 0U); + gpio_set_mask(reverted_direction & DIRECTION_MASK); +} + void turn_left_90() { From c19b665e0bb610411103be0a61bc5c142208e39e Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Mon, 6 Nov 2023 23:46:47 +0800 Subject: [PATCH 08/23] update PWM and PID value --- frtos/config/motor_config.h | 10 ++++++---- frtos/motor/motor_init.h | 16 +++++++++++----- frtos/motor/motor_pid.h | 16 ++++++++-------- frtos/motor/motor_speed.h | 7 ++++++- frtos/motor/motor_test.c | 16 ++++++++++++++++ 5 files changed, 47 insertions(+), 18 deletions(-) diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 509f9f9..38a81ea 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -28,11 +28,11 @@ #define SPEED_PIN_RIGHT 15U #define SPEED_PIN_LEFT 16U -#define PWM_CLK_DIV 250.f -#define PWM_WRAP 5000U +#define PWM_CLK_DIV 50.f +#define PWM_WRAP 100U -#define MAX_SPEED 4900U -#define MIN_SPEED 0U // To be changed +#define MAX_PWM_LEVEL 99U +#define MIN_PWM_LEVEL 0U #define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) #define WHEEL_CONTROL_PRIO (tskIDLE_PRIORITY + 1UL) @@ -97,4 +97,6 @@ typedef struct volatile bool is_running; } distance_to_stop_t; +volatile bool g_use_pid = true; + #endif /* MOTOR_CONFIG_H */ \ No newline at end of file diff --git a/frtos/motor/motor_init.h b/frtos/motor/motor_init.h index 56cb390..7ac9b70 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -23,12 +23,17 @@ motor_t g_motor_left = { .pwm.level = 0u, .pwm.channel = PWM_CHAN_A, .speed.distance_cm = 0.0f }; +// classic ziegler nichols method +// Ku = 1000, Tu = 0.9s, interval = 0.05s +// Kp = 0.6 * Ku = 600 +// Ki = 2 * Kp * 0.05 / Tu = 66.67 +// Kd = Kp * Tu * 0.05 / 8 = 1350 motor_t g_motor_right = { .pwm.level = 0u, .pwm.channel = PWM_CHAN_B, .speed.distance_cm = 0.0f, - .pid.kp_value = 1000.f, - .pid.ki_value = 0.0f, - .pid.kd_value = 10000.0f,}; + .pid.kp_value = 600.f, + .pid.ki_value = 66.67f, + .pid.kd_value = 1350.f,}; void motor_init(void) @@ -62,11 +67,12 @@ motor_init(void) // NOTE: PWM clock is 125MHz for raspberrypi pico w by default - // 125MHz / 250 = 500kHz + // 125MHz / 50 = 2500kHz pwm_set_clkdiv(g_motor_left.pwm.slice_num, PWM_CLK_DIV); pwm_set_clkdiv(g_motor_right.pwm.slice_num, PWM_CLK_DIV); - // have them to be 500kHz / 5000 = 100Hz + // L289N can accept up to 40kHz + // 2500kHz / 100 = 25kHz pwm_set_wrap(g_motor_left.pwm.slice_num, (PWM_WRAP - 1U)); pwm_set_wrap(g_motor_right.pwm.slice_num, (PWM_WRAP - 1U)); diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index 16283f1..1930ed6 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -21,7 +21,7 @@ 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); + printf("%f\n", error); *integral += error; @@ -42,7 +42,7 @@ repeating_pid_handler(__unused struct repeating_timer *t) static float integral = 0.0f; static float prev_error = 0.0f; - if ((g_motor_left.pwm.level == 0u) || (g_motor_right.pwm.level == 0u)) + if (!g_use_pid) { return true; } @@ -51,14 +51,14 @@ repeating_pid_handler(__unused struct repeating_timer *t) float temp = (float)g_motor_right.pwm.level + control_signal * 0.05f; - if (temp > MAX_SPEED) + if (temp > MAX_PWM_LEVEL) { - temp = MAX_SPEED; + temp = MAX_PWM_LEVEL; } - if (temp < MIN_SPEED) + if (temp <= MIN_PWM_LEVEL) { - temp = MIN_SPEED + 1u; + temp = MIN_PWM_LEVEL + 1u; } g_motor_right.pwm.level = (uint16_t)temp; @@ -66,8 +66,8 @@ repeating_pid_handler(__unused struct repeating_timer *t) 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); +// 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 4848f83..53891be 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -86,11 +86,16 @@ set_wheel_speed(uint32_t pwm_level, motor_t * motor) /*! * @brief Set the speed of the wheels - * @param pwm_level The pwm_level of the wheels, from 0 to 5000 + * @param pwm_level The pwm_level of the wheels, from 0 to 99 */ void set_wheel_speed_synced(uint32_t pwm_level) { + if (pwm_level > MAX_PWM_LEVEL) + { + pwm_level = MAX_PWM_LEVEL; + } + set_wheel_speed(pwm_level, &g_motor_left); set_wheel_speed(pwm_level, &g_motor_right); } diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index b7dbe1f..8899ebb 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -4,6 +4,22 @@ #include "motor_direction.h" #include "motor_pid.h" +void +motor_control_task(__unused void *params) +{ + for (;;) + { + set_wheel_direction(DIRECTION_FORWARD); + set_wheel_speed_synced(90); + + vTaskDelay(pdMS_TO_TICKS(10000)); + + revert_wheel_direction(); + set_wheel_speed_synced(90); + + vTaskDelay(pdMS_TO_TICKS(10000)); + } +} void launch() From b02919bf1b83d68327623236ebc36bcda675b239 Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Tue, 7 Nov 2023 09:35:15 +0800 Subject: [PATCH 09/23] add turn to yaw function (pending test) --- frtos/motor/motor_direction.h | 43 +++++++++++++++++++++++++++++++---- frtos/motor/motor_speed.h | 4 ++-- 2 files changed, 41 insertions(+), 6 deletions(-) diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index a5ad873..6c31e29 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -5,6 +5,7 @@ */ #include "motor_init.h" +#include "magnetometer_init.h" /*! * @brief Set the direction of the wheels; can use bitwise OR to set both @@ -41,7 +42,7 @@ void turn_left_90() { set_wheel_direction(DIRECTION_RIGHT_FORWARD); - set_wheel_speed(3500u, &g_motor_right); + set_wheel_speed(90u, &g_motor_right); float initial = g_motor_right.speed.distance_cm; for (;;) @@ -62,7 +63,7 @@ void turn_right_90() { set_wheel_direction(DIRECTION_LEFT_FORWARD); - set_wheel_speed(3500u, &g_motor_left); + set_wheel_speed(90u, &g_motor_left); float initial = g_motor_left.speed.distance_cm; for (;;) @@ -84,7 +85,7 @@ spin_left_90() { set_wheel_direction(DIRECTION_LEFT); - set_wheel_speed_synced(3500u); + set_wheel_speed_synced(90u); float initial = g_motor_left.speed.distance_cm; for (;;) @@ -106,7 +107,7 @@ spin_right_90() { set_wheel_direction(DIRECTION_RIGHT); - set_wheel_speed_synced(3500u); + set_wheel_speed_synced(90u); float initial = g_motor_right.speed.distance_cm; for (;;) @@ -121,4 +122,38 @@ spin_right_90() vTaskDelay(pdMS_TO_TICKS(1000)); g_motor_right.speed.distance_cm = initial; g_motor_left.speed.distance_cm = initial; +} + +void +spin_to_yaw(float target_yaw) +{ + float initial_yaw = g_direction.yaw; + + // if it will to turn more than 180 degrees, turn the other way + if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw < 180.f) + || ((target_yaw < initial_yaw) && (initial_yaw - target_yaw >= 180.f))) + { + set_wheel_direction(DIRECTION_LEFT); + } + else if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw >= 180.f) + || ((target_yaw < initial_yaw) + && (initial_yaw - target_yaw < 180.f))) + { + set_wheel_direction(DIRECTION_RIGHT); + } + + set_wheel_speed_synced(90u); + + g_use_pid = false; + + for (;;) + { + if (initial_yaw == target_yaw) + { + set_wheel_speed_synced(0u); + break; + } + vTaskDelay(pdMS_TO_TICKS(5)); + } + g_use_pid = true; } \ No newline at end of file diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index 53891be..9b556ec 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -51,8 +51,8 @@ monitor_wheel_speed_task(void *pvParameters) for (;;) { - if (xSemaphoreTake(p_motor->sem, pdMS_TO_TICKS(100)) - == pdTRUE) + if ((xSemaphoreTake(p_motor->sem, pdMS_TO_TICKS(100)) + == pdTRUE) && (g_use_pid == true)) { curr_time = time_us_64(); elapsed_time = curr_time - prev_time; From d0553b6ddba9470e912bbde216e029e7cceea9ef Mon Sep 17 00:00:00 2001 From: Devoalda Date: Tue, 7 Nov 2023 10:02:09 +0800 Subject: [PATCH 10/23] fix(Magnetometer): Added filter for magnetometer outliers --- frtos/config/magnetometer_config.h | 9 ++++-- frtos/magnetometer/magnetometer_direction.h | 23 ++++++++------- frtos/magnetometer/magnetometer_read.h | 31 +++++++++++++-------- 3 files changed, 38 insertions(+), 25 deletions(-) diff --git a/frtos/config/magnetometer_config.h b/frtos/config/magnetometer_config.h index 5acb030..1128376 100644 --- a/frtos/config/magnetometer_config.h +++ b/frtos/config/magnetometer_config.h @@ -7,8 +7,13 @@ #define DIRECTION_READ_DELAY ( 100 ) -#define ALPHA ( 0.0f ) // Complementary - // Filter Constant +#define NUM_READINGS ( 10 ) // Number of readings to + // take before + // calculating + // direction + +//#define ALPHA ( 0.1f ) // Low Pass Filter + // Coefficient // LSM303DLHC temperature compensation coefficients #define SCALE_Z ( 1.0f ) // Scale for Z-axis diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h index b8addee..2cabc97 100644 --- a/frtos/magnetometer/magnetometer_direction.h +++ b/frtos/magnetometer/magnetometer_direction.h @@ -65,10 +65,10 @@ calculate_yaw_magnetometer(int16_t magnetometer[3]) { * @param yaw_mag Yaw calculated from Magnetometer Data * @return yaw Yaw calculated from Complementary Filter */ -static inline float -calculate_yaw_complementary(float yaw_acc, float yaw_mag) { - return ALPHA * yaw_acc + (1 - ALPHA) * yaw_mag; -} +//static inline float +//calculate_yaw_complementary(float yaw_acc, float yaw_mag) { +// return ALPHA * yaw_acc + (1 - ALPHA) * yaw_mag; +//} /** * @brief Compensate the magnetometer readings for temperature @@ -220,9 +220,7 @@ update_orientation_data(float roll, float pitch, float yaw, * @param magnetometer Magnetometer Data */ static void -read_direction(int16_t acceleration[3], - int16_t magnetometer[3], - int16_t temperature[1]) { +read_direction(int16_t acceleration[3], int16_t magnetometer[3]) { float roll = calculate_roll(acceleration); float pitch = calculate_pitch(acceleration); @@ -260,7 +258,8 @@ read_direction(int16_t acceleration[3], * @param params */ void print_orientation_data() { - printf("Roll: %f, Pitch: %f, Yaw: %f\n", +// printf("Roll: %f, Pitch: %f, Yaw: %f\n", + printf("%f %f %f\n", g_direction.roll, g_direction.pitch, g_direction.yaw @@ -331,16 +330,16 @@ void updateDirection() { read_accelerometer(accelerometer); read_temperature(temperature); - read_direction(accelerometer, magnetometer, temperature); + read_direction(accelerometer, magnetometer); // Temperature in degrees Celsius - printf("Temperature: %d\n", temperature[0]); +// printf("Temperature: %d\n", temperature[0]); print_orientation_data(); - printf("Direction: "); +// printf("Direction: "); - print_direction(g_direction.orientation); +// print_direction(g_direction.orientation); switch (g_direction.orientation) { diff --git a/frtos/magnetometer/magnetometer_read.h b/frtos/magnetometer/magnetometer_read.h index fde4b0b..36f3f35 100644 --- a/frtos/magnetometer/magnetometer_read.h +++ b/frtos/magnetometer/magnetometer_read.h @@ -58,25 +58,34 @@ read_accelerometer(int16_t accelerometer[3]) { } /** - * @brief Read Magnetometer Data + * @brief Read Magnetometer Data with Moving Average * @param magnetometer Magnetometer Data */ static inline void read_magnetometer(int16_t magnetometer[3]) { uint8_t buffer[6]; + int32_t xMagFiltered = 0; + int32_t yMagFiltered = 0; + int32_t zMagFiltered = 0; - buffer[0] = read_data(MAG_ADDR, LSM303_OUT_X_H_M); - buffer[1] = read_data(MAG_ADDR, LSM303_OUT_X_L_M); - buffer[2] = read_data(MAG_ADDR, LSM303_OUT_Y_H_M); - buffer[3] = read_data(MAG_ADDR, LSM303_OUT_Y_L_M); - buffer[4] = read_data(MAG_ADDR, LSM303_OUT_Z_H_M); - buffer[5] = read_data(MAG_ADDR, LSM303_OUT_Z_L_M); + for (int i = 0; i < NUM_READINGS; i++) { + buffer[0] = read_data(MAG_ADDR, LSM303_OUT_X_H_M); + buffer[1] = read_data(MAG_ADDR, LSM303_OUT_X_L_M); + buffer[2] = read_data(MAG_ADDR, LSM303_OUT_Y_H_M); + buffer[3] = read_data(MAG_ADDR, LSM303_OUT_Y_L_M); + buffer[4] = read_data(MAG_ADDR, LSM303_OUT_Z_H_M); + buffer[5] = read_data(MAG_ADDR, LSM303_OUT_Z_L_M); - magnetometer[0] = (int16_t) (buffer[0] << 8 | buffer[1]); //xMag + // Update the cumulative sum of the magnetometer data + xMagFiltered += (int16_t)(buffer[0] << 8 | buffer[1]); + yMagFiltered += (int16_t)(buffer[2] << 8 | buffer[3]); + zMagFiltered += (int16_t)(buffer[4] << 8 | buffer[5]); + } - magnetometer[1] = (int16_t) (buffer[2] << 8 | buffer[3]); //yMag - - magnetometer[2] = (int16_t) (buffer[4] << 8 | buffer[5]); //zMag + // Calculate the moving average + magnetometer[0] = xMagFiltered / NUM_READINGS; + magnetometer[1] = yMagFiltered / NUM_READINGS; + magnetometer[2] = zMagFiltered / NUM_READINGS; } /** From 678aa00eccdb754fe95953e7a7d578f20452bb6d Mon Sep 17 00:00:00 2001 From: soapysoltionss <2200754@sit.singaporetech.edu.sg> Date: Tue, 7 Nov 2023 12:48:19 +0800 Subject: [PATCH 11/23] update methods and added bar width detection --- frtos/barcode_sensor/CMakeLists.txt | 19 ++++ frtos/barcode_sensor/barcode_sensor.h | 124 +++++++++++++++++++++ frtos/barcode_sensor/barcode_sensor_init.h | 97 ++++++++++++++++ frtos/barcode_sensor/barcode_sensor_test.c | 48 ++++++++ 4 files changed, 288 insertions(+) create mode 100644 frtos/barcode_sensor/CMakeLists.txt create mode 100644 frtos/barcode_sensor/barcode_sensor.h create mode 100644 frtos/barcode_sensor/barcode_sensor_init.h create mode 100644 frtos/barcode_sensor/barcode_sensor_test.c diff --git a/frtos/barcode_sensor/CMakeLists.txt b/frtos/barcode_sensor/CMakeLists.txt new file mode 100644 index 0000000..e61432b --- /dev/null +++ b/frtos/barcode_sensor/CMakeLists.txt @@ -0,0 +1,19 @@ +add_executable( + barcode_sensor_test + barcode_sensor_test.c +) + +target_link_libraries( + barcode_sensor_test + hardware_adc + pico_stdlib + FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap +) + +target_include_directories(barcode_sensor_test + PRIVATE ../config + ../line_sensor +) + +pico_enable_stdio_usb(barcode_sensor_test 1) +pico_add_extra_outputs(barcode_sensor_test) \ No newline at end of file diff --git a/frtos/barcode_sensor/barcode_sensor.h b/frtos/barcode_sensor/barcode_sensor.h new file mode 100644 index 0000000..1a95a67 --- /dev/null +++ b/frtos/barcode_sensor/barcode_sensor.h @@ -0,0 +1,124 @@ +/* Barcode sensor */ + +#include "barcode_sensor_init.h" + +#define MAX_BARCODES 10 // Define the maximum number of barcodes to store +#define BARCODE_SENSOR_TIMER_PERIOD_MS 100 // Define the barcode sensor timer period + +// Define the barcode sensor timer +static struct repeating_timer barcode_sensor_timer; + + +/** + * @brief Decode a Code 39 barcode + * + * This function decodes a Code 39 barcode represented as a 9-bit binary number. + * + * @param barcode_data Binary representation of the barcode data (9 bits) + * @return Decoded value as an integer + */ +int code39_decode(uint32_t barcode_data) { + // Define the binary representations of Code 39 characters + const uint32_t code39_characters[] = { + 0b001001001, // 0 + 0b001001011, // 1 + 0b001011001, // 2 + 0b001011011, // 3 + 0b001100011, // 4 + 0b001101001, // 5 + 0b001101011, // 6 + 0b001010011, // 7 + 0b001011101, // 8 + 0b001111001, // 9 + // Add more character representations as needed + }; + + // Compare the barcode data to known Code 39 character representations + for (int i = 0; i < 10; i++) { + if (barcode_data == code39_characters[i]) { + return i; // Return the decoded value (0-9) + } + } + + // If the barcode data does not match any known character, return -1 to indicate an error + return -1; +} + + +/** + * @brief Monitor the barcode sensor + * + * This function will monitor the barcode sensor and send the state to the + * barcode sensor message buffer, including Code 39 decoding. + * + * @param params + */ +void monitor_barcode_sensor_task(void *params) { + // Create the barcode sensor timer + add_repeating_timer_ms(BARCODE_SENSOR_TIMER_PERIOD_MS, h_barcode_sensor_timer_handler, NULL, &barcode_sensor_timer); + + for (;;) { + if (xSemaphoreTake(g_barcode_sensor_sem, portMAX_DELAY) == pdTRUE) { + // Check the flag or receive the message + if (barcode_sensor_triggered == pdTRUE) { + uint32_t barcode_data = 0; + int bar_width = 0; // Variable to store the width of the current bar + + for (int i = 0; i < 9; i++) { + sleep_ms(100); // Wait for a segment of the barcode + + // Measure bar width using the IR sensor + if (gpio_get(BARCODE_SENSOR_PIN)) { + bar_width++; + } else { + // Bar ended, process the width + if (bar_width > 0) { + printf("Bar Width: %d\n", bar_width); + // Process or store the bar width as needed + bar_width = 0; // Reset the bar width measurement + } + + barcode_data |= (1u << i); + } + } + + printf("Barcode Data (binary): %09b\n", barcode_data); + + // Decode the barcode data + int decoded_value = code39_decode(barcode_data); + + if (decoded_value != -1) { + printf("Decoded Value: %d\n", decoded_value); + // Store or process the decoded value as needed + + // Send the decoded value instead of the raw barcode data + xMessageBufferSend(barcode_sensor_msg_buffer, &decoded_value, sizeof(int), 0); + } else { + printf("Error: Unable to decode the barcode.\n"); + } + + // Reset the flag + barcode_sensor_triggered = pdFALSE; + } + } + } +} + + + + +/** + * @brief Monitor the barcode sensor + * @param params + */ +void +monitor_barcode_task(__unused void *params) { + state_t barcode_state; + + // Receive from Buffer + xMessageBufferReceive(barcode_sensor_msg_buffer, + &barcode_state, + sizeof(state_t), + portMAX_DELAY); + +} \ No newline at end of file diff --git a/frtos/barcode_sensor/barcode_sensor_init.h b/frtos/barcode_sensor/barcode_sensor_init.h new file mode 100644 index 0000000..e33041e --- /dev/null +++ b/frtos/barcode_sensor/barcode_sensor_init.h @@ -0,0 +1,97 @@ +/* Initialise the barcode sensor */ + +#ifndef BARCODE_SENSOR_INIT_H +#define BARCODE_SENSOR_INIT_H + +#include +#include "pico/stdlib.h" + +#include "hardware/adc.h" + +#include "FreeRTOS.h" +#include "task.h" +#include "message_buffer.h" +#include "semphr.h" + +#include "barcode_sensor_config.h" +#include "line_sensor_init.h" + +// Set barcode time to 0 +static TickType_t lastBarcodeTime = 0; + + +// Semaphore +SemaphoreHandle_t g_barcode_sensor_sem = NULL; + + +// Queue +static MessageBufferHandle_t barcode_sensor_msg_buffer; // Barcode Sensor Buffer + +// Flag +static volatile BaseType_t barcode_sensor_triggered = pdFALSE; + + +/** + * @brief Setup the Line Sensor + * + * This function will setup the Line Sensor by initializing it as an input + */ +static inline void +barcode_sensor_setup() { + g_barcode_sensor_sem = xSemaphoreCreateBinary(); + + + uint mask = (1 << BARCODE_SENSOR_PIN); + + // Initialise 3 GPIO pins and set them to input + gpio_init_mask(mask); + gpio_set_dir_in_masked(mask); + + barcode_sensor_msg_buffer = xMessageBufferCreate(30); + +} + +/** + * @brief Timer Interrupt Handler for the barcode sensor + * + * @param repeatingTimer + * @return True (To keep the timer running) + */ +bool h_barcode_sensor_timer_handler(repeating_timer_t *repeatingTimer) { + + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(g_barcode_sensor_sem, + &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + + return true; +} + +void h_barcode_sensor_handler(void) { + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + TickType_t currentTicks = xTaskGetTickCount(); + printf("Interrupt triggered\n"); + + if (gpio_get_irq_event_mask(BARCODE_SENSOR_PIN) & GPIO_IRQ_EDGE_FALL) + { + if ((currentTicks - lastBarcodeTime) >= + pdMS_TO_TICKS(DEBOUNCE_DELAY_MS)) + { + lastBarcodeTime = currentTicks; + gpio_acknowledge_irq(BARCODE_SENSOR_PIN, GPIO_IRQ_EDGE_FALL); + // Set the flag to notify the task + barcode_sensor_triggered = pdTRUE; + xSemaphoreGiveFromISR(g_barcode_sensor_sem, + &xHigherPriorityTaskWoken); + } + else + { + // Reset the timer to the currentTicks if the edge is ignored + lastBarcodeTime = currentTicks; + } + } + + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); +} + +#endif /* LINE_SENSOR_INIT_H */ diff --git a/frtos/barcode_sensor/barcode_sensor_test.c b/frtos/barcode_sensor/barcode_sensor_test.c new file mode 100644 index 0000000..1aa45b4 --- /dev/null +++ b/frtos/barcode_sensor/barcode_sensor_test.c @@ -0,0 +1,48 @@ +#include "barcode_sensor.h" + +#define READ_BARCODE_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) + +void +launch() +{ + // isr to detect left line sensor + gpio_set_irq_enabled(BARCODE_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true); + gpio_add_raw_irq_handler(BARCODE_SENSOR_PIN, h_barcode_sensor_handler); + + + irq_set_enabled(IO_IRQ_BANK0, true); + + + 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); + + + 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); + + vTaskStartScheduler(); +} + +int +main (void) +{ + stdio_usb_init(); + + sleep_ms(10000); + printf("Test started!\n"); + + barcode_sensor_setup(); + initialize_car_state(); + + launch(); + + return (0); +} \ No newline at end of file From d840b2710d44ec5d6cbbe3ffef3ba248f0f339d8 Mon Sep 17 00:00:00 2001 From: soapysoltionss <2200754@sit.singaporetech.edu.sg> Date: Tue, 7 Nov 2023 13:05:12 +0800 Subject: [PATCH 12/23] update barcode --- barcode_sensor/CMakeLists.txt | 19 ++++ barcode_sensor/barcode_sensor.h | 124 +++++++++++++++++++++++++++ barcode_sensor/barcode_sensor_init.h | 97 +++++++++++++++++++++ barcode_sensor/barcode_sensor_test.c | 48 +++++++++++ 4 files changed, 288 insertions(+) create mode 100644 barcode_sensor/CMakeLists.txt create mode 100644 barcode_sensor/barcode_sensor.h create mode 100644 barcode_sensor/barcode_sensor_init.h create mode 100644 barcode_sensor/barcode_sensor_test.c diff --git a/barcode_sensor/CMakeLists.txt b/barcode_sensor/CMakeLists.txt new file mode 100644 index 0000000..e61432b --- /dev/null +++ b/barcode_sensor/CMakeLists.txt @@ -0,0 +1,19 @@ +add_executable( + barcode_sensor_test + barcode_sensor_test.c +) + +target_link_libraries( + barcode_sensor_test + hardware_adc + pico_stdlib + FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap +) + +target_include_directories(barcode_sensor_test + PRIVATE ../config + ../line_sensor +) + +pico_enable_stdio_usb(barcode_sensor_test 1) +pico_add_extra_outputs(barcode_sensor_test) \ No newline at end of file diff --git a/barcode_sensor/barcode_sensor.h b/barcode_sensor/barcode_sensor.h new file mode 100644 index 0000000..1a95a67 --- /dev/null +++ b/barcode_sensor/barcode_sensor.h @@ -0,0 +1,124 @@ +/* Barcode sensor */ + +#include "barcode_sensor_init.h" + +#define MAX_BARCODES 10 // Define the maximum number of barcodes to store +#define BARCODE_SENSOR_TIMER_PERIOD_MS 100 // Define the barcode sensor timer period + +// Define the barcode sensor timer +static struct repeating_timer barcode_sensor_timer; + + +/** + * @brief Decode a Code 39 barcode + * + * This function decodes a Code 39 barcode represented as a 9-bit binary number. + * + * @param barcode_data Binary representation of the barcode data (9 bits) + * @return Decoded value as an integer + */ +int code39_decode(uint32_t barcode_data) { + // Define the binary representations of Code 39 characters + const uint32_t code39_characters[] = { + 0b001001001, // 0 + 0b001001011, // 1 + 0b001011001, // 2 + 0b001011011, // 3 + 0b001100011, // 4 + 0b001101001, // 5 + 0b001101011, // 6 + 0b001010011, // 7 + 0b001011101, // 8 + 0b001111001, // 9 + // Add more character representations as needed + }; + + // Compare the barcode data to known Code 39 character representations + for (int i = 0; i < 10; i++) { + if (barcode_data == code39_characters[i]) { + return i; // Return the decoded value (0-9) + } + } + + // If the barcode data does not match any known character, return -1 to indicate an error + return -1; +} + + +/** + * @brief Monitor the barcode sensor + * + * This function will monitor the barcode sensor and send the state to the + * barcode sensor message buffer, including Code 39 decoding. + * + * @param params + */ +void monitor_barcode_sensor_task(void *params) { + // Create the barcode sensor timer + add_repeating_timer_ms(BARCODE_SENSOR_TIMER_PERIOD_MS, h_barcode_sensor_timer_handler, NULL, &barcode_sensor_timer); + + for (;;) { + if (xSemaphoreTake(g_barcode_sensor_sem, portMAX_DELAY) == pdTRUE) { + // Check the flag or receive the message + if (barcode_sensor_triggered == pdTRUE) { + uint32_t barcode_data = 0; + int bar_width = 0; // Variable to store the width of the current bar + + for (int i = 0; i < 9; i++) { + sleep_ms(100); // Wait for a segment of the barcode + + // Measure bar width using the IR sensor + if (gpio_get(BARCODE_SENSOR_PIN)) { + bar_width++; + } else { + // Bar ended, process the width + if (bar_width > 0) { + printf("Bar Width: %d\n", bar_width); + // Process or store the bar width as needed + bar_width = 0; // Reset the bar width measurement + } + + barcode_data |= (1u << i); + } + } + + printf("Barcode Data (binary): %09b\n", barcode_data); + + // Decode the barcode data + int decoded_value = code39_decode(barcode_data); + + if (decoded_value != -1) { + printf("Decoded Value: %d\n", decoded_value); + // Store or process the decoded value as needed + + // Send the decoded value instead of the raw barcode data + xMessageBufferSend(barcode_sensor_msg_buffer, &decoded_value, sizeof(int), 0); + } else { + printf("Error: Unable to decode the barcode.\n"); + } + + // Reset the flag + barcode_sensor_triggered = pdFALSE; + } + } + } +} + + + + +/** + * @brief Monitor the barcode sensor + * @param params + */ +void +monitor_barcode_task(__unused void *params) { + state_t barcode_state; + + // Receive from Buffer + xMessageBufferReceive(barcode_sensor_msg_buffer, + &barcode_state, + sizeof(state_t), + portMAX_DELAY); + +} \ No newline at end of file diff --git a/barcode_sensor/barcode_sensor_init.h b/barcode_sensor/barcode_sensor_init.h new file mode 100644 index 0000000..e33041e --- /dev/null +++ b/barcode_sensor/barcode_sensor_init.h @@ -0,0 +1,97 @@ +/* Initialise the barcode sensor */ + +#ifndef BARCODE_SENSOR_INIT_H +#define BARCODE_SENSOR_INIT_H + +#include +#include "pico/stdlib.h" + +#include "hardware/adc.h" + +#include "FreeRTOS.h" +#include "task.h" +#include "message_buffer.h" +#include "semphr.h" + +#include "barcode_sensor_config.h" +#include "line_sensor_init.h" + +// Set barcode time to 0 +static TickType_t lastBarcodeTime = 0; + + +// Semaphore +SemaphoreHandle_t g_barcode_sensor_sem = NULL; + + +// Queue +static MessageBufferHandle_t barcode_sensor_msg_buffer; // Barcode Sensor Buffer + +// Flag +static volatile BaseType_t barcode_sensor_triggered = pdFALSE; + + +/** + * @brief Setup the Line Sensor + * + * This function will setup the Line Sensor by initializing it as an input + */ +static inline void +barcode_sensor_setup() { + g_barcode_sensor_sem = xSemaphoreCreateBinary(); + + + uint mask = (1 << BARCODE_SENSOR_PIN); + + // Initialise 3 GPIO pins and set them to input + gpio_init_mask(mask); + gpio_set_dir_in_masked(mask); + + barcode_sensor_msg_buffer = xMessageBufferCreate(30); + +} + +/** + * @brief Timer Interrupt Handler for the barcode sensor + * + * @param repeatingTimer + * @return True (To keep the timer running) + */ +bool h_barcode_sensor_timer_handler(repeating_timer_t *repeatingTimer) { + + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(g_barcode_sensor_sem, + &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + + return true; +} + +void h_barcode_sensor_handler(void) { + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + TickType_t currentTicks = xTaskGetTickCount(); + printf("Interrupt triggered\n"); + + if (gpio_get_irq_event_mask(BARCODE_SENSOR_PIN) & GPIO_IRQ_EDGE_FALL) + { + if ((currentTicks - lastBarcodeTime) >= + pdMS_TO_TICKS(DEBOUNCE_DELAY_MS)) + { + lastBarcodeTime = currentTicks; + gpio_acknowledge_irq(BARCODE_SENSOR_PIN, GPIO_IRQ_EDGE_FALL); + // Set the flag to notify the task + barcode_sensor_triggered = pdTRUE; + xSemaphoreGiveFromISR(g_barcode_sensor_sem, + &xHigherPriorityTaskWoken); + } + else + { + // Reset the timer to the currentTicks if the edge is ignored + lastBarcodeTime = currentTicks; + } + } + + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); +} + +#endif /* LINE_SENSOR_INIT_H */ diff --git a/barcode_sensor/barcode_sensor_test.c b/barcode_sensor/barcode_sensor_test.c new file mode 100644 index 0000000..1aa45b4 --- /dev/null +++ b/barcode_sensor/barcode_sensor_test.c @@ -0,0 +1,48 @@ +#include "barcode_sensor.h" + +#define READ_BARCODE_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) + +void +launch() +{ + // isr to detect left line sensor + gpio_set_irq_enabled(BARCODE_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true); + gpio_add_raw_irq_handler(BARCODE_SENSOR_PIN, h_barcode_sensor_handler); + + + irq_set_enabled(IO_IRQ_BANK0, true); + + + 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); + + + 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); + + vTaskStartScheduler(); +} + +int +main (void) +{ + stdio_usb_init(); + + sleep_ms(10000); + printf("Test started!\n"); + + barcode_sensor_setup(); + initialize_car_state(); + + launch(); + + return (0); +} \ No newline at end of file From 6e65fac70816c1ee97e5dc34bdaf321a81a9c670 Mon Sep 17 00:00:00 2001 From: soapysoltionss <2200754@sit.singaporetech.edu.sg> Date: Tue, 7 Nov 2023 13:11:21 +0800 Subject: [PATCH 13/23] barcode --- barcode_sensor/CMakeLists.txt | 19 ++++ barcode_sensor/barcode_sensor.h | 124 +++++++++++++++++++++++++++ barcode_sensor/barcode_sensor_init.h | 97 +++++++++++++++++++++ barcode_sensor/barcode_sensor_test.c | 48 +++++++++++ 4 files changed, 288 insertions(+) create mode 100644 barcode_sensor/CMakeLists.txt create mode 100644 barcode_sensor/barcode_sensor.h create mode 100644 barcode_sensor/barcode_sensor_init.h create mode 100644 barcode_sensor/barcode_sensor_test.c diff --git a/barcode_sensor/CMakeLists.txt b/barcode_sensor/CMakeLists.txt new file mode 100644 index 0000000..e61432b --- /dev/null +++ b/barcode_sensor/CMakeLists.txt @@ -0,0 +1,19 @@ +add_executable( + barcode_sensor_test + barcode_sensor_test.c +) + +target_link_libraries( + barcode_sensor_test + hardware_adc + pico_stdlib + FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap +) + +target_include_directories(barcode_sensor_test + PRIVATE ../config + ../line_sensor +) + +pico_enable_stdio_usb(barcode_sensor_test 1) +pico_add_extra_outputs(barcode_sensor_test) \ No newline at end of file diff --git a/barcode_sensor/barcode_sensor.h b/barcode_sensor/barcode_sensor.h new file mode 100644 index 0000000..1a95a67 --- /dev/null +++ b/barcode_sensor/barcode_sensor.h @@ -0,0 +1,124 @@ +/* Barcode sensor */ + +#include "barcode_sensor_init.h" + +#define MAX_BARCODES 10 // Define the maximum number of barcodes to store +#define BARCODE_SENSOR_TIMER_PERIOD_MS 100 // Define the barcode sensor timer period + +// Define the barcode sensor timer +static struct repeating_timer barcode_sensor_timer; + + +/** + * @brief Decode a Code 39 barcode + * + * This function decodes a Code 39 barcode represented as a 9-bit binary number. + * + * @param barcode_data Binary representation of the barcode data (9 bits) + * @return Decoded value as an integer + */ +int code39_decode(uint32_t barcode_data) { + // Define the binary representations of Code 39 characters + const uint32_t code39_characters[] = { + 0b001001001, // 0 + 0b001001011, // 1 + 0b001011001, // 2 + 0b001011011, // 3 + 0b001100011, // 4 + 0b001101001, // 5 + 0b001101011, // 6 + 0b001010011, // 7 + 0b001011101, // 8 + 0b001111001, // 9 + // Add more character representations as needed + }; + + // Compare the barcode data to known Code 39 character representations + for (int i = 0; i < 10; i++) { + if (barcode_data == code39_characters[i]) { + return i; // Return the decoded value (0-9) + } + } + + // If the barcode data does not match any known character, return -1 to indicate an error + return -1; +} + + +/** + * @brief Monitor the barcode sensor + * + * This function will monitor the barcode sensor and send the state to the + * barcode sensor message buffer, including Code 39 decoding. + * + * @param params + */ +void monitor_barcode_sensor_task(void *params) { + // Create the barcode sensor timer + add_repeating_timer_ms(BARCODE_SENSOR_TIMER_PERIOD_MS, h_barcode_sensor_timer_handler, NULL, &barcode_sensor_timer); + + for (;;) { + if (xSemaphoreTake(g_barcode_sensor_sem, portMAX_DELAY) == pdTRUE) { + // Check the flag or receive the message + if (barcode_sensor_triggered == pdTRUE) { + uint32_t barcode_data = 0; + int bar_width = 0; // Variable to store the width of the current bar + + for (int i = 0; i < 9; i++) { + sleep_ms(100); // Wait for a segment of the barcode + + // Measure bar width using the IR sensor + if (gpio_get(BARCODE_SENSOR_PIN)) { + bar_width++; + } else { + // Bar ended, process the width + if (bar_width > 0) { + printf("Bar Width: %d\n", bar_width); + // Process or store the bar width as needed + bar_width = 0; // Reset the bar width measurement + } + + barcode_data |= (1u << i); + } + } + + printf("Barcode Data (binary): %09b\n", barcode_data); + + // Decode the barcode data + int decoded_value = code39_decode(barcode_data); + + if (decoded_value != -1) { + printf("Decoded Value: %d\n", decoded_value); + // Store or process the decoded value as needed + + // Send the decoded value instead of the raw barcode data + xMessageBufferSend(barcode_sensor_msg_buffer, &decoded_value, sizeof(int), 0); + } else { + printf("Error: Unable to decode the barcode.\n"); + } + + // Reset the flag + barcode_sensor_triggered = pdFALSE; + } + } + } +} + + + + +/** + * @brief Monitor the barcode sensor + * @param params + */ +void +monitor_barcode_task(__unused void *params) { + state_t barcode_state; + + // Receive from Buffer + xMessageBufferReceive(barcode_sensor_msg_buffer, + &barcode_state, + sizeof(state_t), + portMAX_DELAY); + +} \ No newline at end of file diff --git a/barcode_sensor/barcode_sensor_init.h b/barcode_sensor/barcode_sensor_init.h new file mode 100644 index 0000000..e33041e --- /dev/null +++ b/barcode_sensor/barcode_sensor_init.h @@ -0,0 +1,97 @@ +/* Initialise the barcode sensor */ + +#ifndef BARCODE_SENSOR_INIT_H +#define BARCODE_SENSOR_INIT_H + +#include +#include "pico/stdlib.h" + +#include "hardware/adc.h" + +#include "FreeRTOS.h" +#include "task.h" +#include "message_buffer.h" +#include "semphr.h" + +#include "barcode_sensor_config.h" +#include "line_sensor_init.h" + +// Set barcode time to 0 +static TickType_t lastBarcodeTime = 0; + + +// Semaphore +SemaphoreHandle_t g_barcode_sensor_sem = NULL; + + +// Queue +static MessageBufferHandle_t barcode_sensor_msg_buffer; // Barcode Sensor Buffer + +// Flag +static volatile BaseType_t barcode_sensor_triggered = pdFALSE; + + +/** + * @brief Setup the Line Sensor + * + * This function will setup the Line Sensor by initializing it as an input + */ +static inline void +barcode_sensor_setup() { + g_barcode_sensor_sem = xSemaphoreCreateBinary(); + + + uint mask = (1 << BARCODE_SENSOR_PIN); + + // Initialise 3 GPIO pins and set them to input + gpio_init_mask(mask); + gpio_set_dir_in_masked(mask); + + barcode_sensor_msg_buffer = xMessageBufferCreate(30); + +} + +/** + * @brief Timer Interrupt Handler for the barcode sensor + * + * @param repeatingTimer + * @return True (To keep the timer running) + */ +bool h_barcode_sensor_timer_handler(repeating_timer_t *repeatingTimer) { + + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(g_barcode_sensor_sem, + &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + + return true; +} + +void h_barcode_sensor_handler(void) { + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + TickType_t currentTicks = xTaskGetTickCount(); + printf("Interrupt triggered\n"); + + if (gpio_get_irq_event_mask(BARCODE_SENSOR_PIN) & GPIO_IRQ_EDGE_FALL) + { + if ((currentTicks - lastBarcodeTime) >= + pdMS_TO_TICKS(DEBOUNCE_DELAY_MS)) + { + lastBarcodeTime = currentTicks; + gpio_acknowledge_irq(BARCODE_SENSOR_PIN, GPIO_IRQ_EDGE_FALL); + // Set the flag to notify the task + barcode_sensor_triggered = pdTRUE; + xSemaphoreGiveFromISR(g_barcode_sensor_sem, + &xHigherPriorityTaskWoken); + } + else + { + // Reset the timer to the currentTicks if the edge is ignored + lastBarcodeTime = currentTicks; + } + } + + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); +} + +#endif /* LINE_SENSOR_INIT_H */ diff --git a/barcode_sensor/barcode_sensor_test.c b/barcode_sensor/barcode_sensor_test.c new file mode 100644 index 0000000..1aa45b4 --- /dev/null +++ b/barcode_sensor/barcode_sensor_test.c @@ -0,0 +1,48 @@ +#include "barcode_sensor.h" + +#define READ_BARCODE_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) + +void +launch() +{ + // isr to detect left line sensor + gpio_set_irq_enabled(BARCODE_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true); + gpio_add_raw_irq_handler(BARCODE_SENSOR_PIN, h_barcode_sensor_handler); + + + irq_set_enabled(IO_IRQ_BANK0, true); + + + 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); + + + 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); + + vTaskStartScheduler(); +} + +int +main (void) +{ + stdio_usb_init(); + + sleep_ms(10000); + printf("Test started!\n"); + + barcode_sensor_setup(); + initialize_car_state(); + + launch(); + + return (0); +} \ No newline at end of file From 0dd80ab90f9cf696fa35999717983a4b6322ae56 Mon Sep 17 00:00:00 2001 From: soapysoltionss <2200754@sit.singaporetech.edu.sg> Date: Tue, 7 Nov 2023 13:13:10 +0800 Subject: [PATCH 14/23] edit --- barcode_sensor/CMakeLists.txt | 19 ---- barcode_sensor/barcode_sensor.h | 124 --------------------------- barcode_sensor/barcode_sensor_init.h | 97 --------------------- barcode_sensor/barcode_sensor_test.c | 48 ----------- 4 files changed, 288 deletions(-) delete mode 100644 barcode_sensor/CMakeLists.txt delete mode 100644 barcode_sensor/barcode_sensor.h delete mode 100644 barcode_sensor/barcode_sensor_init.h delete mode 100644 barcode_sensor/barcode_sensor_test.c diff --git a/barcode_sensor/CMakeLists.txt b/barcode_sensor/CMakeLists.txt deleted file mode 100644 index e61432b..0000000 --- a/barcode_sensor/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -add_executable( - barcode_sensor_test - barcode_sensor_test.c -) - -target_link_libraries( - barcode_sensor_test - hardware_adc - pico_stdlib - FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap -) - -target_include_directories(barcode_sensor_test - PRIVATE ../config - ../line_sensor -) - -pico_enable_stdio_usb(barcode_sensor_test 1) -pico_add_extra_outputs(barcode_sensor_test) \ No newline at end of file diff --git a/barcode_sensor/barcode_sensor.h b/barcode_sensor/barcode_sensor.h deleted file mode 100644 index 1a95a67..0000000 --- a/barcode_sensor/barcode_sensor.h +++ /dev/null @@ -1,124 +0,0 @@ -/* Barcode sensor */ - -#include "barcode_sensor_init.h" - -#define MAX_BARCODES 10 // Define the maximum number of barcodes to store -#define BARCODE_SENSOR_TIMER_PERIOD_MS 100 // Define the barcode sensor timer period - -// Define the barcode sensor timer -static struct repeating_timer barcode_sensor_timer; - - -/** - * @brief Decode a Code 39 barcode - * - * This function decodes a Code 39 barcode represented as a 9-bit binary number. - * - * @param barcode_data Binary representation of the barcode data (9 bits) - * @return Decoded value as an integer - */ -int code39_decode(uint32_t barcode_data) { - // Define the binary representations of Code 39 characters - const uint32_t code39_characters[] = { - 0b001001001, // 0 - 0b001001011, // 1 - 0b001011001, // 2 - 0b001011011, // 3 - 0b001100011, // 4 - 0b001101001, // 5 - 0b001101011, // 6 - 0b001010011, // 7 - 0b001011101, // 8 - 0b001111001, // 9 - // Add more character representations as needed - }; - - // Compare the barcode data to known Code 39 character representations - for (int i = 0; i < 10; i++) { - if (barcode_data == code39_characters[i]) { - return i; // Return the decoded value (0-9) - } - } - - // If the barcode data does not match any known character, return -1 to indicate an error - return -1; -} - - -/** - * @brief Monitor the barcode sensor - * - * This function will monitor the barcode sensor and send the state to the - * barcode sensor message buffer, including Code 39 decoding. - * - * @param params - */ -void monitor_barcode_sensor_task(void *params) { - // Create the barcode sensor timer - add_repeating_timer_ms(BARCODE_SENSOR_TIMER_PERIOD_MS, h_barcode_sensor_timer_handler, NULL, &barcode_sensor_timer); - - for (;;) { - if (xSemaphoreTake(g_barcode_sensor_sem, portMAX_DELAY) == pdTRUE) { - // Check the flag or receive the message - if (barcode_sensor_triggered == pdTRUE) { - uint32_t barcode_data = 0; - int bar_width = 0; // Variable to store the width of the current bar - - for (int i = 0; i < 9; i++) { - sleep_ms(100); // Wait for a segment of the barcode - - // Measure bar width using the IR sensor - if (gpio_get(BARCODE_SENSOR_PIN)) { - bar_width++; - } else { - // Bar ended, process the width - if (bar_width > 0) { - printf("Bar Width: %d\n", bar_width); - // Process or store the bar width as needed - bar_width = 0; // Reset the bar width measurement - } - - barcode_data |= (1u << i); - } - } - - printf("Barcode Data (binary): %09b\n", barcode_data); - - // Decode the barcode data - int decoded_value = code39_decode(barcode_data); - - if (decoded_value != -1) { - printf("Decoded Value: %d\n", decoded_value); - // Store or process the decoded value as needed - - // Send the decoded value instead of the raw barcode data - xMessageBufferSend(barcode_sensor_msg_buffer, &decoded_value, sizeof(int), 0); - } else { - printf("Error: Unable to decode the barcode.\n"); - } - - // Reset the flag - barcode_sensor_triggered = pdFALSE; - } - } - } -} - - - - -/** - * @brief Monitor the barcode sensor - * @param params - */ -void -monitor_barcode_task(__unused void *params) { - state_t barcode_state; - - // Receive from Buffer - xMessageBufferReceive(barcode_sensor_msg_buffer, - &barcode_state, - sizeof(state_t), - portMAX_DELAY); - -} \ No newline at end of file diff --git a/barcode_sensor/barcode_sensor_init.h b/barcode_sensor/barcode_sensor_init.h deleted file mode 100644 index e33041e..0000000 --- a/barcode_sensor/barcode_sensor_init.h +++ /dev/null @@ -1,97 +0,0 @@ -/* Initialise the barcode sensor */ - -#ifndef BARCODE_SENSOR_INIT_H -#define BARCODE_SENSOR_INIT_H - -#include -#include "pico/stdlib.h" - -#include "hardware/adc.h" - -#include "FreeRTOS.h" -#include "task.h" -#include "message_buffer.h" -#include "semphr.h" - -#include "barcode_sensor_config.h" -#include "line_sensor_init.h" - -// Set barcode time to 0 -static TickType_t lastBarcodeTime = 0; - - -// Semaphore -SemaphoreHandle_t g_barcode_sensor_sem = NULL; - - -// Queue -static MessageBufferHandle_t barcode_sensor_msg_buffer; // Barcode Sensor Buffer - -// Flag -static volatile BaseType_t barcode_sensor_triggered = pdFALSE; - - -/** - * @brief Setup the Line Sensor - * - * This function will setup the Line Sensor by initializing it as an input - */ -static inline void -barcode_sensor_setup() { - g_barcode_sensor_sem = xSemaphoreCreateBinary(); - - - uint mask = (1 << BARCODE_SENSOR_PIN); - - // Initialise 3 GPIO pins and set them to input - gpio_init_mask(mask); - gpio_set_dir_in_masked(mask); - - barcode_sensor_msg_buffer = xMessageBufferCreate(30); - -} - -/** - * @brief Timer Interrupt Handler for the barcode sensor - * - * @param repeatingTimer - * @return True (To keep the timer running) - */ -bool h_barcode_sensor_timer_handler(repeating_timer_t *repeatingTimer) { - - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(g_barcode_sensor_sem, - &xHigherPriorityTaskWoken); - portYIELD_FROM_ISR(xHigherPriorityTaskWoken); - - return true; -} - -void h_barcode_sensor_handler(void) { - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - TickType_t currentTicks = xTaskGetTickCount(); - printf("Interrupt triggered\n"); - - if (gpio_get_irq_event_mask(BARCODE_SENSOR_PIN) & GPIO_IRQ_EDGE_FALL) - { - if ((currentTicks - lastBarcodeTime) >= - pdMS_TO_TICKS(DEBOUNCE_DELAY_MS)) - { - lastBarcodeTime = currentTicks; - gpio_acknowledge_irq(BARCODE_SENSOR_PIN, GPIO_IRQ_EDGE_FALL); - // Set the flag to notify the task - barcode_sensor_triggered = pdTRUE; - xSemaphoreGiveFromISR(g_barcode_sensor_sem, - &xHigherPriorityTaskWoken); - } - else - { - // Reset the timer to the currentTicks if the edge is ignored - lastBarcodeTime = currentTicks; - } - } - - portYIELD_FROM_ISR(xHigherPriorityTaskWoken); -} - -#endif /* LINE_SENSOR_INIT_H */ diff --git a/barcode_sensor/barcode_sensor_test.c b/barcode_sensor/barcode_sensor_test.c deleted file mode 100644 index 1aa45b4..0000000 --- a/barcode_sensor/barcode_sensor_test.c +++ /dev/null @@ -1,48 +0,0 @@ -#include "barcode_sensor.h" - -#define READ_BARCODE_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) - -void -launch() -{ - // isr to detect left line sensor - gpio_set_irq_enabled(BARCODE_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true); - gpio_add_raw_irq_handler(BARCODE_SENSOR_PIN, h_barcode_sensor_handler); - - - irq_set_enabled(IO_IRQ_BANK0, true); - - - 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); - - - 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); - - vTaskStartScheduler(); -} - -int -main (void) -{ - stdio_usb_init(); - - sleep_ms(10000); - printf("Test started!\n"); - - barcode_sensor_setup(); - initialize_car_state(); - - launch(); - - return (0); -} \ No newline at end of file From 96322b2fcd49ea1cd3604d6f711fcee66c2f83e0 Mon Sep 17 00:00:00 2001 From: soapysoltionss <2200754@sit.singaporetech.edu.sg> Date: Tue, 7 Nov 2023 13:13:45 +0800 Subject: [PATCH 15/23] edit --- barcode_sensor/CMakeLists.txt | 19 ---- barcode_sensor/barcode_sensor.h | 124 --------------------------- barcode_sensor/barcode_sensor_init.h | 97 --------------------- barcode_sensor/barcode_sensor_test.c | 48 ----------- 4 files changed, 288 deletions(-) delete mode 100644 barcode_sensor/CMakeLists.txt delete mode 100644 barcode_sensor/barcode_sensor.h delete mode 100644 barcode_sensor/barcode_sensor_init.h delete mode 100644 barcode_sensor/barcode_sensor_test.c diff --git a/barcode_sensor/CMakeLists.txt b/barcode_sensor/CMakeLists.txt deleted file mode 100644 index e61432b..0000000 --- a/barcode_sensor/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -add_executable( - barcode_sensor_test - barcode_sensor_test.c -) - -target_link_libraries( - barcode_sensor_test - hardware_adc - pico_stdlib - FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap -) - -target_include_directories(barcode_sensor_test - PRIVATE ../config - ../line_sensor -) - -pico_enable_stdio_usb(barcode_sensor_test 1) -pico_add_extra_outputs(barcode_sensor_test) \ No newline at end of file diff --git a/barcode_sensor/barcode_sensor.h b/barcode_sensor/barcode_sensor.h deleted file mode 100644 index 1a95a67..0000000 --- a/barcode_sensor/barcode_sensor.h +++ /dev/null @@ -1,124 +0,0 @@ -/* Barcode sensor */ - -#include "barcode_sensor_init.h" - -#define MAX_BARCODES 10 // Define the maximum number of barcodes to store -#define BARCODE_SENSOR_TIMER_PERIOD_MS 100 // Define the barcode sensor timer period - -// Define the barcode sensor timer -static struct repeating_timer barcode_sensor_timer; - - -/** - * @brief Decode a Code 39 barcode - * - * This function decodes a Code 39 barcode represented as a 9-bit binary number. - * - * @param barcode_data Binary representation of the barcode data (9 bits) - * @return Decoded value as an integer - */ -int code39_decode(uint32_t barcode_data) { - // Define the binary representations of Code 39 characters - const uint32_t code39_characters[] = { - 0b001001001, // 0 - 0b001001011, // 1 - 0b001011001, // 2 - 0b001011011, // 3 - 0b001100011, // 4 - 0b001101001, // 5 - 0b001101011, // 6 - 0b001010011, // 7 - 0b001011101, // 8 - 0b001111001, // 9 - // Add more character representations as needed - }; - - // Compare the barcode data to known Code 39 character representations - for (int i = 0; i < 10; i++) { - if (barcode_data == code39_characters[i]) { - return i; // Return the decoded value (0-9) - } - } - - // If the barcode data does not match any known character, return -1 to indicate an error - return -1; -} - - -/** - * @brief Monitor the barcode sensor - * - * This function will monitor the barcode sensor and send the state to the - * barcode sensor message buffer, including Code 39 decoding. - * - * @param params - */ -void monitor_barcode_sensor_task(void *params) { - // Create the barcode sensor timer - add_repeating_timer_ms(BARCODE_SENSOR_TIMER_PERIOD_MS, h_barcode_sensor_timer_handler, NULL, &barcode_sensor_timer); - - for (;;) { - if (xSemaphoreTake(g_barcode_sensor_sem, portMAX_DELAY) == pdTRUE) { - // Check the flag or receive the message - if (barcode_sensor_triggered == pdTRUE) { - uint32_t barcode_data = 0; - int bar_width = 0; // Variable to store the width of the current bar - - for (int i = 0; i < 9; i++) { - sleep_ms(100); // Wait for a segment of the barcode - - // Measure bar width using the IR sensor - if (gpio_get(BARCODE_SENSOR_PIN)) { - bar_width++; - } else { - // Bar ended, process the width - if (bar_width > 0) { - printf("Bar Width: %d\n", bar_width); - // Process or store the bar width as needed - bar_width = 0; // Reset the bar width measurement - } - - barcode_data |= (1u << i); - } - } - - printf("Barcode Data (binary): %09b\n", barcode_data); - - // Decode the barcode data - int decoded_value = code39_decode(barcode_data); - - if (decoded_value != -1) { - printf("Decoded Value: %d\n", decoded_value); - // Store or process the decoded value as needed - - // Send the decoded value instead of the raw barcode data - xMessageBufferSend(barcode_sensor_msg_buffer, &decoded_value, sizeof(int), 0); - } else { - printf("Error: Unable to decode the barcode.\n"); - } - - // Reset the flag - barcode_sensor_triggered = pdFALSE; - } - } - } -} - - - - -/** - * @brief Monitor the barcode sensor - * @param params - */ -void -monitor_barcode_task(__unused void *params) { - state_t barcode_state; - - // Receive from Buffer - xMessageBufferReceive(barcode_sensor_msg_buffer, - &barcode_state, - sizeof(state_t), - portMAX_DELAY); - -} \ No newline at end of file diff --git a/barcode_sensor/barcode_sensor_init.h b/barcode_sensor/barcode_sensor_init.h deleted file mode 100644 index e33041e..0000000 --- a/barcode_sensor/barcode_sensor_init.h +++ /dev/null @@ -1,97 +0,0 @@ -/* Initialise the barcode sensor */ - -#ifndef BARCODE_SENSOR_INIT_H -#define BARCODE_SENSOR_INIT_H - -#include -#include "pico/stdlib.h" - -#include "hardware/adc.h" - -#include "FreeRTOS.h" -#include "task.h" -#include "message_buffer.h" -#include "semphr.h" - -#include "barcode_sensor_config.h" -#include "line_sensor_init.h" - -// Set barcode time to 0 -static TickType_t lastBarcodeTime = 0; - - -// Semaphore -SemaphoreHandle_t g_barcode_sensor_sem = NULL; - - -// Queue -static MessageBufferHandle_t barcode_sensor_msg_buffer; // Barcode Sensor Buffer - -// Flag -static volatile BaseType_t barcode_sensor_triggered = pdFALSE; - - -/** - * @brief Setup the Line Sensor - * - * This function will setup the Line Sensor by initializing it as an input - */ -static inline void -barcode_sensor_setup() { - g_barcode_sensor_sem = xSemaphoreCreateBinary(); - - - uint mask = (1 << BARCODE_SENSOR_PIN); - - // Initialise 3 GPIO pins and set them to input - gpio_init_mask(mask); - gpio_set_dir_in_masked(mask); - - barcode_sensor_msg_buffer = xMessageBufferCreate(30); - -} - -/** - * @brief Timer Interrupt Handler for the barcode sensor - * - * @param repeatingTimer - * @return True (To keep the timer running) - */ -bool h_barcode_sensor_timer_handler(repeating_timer_t *repeatingTimer) { - - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(g_barcode_sensor_sem, - &xHigherPriorityTaskWoken); - portYIELD_FROM_ISR(xHigherPriorityTaskWoken); - - return true; -} - -void h_barcode_sensor_handler(void) { - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - TickType_t currentTicks = xTaskGetTickCount(); - printf("Interrupt triggered\n"); - - if (gpio_get_irq_event_mask(BARCODE_SENSOR_PIN) & GPIO_IRQ_EDGE_FALL) - { - if ((currentTicks - lastBarcodeTime) >= - pdMS_TO_TICKS(DEBOUNCE_DELAY_MS)) - { - lastBarcodeTime = currentTicks; - gpio_acknowledge_irq(BARCODE_SENSOR_PIN, GPIO_IRQ_EDGE_FALL); - // Set the flag to notify the task - barcode_sensor_triggered = pdTRUE; - xSemaphoreGiveFromISR(g_barcode_sensor_sem, - &xHigherPriorityTaskWoken); - } - else - { - // Reset the timer to the currentTicks if the edge is ignored - lastBarcodeTime = currentTicks; - } - } - - portYIELD_FROM_ISR(xHigherPriorityTaskWoken); -} - -#endif /* LINE_SENSOR_INIT_H */ diff --git a/barcode_sensor/barcode_sensor_test.c b/barcode_sensor/barcode_sensor_test.c deleted file mode 100644 index 1aa45b4..0000000 --- a/barcode_sensor/barcode_sensor_test.c +++ /dev/null @@ -1,48 +0,0 @@ -#include "barcode_sensor.h" - -#define READ_BARCODE_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) - -void -launch() -{ - // isr to detect left line sensor - gpio_set_irq_enabled(BARCODE_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true); - gpio_add_raw_irq_handler(BARCODE_SENSOR_PIN, h_barcode_sensor_handler); - - - irq_set_enabled(IO_IRQ_BANK0, true); - - - 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); - - - 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); - - vTaskStartScheduler(); -} - -int -main (void) -{ - stdio_usb_init(); - - sleep_ms(10000); - printf("Test started!\n"); - - barcode_sensor_setup(); - initialize_car_state(); - - launch(); - - return (0); -} \ No newline at end of file From f284f972d291a48632873c4e828439c3ed868b42 Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Wed, 8 Nov 2023 14:00:47 +0800 Subject: [PATCH 16/23] PID final --- frtos/config/motor_config.h | 5 ++ frtos/motor/motor_direction.h | 150 +++++++++++++++++----------------- frtos/motor/motor_pid.h | 47 ++++++++++- frtos/motor/motor_speed.h | 51 +++++++++++- frtos/motor/motor_test.c | 7 +- 5 files changed, 180 insertions(+), 80 deletions(-) diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 0572d4c..b97c2c7 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -18,6 +18,11 @@ #define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4) #define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3) +#define DIRECTION_FORWARD (DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD) +#define DIRECTION_BACKWARD (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_BACKWARD) + +#define DIRECTION_MASK (DIRECTION_FORWARD | DIRECTION_BACKWARD) + #define SPEED_PIN_RIGHT 15U #define SPEED_PIN_LEFT 16U diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index 6c31e29..9464cb8 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -80,80 +80,80 @@ turn_right_90() g_motor_right.speed.distance_cm = initial; } -void -spin_left_90() -{ - set_wheel_direction(DIRECTION_LEFT); +//void +//spin_left_90() +//{ +// set_wheel_direction(DIRECTION_LEFT); +// +// set_wheel_speed_synced(90u); +// +// 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; +//} - set_wheel_speed_synced(90u); +//void +//spin_right_90() +//{ +// set_wheel_direction(DIRECTION_RIGHT); +// +// set_wheel_speed_synced(90u); +// +// 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; +//} - 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(90u); - - 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; -} - -void -spin_to_yaw(float target_yaw) -{ - float initial_yaw = g_direction.yaw; - - // if it will to turn more than 180 degrees, turn the other way - if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw < 180.f) - || ((target_yaw < initial_yaw) && (initial_yaw - target_yaw >= 180.f))) - { - set_wheel_direction(DIRECTION_LEFT); - } - else if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw >= 180.f) - || ((target_yaw < initial_yaw) - && (initial_yaw - target_yaw < 180.f))) - { - set_wheel_direction(DIRECTION_RIGHT); - } - - set_wheel_speed_synced(90u); - - g_use_pid = false; - - for (;;) - { - if (initial_yaw == target_yaw) - { - set_wheel_speed_synced(0u); - break; - } - vTaskDelay(pdMS_TO_TICKS(5)); - } - g_use_pid = true; -} \ No newline at end of file +//void +//spin_to_yaw(float target_yaw) +//{ +// float initial_yaw = g_direction.yaw; +// +// // if it will to turn more than 180 degrees, turn the other way +// if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw < 180.f) +// || ((target_yaw < initial_yaw) && (initial_yaw - target_yaw >= 180.f))) +// { +// set_wheel_direction(DIRECTION_LEFT); +// } +// else if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw >= 180.f) +// || ((target_yaw < initial_yaw) +// && (initial_yaw - target_yaw < 180.f))) +// { +// set_wheel_direction(DIRECTION_RIGHT); +// } +// +// set_wheel_speed_synced(90u); +// +// g_use_pid = false; +// +// for (;;) +// { +// if (initial_yaw == target_yaw) +// { +// set_wheel_speed_synced(0u); +// break; +// } +// vTaskDelay(pdMS_TO_TICKS(5)); +// } +// g_use_pid = true; +//} \ No newline at end of file diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index 1930ed6..cc7ce33 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -36,6 +36,19 @@ compute_pid(float *integral, float *prev_error) return control_signal; } +float +compute_i_controller(float *integral) +{ + float error + = g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm; + + printf("%f\n", error); + + *integral += error; + + return g_motor_right.pid.ki_value * (*integral); +} + bool repeating_pid_handler(__unused struct repeating_timer *t) { @@ -66,8 +79,38 @@ repeating_pid_handler(__unused struct repeating_timer *t) 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); + // 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; +} + +bool +repeating_i_handler(__unused struct repeating_timer *t) +{ + static float integral = 0.0f; + + if (!g_use_pid) + { + integral = 0.0f; // reset once disabled + return true; + } + + float control_signal = compute_i_controller(&integral); + + float temp = (float)g_motor_right.pwm.level + control_signal * 0.05f; + + if (temp > MAX_PWM_LEVEL) + { + temp = MAX_PWM_LEVEL; + } + + if (temp <= MIN_PWM_LEVEL) + { + temp = MIN_PWM_LEVEL; + } + + set_wheel_speed((uint32_t)temp, &g_motor_right); return true; } \ No newline at end of file diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index 230a596..6fc1c58 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -72,4 +72,53 @@ monitor_wheel_speed_task(void *pvParameters) p_motor->speed.current_cms = 0.f; } } -} \ No newline at end of file +} + +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 +// */ +//void +//set_wheel_speed_synced(uint32_t pwm_level) +//{ +// if (pwm_level > MAX_PWM_LEVEL) +// { +// pwm_level = MAX_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 +// * setting the speed and direction. +// * @param distance_cm distance to travel in cm +// */ +//void +//distance_to_stop(float distance_cm) +//{ +// float initial = g_motor_right.speed.distance_cm; +// +// for (;;) +// { +// if (g_motor_right.speed.distance_cm - initial >= distance_cm) +// { +// set_wheel_speed_synced(0u); +// break; +// } +// vTaskDelay(pdMS_TO_TICKS(10)); +// } +// vTaskDelay(pdMS_TO_TICKS(1000)); +// g_motor_right.speed.distance_cm = g_motor_left.speed.distance_cm; +//} diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index 8899ebb..5661284 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -10,12 +10,14 @@ motor_control_task(__unused void *params) for (;;) { set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed_synced(90); + set_wheel_speed(90u, &g_motor_left); + set_wheel_speed(90u, &g_motor_right); vTaskDelay(pdMS_TO_TICKS(10000)); revert_wheel_direction(); - set_wheel_speed_synced(90); + set_wheel_speed(90u, &g_motor_left); + set_wheel_speed(90u, &g_motor_right); vTaskDelay(pdMS_TO_TICKS(10000)); } @@ -37,6 +39,7 @@ launch() // PID timer struct repeating_timer pid_timer; add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer); +// add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer); // Left wheel // From f65c0ab1896214c87d04b8241aa259f048772b5f Mon Sep 17 00:00:00 2001 From: Devoalda Date: Wed, 8 Nov 2023 14:09:54 +0800 Subject: [PATCH 17/23] fix(Magnetometer): Added option for calibration --- frtos/magnetometer/magnetometer_direction.h | 2 +- frtos/magnetometer/magnetometer_init.h | 197 ++++++++++++++++++++ frtos/magnetometer/magnetometer_read.h | 105 ----------- 3 files changed, 198 insertions(+), 106 deletions(-) diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h index 2cabc97..d9dd5c8 100644 --- a/frtos/magnetometer/magnetometer_direction.h +++ b/frtos/magnetometer/magnetometer_direction.h @@ -377,7 +377,7 @@ void updateDirection() { // update_map(g_direction.orientation, cur_x, cur_y); // printf("Current Position: (%d, %d)\n", cur_x, cur_y); -// print_map(); +// print_map(); // print_roll_and_pitch(g_direction.roll_angle, g_direction.pitch_angle); } diff --git a/frtos/magnetometer/magnetometer_init.h b/frtos/magnetometer/magnetometer_init.h index 1de1432..fdb1b0b 100644 --- a/frtos/magnetometer/magnetometer_init.h +++ b/frtos/magnetometer/magnetometer_init.h @@ -38,6 +38,198 @@ direction_t g_direction = { .pitch_angle = UP }; +struct s_calibration_data { + int16_t accelerometerBias[3]; + int16_t magnetometerBias[3]; +}; + +struct s_calibration_data g_calibration_data = { + .accelerometerBias = {0, 0, 0}, + .magnetometerBias = {0, 0, 0} +}; + +/** + * @brief Read Data with I2C, given the address and register + * @param addr Address of the device + * @param reg Register to read from + * @return 1 piece of data read from the register + */ +static inline int +read_data(uint8_t addr, uint8_t reg) { + uint8_t data[1]; + + // Send the register address to read from + i2c_write_blocking(i2c_default, addr, ®, 1, true); + + // Read the data + i2c_read_blocking(i2c_default, addr, data, 1, false); + + return data[0]; +} + +/** + * @brief Read Accelerometer Data + * @param accelerometer Accelerometer Data + */ +static inline void +read_accelerometer(int16_t accelerometer[3]) { + uint8_t buffer[6]; + + buffer[0] = read_data(ACCEL_ADDR, LSM303_OUT_X_L_A); + buffer[1] = read_data(ACCEL_ADDR, LSM303_OUT_X_H_A); + buffer[2] = read_data(ACCEL_ADDR, LSM303_OUT_Y_L_A); + buffer[3] = read_data(ACCEL_ADDR, LSM303_OUT_Y_H_A); + buffer[4] = read_data(ACCEL_ADDR, LSM303_OUT_Z_L_A); + buffer[5] = read_data(ACCEL_ADDR, LSM303_OUT_Z_H_A); + + // Combine high and low bytes + + // xAcceleration + accelerometer[0] = (int16_t) ((buffer[1] << 8) | buffer[0]); + + // yAcceleration + accelerometer[1] = (int16_t) ((buffer[3] << 8) | buffer[2]); + + // zAcceleration + accelerometer[2] = (int16_t) ((buffer[5] << 8) | buffer[4]); + + // Apply the calibration data + accelerometer[0] -= g_calibration_data.accelerometerBias[0]; + accelerometer[1] -= g_calibration_data.accelerometerBias[1]; + accelerometer[2] -= g_calibration_data.accelerometerBias[2]; + +} + +/** + * @brief Read Magnetometer Data with Moving Average + * @param magnetometer Magnetometer Data + */ +static inline void +read_magnetometer(int16_t magnetometer[3]) { + uint8_t buffer[6]; + int32_t xMagFiltered = 0; + int32_t yMagFiltered = 0; + int32_t zMagFiltered = 0; + + for (int i = 0; i < NUM_READINGS; i ++) + { + buffer[0] = read_data(MAG_ADDR, LSM303_OUT_X_H_M); + buffer[1] = read_data(MAG_ADDR, LSM303_OUT_X_L_M); + buffer[2] = read_data(MAG_ADDR, LSM303_OUT_Y_H_M); + buffer[3] = read_data(MAG_ADDR, LSM303_OUT_Y_L_M); + buffer[4] = read_data(MAG_ADDR, LSM303_OUT_Z_H_M); + buffer[5] = read_data(MAG_ADDR, LSM303_OUT_Z_L_M); + + // Update the cumulative sum of the magnetometer data + xMagFiltered += (int16_t) (buffer[0] << 8 | buffer[1]); + yMagFiltered += (int16_t) (buffer[2] << 8 | buffer[3]); + zMagFiltered += (int16_t) (buffer[4] << 8 | buffer[5]); + } + + // Calculate the moving average + magnetometer[0] = xMagFiltered / NUM_READINGS; + magnetometer[1] = yMagFiltered / NUM_READINGS; + magnetometer[2] = zMagFiltered / NUM_READINGS; + + // Apply the calibration data + magnetometer[0] -= g_calibration_data.magnetometerBias[0]; + magnetometer[1] -= g_calibration_data.magnetometerBias[1]; + magnetometer[2] -= g_calibration_data.magnetometerBias[2]; +} + +/** + * @brief Read Temperature Data in Degrees Celsius + * @param temperature Temperature Data in Degrees Celsius + */ +static inline void +read_temperature(int16_t temperature[1]) { + uint8_t buffer[2]; + + buffer[0] = read_data(MAG_ADDR, LSM303_TEMP_OUT_H_M); + buffer[1] = read_data(MAG_ADDR, LSM303_TEMP_OUT_L_M); + + /** + * Normalize temperature; it is big-endian, fixed-point + * 9 bits signed integer, 3 bits fractional part, 4 bits zeros + * and is relative to 20 degrees Celsius + * Source: https://electronics.stackexchange.com/a/356964 + */ + + int16_t raw_temperature = + (20 << 3) + (((int16_t) buffer[0] << 8 | buffer[1]) >> 4); + + // Convert the raw temperature data to degrees Celsius + float temperature_celsius = (float) raw_temperature / 8.0; + + // Store the result in the temperature array + temperature[0] = (int16_t) temperature_celsius; +} + +static void initial_calibration() { + int16_t accelerometer[3]; + int16_t magnetometer[3]; + + int16_t accelerometerMin[3] = {0, 0, 0}; + int16_t accelerometerMax[3] = {0, 0, 0}; + int16_t magnetometerMin[3] = {0, 0, 0}; + int16_t magnetometerMax[3] = {0, 0, 0}; + + printf("Initial Calibration\n"); + + for (int i = 0; i < 100; i ++) + { + printf("Calibrating... %d\n", i); + + read_accelerometer(accelerometer); + read_magnetometer(magnetometer); + + for (int j = 0; j < 3; j ++) + { + if (accelerometer[j] > accelerometerMax[j]) + { + accelerometerMax[j] = accelerometer[j]; + } + if (accelerometer[j] < accelerometerMin[j]) + { + accelerometerMin[j] = accelerometer[j]; + } + if (magnetometer[j] > magnetometerMax[j]) + { + magnetometerMax[j] = magnetometer[j]; + } + if (magnetometer[j] < magnetometerMin[j]) + { + magnetometerMin[j] = magnetometer[j]; + } + } + sleep_ms(10); + } + + g_calibration_data.accelerometerBias[0] = + (accelerometerMax[0] + accelerometerMin[0]) / 2; + g_calibration_data.accelerometerBias[1] = + (accelerometerMax[1] + accelerometerMin[1]) / 2; + g_calibration_data.accelerometerBias[2] = + (accelerometerMax[2] + accelerometerMin[2]) / 2; + + g_calibration_data.magnetometerBias[0] = + (magnetometerMax[0] + magnetometerMin[0]) / 2; + g_calibration_data.magnetometerBias[1] = + (magnetometerMax[1] + magnetometerMin[1]) / 2; + g_calibration_data.magnetometerBias[2] = + (magnetometerMax[2] + magnetometerMin[2]) / 2; + + printf("Accelerometer Bias: %d, %d, %d\n", + g_calibration_data.accelerometerBias[0], + g_calibration_data.accelerometerBias[1], + g_calibration_data.accelerometerBias[2]); + + printf("Magnetometer Bias: %d, %d, %d\n", + g_calibration_data.magnetometerBias[0], + g_calibration_data.magnetometerBias[1], + g_calibration_data.magnetometerBias[2]); +} + /** * @brief Initialise the LSM303DLHC sensor (Accelerometer and Magnetometer) * @details @@ -104,6 +296,10 @@ magnetometer_init() LSM303DLHC_init(); +// initial_calibration(); + +// sleep_ms(3000); +// printf("Magnetometer Initialised\n"); // Semaphore g_direction_sem = xSemaphoreCreateBinary(); } @@ -122,4 +318,5 @@ h_direction_timer_handler(repeating_timer_t *repeatingTimer) { portYIELD_FROM_ISR(xHigherPriorityTaskWoken); return true; } + #endif \ No newline at end of file diff --git a/frtos/magnetometer/magnetometer_read.h b/frtos/magnetometer/magnetometer_read.h index 36f3f35..8f8911b 100644 --- a/frtos/magnetometer/magnetometer_read.h +++ b/frtos/magnetometer/magnetometer_read.h @@ -10,110 +10,5 @@ #include "magnetometer_init.h" -/** - * @brief Read Data with I2C, given the address and register - * @param addr Address of the device - * @param reg Register to read from - * @return 1 piece of data read from the register - */ -static inline int -read_data(uint8_t addr, uint8_t reg) { - uint8_t data[1]; - - // Send the register address to read from - i2c_write_blocking(i2c_default, addr, ®, 1, true); - - // Read the data - i2c_read_blocking(i2c_default, addr, data, 1, false); - - return data[0]; -} - -/** - * @brief Read Accelerometer Data - * @param accelerometer Accelerometer Data - */ -static inline void -read_accelerometer(int16_t accelerometer[3]) { - uint8_t buffer[6]; - - buffer[0] = read_data(ACCEL_ADDR, LSM303_OUT_X_L_A); - buffer[1] = read_data(ACCEL_ADDR, LSM303_OUT_X_H_A); - buffer[2] = read_data(ACCEL_ADDR, LSM303_OUT_Y_L_A); - buffer[3] = read_data(ACCEL_ADDR, LSM303_OUT_Y_H_A); - buffer[4] = read_data(ACCEL_ADDR, LSM303_OUT_Z_L_A); - buffer[5] = read_data(ACCEL_ADDR, LSM303_OUT_Z_H_A); - - // Combine high and low bytes - - // xAcceleration - accelerometer[0] = (int16_t) ((buffer[1] << 8) | buffer[0]); - - // yAcceleration - accelerometer[1] = (int16_t) ((buffer[3] << 8) | buffer[2]); - - // zAcceleration - accelerometer[2] = (int16_t) ((buffer[5] << 8) | buffer[4]); - -} - -/** - * @brief Read Magnetometer Data with Moving Average - * @param magnetometer Magnetometer Data - */ -static inline void -read_magnetometer(int16_t magnetometer[3]) { - uint8_t buffer[6]; - int32_t xMagFiltered = 0; - int32_t yMagFiltered = 0; - int32_t zMagFiltered = 0; - - for (int i = 0; i < NUM_READINGS; i++) { - buffer[0] = read_data(MAG_ADDR, LSM303_OUT_X_H_M); - buffer[1] = read_data(MAG_ADDR, LSM303_OUT_X_L_M); - buffer[2] = read_data(MAG_ADDR, LSM303_OUT_Y_H_M); - buffer[3] = read_data(MAG_ADDR, LSM303_OUT_Y_L_M); - buffer[4] = read_data(MAG_ADDR, LSM303_OUT_Z_H_M); - buffer[5] = read_data(MAG_ADDR, LSM303_OUT_Z_L_M); - - // Update the cumulative sum of the magnetometer data - xMagFiltered += (int16_t)(buffer[0] << 8 | buffer[1]); - yMagFiltered += (int16_t)(buffer[2] << 8 | buffer[3]); - zMagFiltered += (int16_t)(buffer[4] << 8 | buffer[5]); - } - - // Calculate the moving average - magnetometer[0] = xMagFiltered / NUM_READINGS; - magnetometer[1] = yMagFiltered / NUM_READINGS; - magnetometer[2] = zMagFiltered / NUM_READINGS; -} - -/** - * @brief Read Temperature Data in Degrees Celsius - * @param temperature Temperature Data in Degrees Celsius - */ -static inline void -read_temperature(int16_t temperature[1]) { - uint8_t buffer[2]; - - buffer[0] = read_data(MAG_ADDR, LSM303_TEMP_OUT_H_M); - buffer[1] = read_data(MAG_ADDR, LSM303_TEMP_OUT_L_M); - - /** - * Normalize temperature; it is big-endian, fixed-point - * 9 bits signed integer, 3 bits fractional part, 4 bits zeros - * and is relative to 20 degrees Celsius - * Source: https://electronics.stackexchange.com/a/356964 - */ - - int16_t raw_temperature = - (20 << 3) + (((int16_t) buffer[0] << 8 | buffer[1]) >> 4); - - // Convert the raw temperature data to degrees Celsius - float temperature_celsius = (float) raw_temperature / 8.0; - - // Store the result in the temperature array - temperature[0] = (int16_t) temperature_celsius; -} #endif \ No newline at end of file From a066bd06a66cc004d1b5d5d4aafcac067a742f77 Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Wed, 8 Nov 2023 15:11:42 +0800 Subject: [PATCH 18/23] add redeclare prevent --- frtos/config/motor_config.h | 2 + frtos/motor/motor_direction.h | 74 ++++++++++++++++++++--------------- frtos/motor/motor_pid.h | 7 +++- frtos/motor/motor_speed.h | 34 +++++++++------- 4 files changed, 70 insertions(+), 47 deletions(-) diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index b97c2c7..f22a356 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -20,6 +20,8 @@ #define DIRECTION_FORWARD (DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD) #define DIRECTION_BACKWARD (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_BACKWARD) +#define DIRECTION_LEFT (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_FORWARD) +#define DIRECTION_RIGHT (DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_BACKWARD) #define DIRECTION_MASK (DIRECTION_FORWARD | DIRECTION_BACKWARD) diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index 9464cb8..57f6463 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -4,8 +4,12 @@ * @author Richie */ +#ifndef MOTOR_DIRECTION_H +#define MOTOR_DIRECTION_H + #include "motor_init.h" #include "magnetometer_init.h" +#include "magnetometer_direction.h" /*! * @brief Set the direction of the wheels; can use bitwise OR to set both @@ -124,36 +128,44 @@ turn_right_90() // g_motor_left.speed.distance_cm = initial; //} -//void -//spin_to_yaw(float target_yaw) -//{ -// float initial_yaw = g_direction.yaw; -// -// // if it will to turn more than 180 degrees, turn the other way -// if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw < 180.f) -// || ((target_yaw < initial_yaw) && (initial_yaw - target_yaw >= 180.f))) -// { -// set_wheel_direction(DIRECTION_LEFT); -// } -// else if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw >= 180.f) -// || ((target_yaw < initial_yaw) -// && (initial_yaw - target_yaw < 180.f))) -// { -// set_wheel_direction(DIRECTION_RIGHT); -// } -// -// set_wheel_speed_synced(90u); -// -// g_use_pid = false; -// -// for (;;) -// { -// if (initial_yaw == target_yaw) +void +spin_to_yaw(float target_yaw) +{ + float initial_yaw = g_direction.yaw; + + // if it will to turn more than 180 degrees, turn the other way + if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw < 180.f) + || ((target_yaw < initial_yaw) && (initial_yaw - target_yaw >= 180.f))) + { + set_wheel_direction(DIRECTION_LEFT); + } + else if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw >= 180.f) + || ((target_yaw < initial_yaw) + && (initial_yaw - target_yaw < 180.f))) + { + set_wheel_direction(DIRECTION_RIGHT); + } + + set_wheel_speed_synced(50u); + + g_use_pid = false; + + for (;;) + { +// if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE) // { -// set_wheel_speed_synced(0u); -// break; +// updateDirection(); // } -// vTaskDelay(pdMS_TO_TICKS(5)); -// } -// g_use_pid = true; -//} \ No newline at end of file + updateDirection(); + print_orientation_data(); + if (initial_yaw == target_yaw) + { + set_wheel_speed_synced(0u); + break; + } + vTaskDelay(pdMS_TO_TICKS(5)); + } + g_use_pid = true; +} + +#endif /* MOTOR_DIRECTION_H */ \ No newline at end of file diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index cc7ce33..706b27f 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -5,6 +5,9 @@ * @author Richie */ +#ifndef MOTOR_PID_H +#define MOTOR_PID_H + // #include "magnetometer_init.h" /*! @@ -113,4 +116,6 @@ repeating_i_handler(__unused struct repeating_timer *t) set_wheel_speed((uint32_t)temp, &g_motor_right); return true; -} \ No newline at end of file +} + +#endif /* MOTOR_PID_H */ \ No newline at end of file diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index 6fc1c58..be0072e 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -3,6 +3,8 @@ * @brief monitor and update the speed of the wheels * @author Richie */ +#ifndef MOTOR_SPEED_H +#define MOTOR_SPEED_H #include "motor_init.h" @@ -84,21 +86,21 @@ set_wheel_speed(uint32_t pwm_level, motor_t * motor) motor->pwm.level); } -///*! -// * @brief Set the speed of the wheels -// * @param pwm_level The pwm_level of the wheels, from 0 to 99 -// */ -//void -//set_wheel_speed_synced(uint32_t pwm_level) -//{ -// if (pwm_level > MAX_PWM_LEVEL) -// { -// pwm_level = MAX_PWM_LEVEL; -// } -// -// set_wheel_speed(pwm_level, &g_motor_left); -// set_wheel_speed(pwm_level, &g_motor_right); -//} +/*! + * @brief Set the speed of the wheels + * @param pwm_level The pwm_level of the wheels, from 0 to 99 + */ +void +set_wheel_speed_synced(uint32_t pwm_level) +{ + if (pwm_level > MAX_PWM_LEVEL) + { + pwm_level = MAX_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 @@ -122,3 +124,5 @@ set_wheel_speed(uint32_t pwm_level, motor_t * motor) // vTaskDelay(pdMS_TO_TICKS(1000)); // g_motor_right.speed.distance_cm = g_motor_left.speed.distance_cm; //} + +#endif /* MOTOR_SPEED_H */ From b52685d25e88eb017358d595ddf2f41073ea7beb Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Wed, 8 Nov 2023 17:07:35 +0800 Subject: [PATCH 19/23] add direction calculation --- frtos/motor/motor_direction.h | 114 +++++++--------------------------- 1 file changed, 24 insertions(+), 90 deletions(-) diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index 57f6463..b4f10ff 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -10,6 +10,7 @@ #include "motor_init.h" #include "magnetometer_init.h" #include "magnetometer_direction.h" +#include "math.h" /*! * @brief Set the direction of the wheels; can use bitwise OR to set both @@ -42,130 +43,63 @@ revert_wheel_direction() gpio_set_mask(reverted_direction & DIRECTION_MASK); } -void -turn_left_90() +bool +check_direction(float current_direction, float target_direction, float range) { - set_wheel_direction(DIRECTION_RIGHT_FORWARD); - set_wheel_speed(90u, &g_motor_right); + // Normalize directions to be within 0 to 360 degrees + current_direction = fmod(current_direction, 360.0f); + if (current_direction < 0) + current_direction += 360.0f; - float initial = g_motor_right.speed.distance_cm; - for (;;) + target_direction = fmod(target_direction, 360.0f); + if (target_direction < 0) + target_direction += 360.0f; + + // Check if current_direction is within ±1 degree of target_direction + if (fabs(current_direction - target_direction) <= range + || fabs(current_direction - target_direction) >= (360 - range)) { - if (g_motor_right.speed.distance_cm - initial >= 17.f) - { - set_wheel_speed(0u, &g_motor_right); - break; - } - vTaskDelay(pdMS_TO_TICKS(5)); + return true; } - vTaskDelay(pdMS_TO_TICKS(1000)); - g_motor_right.speed.distance_cm = initial; - g_motor_left.speed.distance_cm = initial; + return false; } -void -turn_right_90() -{ - set_wheel_direction(DIRECTION_LEFT_FORWARD); - set_wheel_speed(90u, &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(90u); -// -// 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(90u); -// -// 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; -//} - void spin_to_yaw(float target_yaw) { + updateDirection(); float initial_yaw = g_direction.yaw; // if it will to turn more than 180 degrees, turn the other way if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw < 180.f) || ((target_yaw < initial_yaw) && (initial_yaw - target_yaw >= 180.f))) { - set_wheel_direction(DIRECTION_LEFT); + set_wheel_direction(DIRECTION_RIGHT); } else if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw >= 180.f) || ((target_yaw < initial_yaw) && (initial_yaw - target_yaw < 180.f))) { - set_wheel_direction(DIRECTION_RIGHT); + set_wheel_direction(DIRECTION_LEFT); } - set_wheel_speed_synced(50u); + set_wheel_speed_synced(80u); g_use_pid = false; for (;;) { -// if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE) -// { -// updateDirection(); -// } updateDirection(); - print_orientation_data(); - if (initial_yaw == target_yaw) + if (check_direction(g_direction.yaw, target_yaw, 1)) { + set_wheel_direction(DIRECTION_MASK); set_wheel_speed_synced(0u); break; } - vTaskDelay(pdMS_TO_TICKS(5)); } + g_use_pid = true; + vTaskDelay(pdMS_TO_TICKS(50)); } #endif /* MOTOR_DIRECTION_H */ \ No newline at end of file From 191c8686c0e17cc5bef404a2e8e295a2a204ecd8 Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Wed, 8 Nov 2023 17:09:01 +0800 Subject: [PATCH 20/23] push every thing --- frtos/CMakeLists.txt | 1 + frtos/config/ultrasonic_sensor_config.h | 4 +- frtos/line_sensor/line_sensor_init.h | 58 ++++----- frtos/magnetometer/magnetometer_direction.h | 5 + frtos/magnetometer/magnetometer_init.h | 4 +- frtos/rtos_car.c | 129 +++++++++++--------- frtos/ultrasonic_sensor/CMakeLists.txt | 5 + frtos/ultrasonic_sensor/ultrasonic_sensor.h | 125 ++++++++++--------- 8 files changed, 183 insertions(+), 148 deletions(-) diff --git a/frtos/CMakeLists.txt b/frtos/CMakeLists.txt index 3ded301..28447a9 100644 --- a/frtos/CMakeLists.txt +++ b/frtos/CMakeLists.txt @@ -23,6 +23,7 @@ target_include_directories(rtos_car PRIVATE ${CMAKE_CURRENT_LIST_DIR}/line_sensor ${CMAKE_CURRENT_LIST_DIR}/ultrasonic_sensor ${CMAKE_CURRENT_LIST_DIR}/frontend + ${CMAKE_CURRENT_LIST_DIR}/magnetometer ) target_link_libraries(rtos_car pico_cyw43_arch_lwip_sys_freertos diff --git a/frtos/config/ultrasonic_sensor_config.h b/frtos/config/ultrasonic_sensor_config.h index a271db0..3bdcc22 100644 --- a/frtos/config/ultrasonic_sensor_config.h +++ b/frtos/config/ultrasonic_sensor_config.h @@ -3,8 +3,8 @@ /* ADC Configuration */ -#define TRIG_PIN ( 0 ) -#define ECHO_PIN ( 1 ) +#define TRIG_PIN ( 2 ) +#define ECHO_PIN ( 3 ) #define ULTRASONIC_SENSOR_READ_DELAY ( 500 ) diff --git a/frtos/line_sensor/line_sensor_init.h b/frtos/line_sensor/line_sensor_init.h index 0d45987..b15447a 100644 --- a/frtos/line_sensor/line_sensor_init.h +++ b/frtos/line_sensor/line_sensor_init.h @@ -30,26 +30,26 @@ typedef enum { // Unused, useful for readability LINE_NOT_DETECTED = 1, } state_t; -typedef enum { - ERROR = 0, - RIGHT = 1, - LEFT = 2, - FORWARD = 3 -} direction_t; +//typedef enum { +// ERROR = 0, +// RIGHT = 1, +// LEFT = 2, +// FORWARD = 3 +//} direction_t; -typedef enum { - NORTH = 0, - EAST = 1, - SOUTH = 2, - WEST = 3, -} orientation_t; +//typedef enum { +// NORTH = 0, +// EAST = 1, +// SOUTH = 2, +// WEST = 3, +//} orientation_t; -typedef struct { - u_int8_t x; // Current x coordinate - u_int8_t y; // Current y coordinate - direction_t current_direction; // Current direction (forward, left, right) - orientation_t orientation; // Current orientation (N, E, S, W) -} car_state_t; +//typedef struct { +// uint8_t x; // Current x coordinate +// uint8_t y; // Current y coordinate +// direction_t current_direction; // Current direction (forward, left, right) +// orientation_t orientation; // Current orientation (N, E, S, W) +//} car_state_t; // Semaphore SemaphoreHandle_t g_left_sensor_sem = NULL; @@ -65,17 +65,17 @@ static volatile BaseType_t right_sensor_triggered = pdFALSE; static volatile BaseType_t left_sensor_triggered = pdFALSE; static volatile BaseType_t barcode_sensor_triggered = pdFALSE; -// Car State Struct -static car_state_t g_car_state; - -static car_state_t initialize_car_state() { - g_car_state.x = MAP_SIZE >> 1; - g_car_state.y = MAP_SIZE >> 1; - g_car_state.current_direction = FORWARD; - g_car_state.orientation = NORTH; - - return g_car_state; -} +//// Car State Struct +//static car_state_t g_car_state; +// +//static car_state_t initialize_car_state() { +// g_car_state.x = MAP_SIZE >> 1; +// g_car_state.y = MAP_SIZE >> 1; +// g_car_state.current_direction = FORWARD; +// g_car_state.orientation = NORTH; +// +// return g_car_state; +//} /** * @brief Setup the Line Sensor diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h index d9dd5c8..e21ca74 100644 --- a/frtos/magnetometer/magnetometer_direction.h +++ b/frtos/magnetometer/magnetometer_direction.h @@ -103,6 +103,11 @@ adjust_yaw(float yaw) { yaw += 360; } + if (yaw > 360) + { + yaw -= 360; + } + return yaw; } diff --git a/frtos/magnetometer/magnetometer_init.h b/frtos/magnetometer/magnetometer_init.h index fdb1b0b..f704de8 100644 --- a/frtos/magnetometer/magnetometer_init.h +++ b/frtos/magnetometer/magnetometer_init.h @@ -299,9 +299,9 @@ magnetometer_init() // initial_calibration(); // sleep_ms(3000); -// printf("Magnetometer Initialised\n"); + printf("Magnetometer Initialised\n"); // Semaphore - g_direction_sem = xSemaphoreCreateBinary(); + // g_direction_sem = xSemaphoreCreateBinary(); } /** diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c index 7c52131..2d0da53 100644 --- a/frtos/rtos_car.c +++ b/frtos/rtos_car.c @@ -16,15 +16,15 @@ #include "motor_direction.h" #include "motor_pid.h" -#include "line_sensor.h" +// #include "line_sensor.h" #include "ultrasonic_sensor.h" -#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 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 DISTANCE_TASK_PRIORITY (tskIDLE_PRIORITY + 4UL) @@ -34,49 +34,37 @@ static void motor_control_task(__unused void *p_param) { - static bool left = false; - static bool right = false; + vTaskDelay(1000); - for (;;) - { - state_t state = gpio_get(LEFT_SENSOR_PIN); - printf("state: %d\n", state); +// set_wheel_direction(DIRECTION_FORWARD); +// set_wheel_speed_synced(90); +// vTaskDelay(1000); - 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)); - } +// spin_to_yaw(300); +// +// set_wheel_direction(DIRECTION_FORWARD); +// set_wheel_speed_synced(90); +// vTaskDelay(1000); + + spin_to_yaw(65); + + set_wheel_direction(DIRECTION_FORWARD); + set_wheel_speed_synced(90); + vTaskDelay(10000); + +// set_wheel_direction(DIRECTION_MASK); + + for (;;); } 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); + // // 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); + + // isr for ultrasonic // isr to detect right motor slot gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true); @@ -88,24 +76,25 @@ launch() 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); + // // 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, - "Monitor Left Sensor Task", - configMINIMAL_STACK_SIZE, - NULL, - READ_LEFT_SENSOR_PRIO, - &h_monitor_left_sensor_task); + // Line sensor + // TaskHandle_t h_monitor_left_sensor_task; + // xTaskCreate(monitor_left_sensor_task, + // "Monitor Left Sensor Task", + // configMINIMAL_STACK_SIZE, + // NULL, + // READ_LEFT_SENSOR_PRIO, + // &h_monitor_left_sensor_task); // Left wheel // @@ -136,6 +125,22 @@ launch() WHEEL_CONTROL_PRIO, &h_motor_turning_task_handle); + // ultra + TaskHandle_t disttask; + xTaskCreate(distance_task, + "TestDistThread", + configMINIMAL_STACK_SIZE, + NULL, + 1, + &disttask); + + // magnetometer + // struct repeating_timer g_direction_timer; + // add_repeating_timer_ms(DIRECTION_READ_DELAY, + // h_direction_timer_handler, + // NULL, + // &g_direction_timer); + vTaskStartScheduler(); } @@ -148,13 +153,19 @@ main(void) printf("Test started!\n"); motor_init(); + printf("motor init"); - line_sensor_setup(); + magnetometer_init(); + printf("magnet init"); - //init_ultrasonic(); + // line_sensor_setup(); - //initialize_car_state(); // TODO: Could be common functionality, To confirm - // during Integration + init_ultrasonic(); + printf("ultraman init"); + + // initialize_car_state(); // TODO: Could be common functionality, To + // confirm + // during Integration launch(); return (0); diff --git a/frtos/ultrasonic_sensor/CMakeLists.txt b/frtos/ultrasonic_sensor/CMakeLists.txt index 1a9bf1b..30fe799 100644 --- a/frtos/ultrasonic_sensor/CMakeLists.txt +++ b/frtos/ultrasonic_sensor/CMakeLists.txt @@ -6,12 +6,17 @@ add_executable( target_link_libraries( ultrasonic_sensor hardware_adc + 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(ultrasonic_sensor PRIVATE ../config + ../motor ) pico_enable_stdio_usb(ultrasonic_sensor 1) diff --git a/frtos/ultrasonic_sensor/ultrasonic_sensor.h b/frtos/ultrasonic_sensor/ultrasonic_sensor.h index f5f1899..014f1bc 100644 --- a/frtos/ultrasonic_sensor/ultrasonic_sensor.h +++ b/frtos/ultrasonic_sensor/ultrasonic_sensor.h @@ -1,82 +1,95 @@ /** - * @file ultrasonic_sensor.h - * @brief Monitor the distance between the car and the wall - * @author Poon Xiang Yuan - */ +* @file ultrasonic_sensor.h +* @brief Monitor the distance between the car and the wall +* @author Poon Xiang Yuan +*/ #ifndef ULTRASONIC_SENSOR_H #define ULTRASONIC_SENSOR_H #include "ultrasonic_init.h" +#include "motor_speed.h" -volatile uint32_t start_time; -volatile uint32_t end_time; -volatile bool echo_rising = false; +// volatile uint32_t start_time; +// volatile uint32_t end_time; +volatile bool echo_rising = false; float KalmanFilter(float U) { - static float R = 10; // noise convariance can be 10, higher better smooth - static float H = 1; // Measurement Map scalar - static float Q = 10; // initial estimated convariance - static float P = 0; // initial error covariance - static float U_hat = 0; // initial estimated state - static float K = 0; // initial Kalman gain + static float R = 10; // noise convariance can be 10, higher better smooth + static float H = 1; // Measurement Map scalar + static float Q = 10; // initial estimated convariance + static float P = 0; // initial error covariance + static float U_hat = 0; // initial estimated state + static float K = 0; // initial Kalman gain - // Predict - // - K = P * H / (H * P * H + R); // Update Kalman gain - U_hat = U_hat + K * (U - H * U_hat); // Update estimated state + // Predict + // + K = P * H / (H * P * H + R); // Update Kalman gain + U_hat = U_hat + K * (U - H * U_hat); // Update estimated state - // Update error covariance - // - P = (1 - K * H) * P + Q; + // Update error covariance + // + P = (1 - K * H) * P + Q; - return U_hat; + return U_hat; } -void -echo_handler() -{ - if (gpio_get(ECHO_PIN)) - { - start_time = time_us_32(); - echo_rising = true; - } - else - { - end_time = time_us_32(); - echo_rising = false; - } -} +// void +// echo_handler() +// { +// if (gpio_get(ECHO_PIN)) +// { +// start_time = time_us_32(); +// echo_rising = true; +// } +// else +// { +// end_time = time_us_32(); +// echo_rising = false; +// } +// } void distance_task(__unused void *params) { - while (true) - { - vTaskDelay(1000); + while (true) + { + vTaskDelay(1000); - gpio_put(TRIG_PIN, 1); - sleep_us(10); - gpio_put(TRIG_PIN, 0); + gpio_put(TRIG_PIN, 1); + sleep_us(10); + gpio_put(TRIG_PIN, 0); - while (echo_rising) - { - tight_loop_contents(); - } + while (gpio_get(ECHO_PIN) == 0) + tight_loop_contents(); - // Calculate the distance (in centimeters) - uint32_t pulse_duration = end_time - start_time; - float distance = (pulse_duration * 0.034 / 2) * 1.125; // Speed of sound in cm/us + // Measure the pulse width (time taken for the echo to return) + uint32_t start_time = time_us_32(); + while (gpio_get(ECHO_PIN) == 1) + tight_loop_contents(); + uint32_t end_time = time_us_32(); - // printf("Distance: %.2f cm\n", distance); - printf("Kalman Filtered Distance: %.2f cm\n", KalmanFilter(distance)); + // Calculate the distance (in centimeters) + uint32_t pulse_duration = end_time - start_time; + float distance + = (pulse_duration * 0.034 / 2); // Speed of sound in cm/us - if (distance < 5) - { - printf("Collision Imminent!\n"); - } - } + printf("Distance: %.2f cm\n", distance); + // printf("Kalman Filtered Distance: %.2f cm\n", KalmanFilter(distance)); + + if (distance < 7) + { + // set_wheel_direction(DIRECTION_MASK); + set_wheel_speed_synced(0u); + printf("Collision Imminent!\n"); + vTaskDelay(pdMS_TO_TICKS(3000)); + spin_to_yaw(350); + set_wheel_direction(DIRECTION_FORWARD); + set_wheel_speed_synced(90u); + } + start_time, end_time = 0; + } } -#endif /* ULTRASONIC_SENSOR_H */ +#endif /* ULTRASONIC_SENSOR_H */ \ No newline at end of file From 0a62f7738c929cb4b67fc07087c333b896c169d6 Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Thu, 9 Nov 2023 11:38:20 +0800 Subject: [PATCH 21/23] use minimum global variables --- frtos/config/motor_config.h | 43 +++++++++++-------- frtos/motor/motor_direction.h | 10 ++--- frtos/motor/motor_init.h | 54 ++++++++++++------------ frtos/motor/motor_pid.h | 78 +++++++++-------------------------- frtos/motor/motor_speed.h | 14 +++---- frtos/motor/motor_test.c | 53 ++++++++++++++++++------ 6 files changed, 126 insertions(+), 126 deletions(-) diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index f22a356..dbc411e 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -13,17 +13,17 @@ #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_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD) -#define DIRECTION_BACKWARD (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_BACKWARD) -#define DIRECTION_LEFT (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_FORWARD) -#define DIRECTION_RIGHT (DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_BACKWARD) +#define DIRECTION_FORWARD (DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD) +#define DIRECTION_BACKWARD (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_BACKWARD) +#define DIRECTION_LEFT (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_FORWARD) +#define DIRECTION_RIGHT (DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_BACKWARD) -#define DIRECTION_MASK (DIRECTION_FORWARD | DIRECTION_BACKWARD) +#define DIRECTION_MASK (DIRECTION_FORWARD | DIRECTION_BACKWARD) #define SPEED_PIN_RIGHT 15U #define SPEED_PIN_LEFT 16U @@ -34,9 +34,9 @@ #define MAX_PWM_LEVEL 99U #define MIN_PWM_LEVEL 0U -#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) -#define WHEEL_CONTROL_PRIO (tskIDLE_PRIORITY + 1UL) -#define WHEEL_PID_PRIO (tskIDLE_PRIORITY + 1UL) +#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 @@ -70,6 +70,7 @@ typedef struct */ typedef struct { + bool use_pid; float kp_value; float ki_value; float kd_value; @@ -84,10 +85,10 @@ typedef struct */ typedef struct { - motor_speed_t speed; - SemaphoreHandle_t sem; - motor_pwm_t pwm; - motor_pid_t pid; + motor_speed_t speed; + motor_pwm_t pwm; + SemaphoreHandle_t *p_sem; + motor_pid_t *p_pid; } motor_t; typedef struct @@ -97,6 +98,14 @@ typedef struct volatile bool is_running; } distance_to_stop_t; -volatile bool g_use_pid = true; +SemaphoreHandle_t g_left_sem; +SemaphoreHandle_t g_right_sem; + +typedef struct +{ + motor_t *p_left_motor; + motor_t *p_right_motor; + +} car_struct_t; #endif /* MOTOR_CONFIG_H */ \ No newline at end of file diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index b4f10ff..6989302 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -65,7 +65,7 @@ check_direction(float current_direction, float target_direction, float range) } void -spin_to_yaw(float target_yaw) +spin_to_yaw(float target_yaw, car_struct_t *car_struct) { updateDirection(); float initial_yaw = g_direction.yaw; @@ -83,9 +83,9 @@ spin_to_yaw(float target_yaw) set_wheel_direction(DIRECTION_LEFT); } - set_wheel_speed_synced(80u); + set_wheel_speed_synced(80u, car_struct); - g_use_pid = false; + car_struct->p_right_motor->p_pid->use_pid = false; for (;;) { @@ -93,12 +93,12 @@ spin_to_yaw(float target_yaw) if (check_direction(g_direction.yaw, target_yaw, 1)) { set_wheel_direction(DIRECTION_MASK); - set_wheel_speed_synced(0u); + set_wheel_speed_synced(0u, car_struct); break; } } - g_use_pid = true; + car_struct->p_right_motor->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 7ac9b70..03db765 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -19,28 +19,28 @@ #include "motor_config.h" -motor_t g_motor_left = { .pwm.level = 0u, - .pwm.channel = PWM_CHAN_A, - .speed.distance_cm = 0.0f }; - -// classic ziegler nichols method -// Ku = 1000, Tu = 0.9s, interval = 0.05s -// Kp = 0.6 * Ku = 600 -// Ki = 2 * Kp * 0.05 / Tu = 66.67 -// Kd = Kp * Tu * 0.05 / 8 = 1350 -motor_t g_motor_right = { .pwm.level = 0u, - .pwm.channel = PWM_CHAN_B, - .speed.distance_cm = 0.0f, - .pid.kp_value = 600.f, - .pid.ki_value = 66.67f, - .pid.kd_value = 1350.f,}; +//motor_t g_motor_left = { .pwm.level = 0u, +// .pwm.channel = PWM_CHAN_A, +// .speed.distance_cm = 0.0f }; +// +//// classic ziegler nichols method +//// Ku = 1000, Tu = 0.9s, interval = 0.05s +//// Kp = 0.6 * Ku = 600 +//// Ki = 2 * Kp * 0.05 / Tu = 66.67 +//// Kd = Kp * Tu * 0.05 / 8 = 1350 +//motor_t g_motor_right = { .pwm.level = 0u, +// .pwm.channel = PWM_CHAN_B, +// .speed.distance_cm = 0.0f, +// .p_pid.kp_value = 600.f, +// .p_pid.ki_value = 66.67f, +// .p_pid.kd_value = 1350.f,}; void -motor_init(void) +motor_init(car_struct_t * car_struct) { // Semaphore - g_motor_left.sem = xSemaphoreCreateBinary(); - g_motor_right.sem = xSemaphoreCreateBinary(); + g_left_sem = xSemaphoreCreateBinary(); + g_right_sem = xSemaphoreCreateBinary(); gpio_init(SPEED_PIN_RIGHT); gpio_init(SPEED_PIN_LEFT); @@ -62,22 +62,24 @@ motor_init(void) gpio_set_function(PWM_PIN_LEFT, GPIO_FUNC_PWM); gpio_set_function(PWM_PIN_RIGHT, GPIO_FUNC_PWM); - g_motor_left.pwm.slice_num = pwm_gpio_to_slice_num(PWM_PIN_LEFT); - g_motor_right.pwm.slice_num = pwm_gpio_to_slice_num(PWM_PIN_RIGHT); + car_struct->p_left_motor->pwm.slice_num = pwm_gpio_to_slice_num + (PWM_PIN_LEFT); + car_struct->p_right_motor->pwm.slice_num = pwm_gpio_to_slice_num + (PWM_PIN_RIGHT); // NOTE: PWM clock is 125MHz for raspberrypi pico w by default // 125MHz / 50 = 2500kHz - pwm_set_clkdiv(g_motor_left.pwm.slice_num, PWM_CLK_DIV); - pwm_set_clkdiv(g_motor_right.pwm.slice_num, PWM_CLK_DIV); + pwm_set_clkdiv(car_struct->p_left_motor->pwm.slice_num, PWM_CLK_DIV); + pwm_set_clkdiv(car_struct->p_right_motor->pwm.slice_num, PWM_CLK_DIV); // L289N can accept up to 40kHz // 2500kHz / 100 = 25kHz - pwm_set_wrap(g_motor_left.pwm.slice_num, (PWM_WRAP - 1U)); - pwm_set_wrap(g_motor_right.pwm.slice_num, (PWM_WRAP - 1U)); + pwm_set_wrap(car_struct->p_left_motor->pwm.slice_num, (PWM_WRAP - 1U)); + pwm_set_wrap(car_struct->p_right_motor->pwm.slice_num, (PWM_WRAP - 1U)); - pwm_set_enabled(g_motor_left.pwm.slice_num, true); - pwm_set_enabled(g_motor_right.pwm.slice_num, true); + pwm_set_enabled(car_struct->p_left_motor->pwm.slice_num, true); + pwm_set_enabled(car_struct->p_right_motor->pwm.slice_num, true); } #endif /* MOTOR_INIT_H */ \ No newline at end of file diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index 706b27f..b56acd2 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -19,10 +19,10 @@ * @return The control signal */ float -compute_pid(float *integral, float *prev_error) +compute_pid(float *integral, float *prev_error, car_struct_t *car_struct) { - float error - = g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm; + float error = car_struct->p_left_motor->speed.distance_cm + - car_struct->p_right_motor->speed.distance_cm; printf("%f\n", error); @@ -30,42 +30,35 @@ compute_pid(float *integral, float *prev_error) float derivative = error - *prev_error; - float control_signal = g_motor_right.pid.kp_value * error - + g_motor_right.pid.ki_value * (*integral) - + g_motor_right.pid.kd_value * derivative; + float control_signal + = car_struct->p_right_motor->p_pid->kp_value * error + + car_struct->p_right_motor->p_pid->ki_value * (*integral) + + car_struct->p_right_motor->p_pid->kd_value * derivative; *prev_error = error; return control_signal; } -float -compute_i_controller(float *integral) -{ - float error - = g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm; - - printf("%f\n", error); - - *integral += error; - - return g_motor_right.pid.ki_value * (*integral); -} - bool -repeating_pid_handler(__unused struct repeating_timer *t) +repeating_pid_handler(struct repeating_timer *t) { + car_struct_t *car_strut = (car_struct_t *)t->user_data; + static float integral = 0.0f; static float prev_error = 0.0f; - if (!g_use_pid) + if (!car_strut->p_right_motor->p_pid->use_pid) { return true; } - float control_signal = compute_pid(&integral, &prev_error); + float control_signal = compute_pid(&integral, &prev_error, car_strut); - float temp = (float)g_motor_right.pwm.level + control_signal * 0.05f; + printf("control: %f\n", control_signal); + + float temp + = (float)car_strut->p_right_motor->pwm.level + control_signal * 0.05f; if (temp > MAX_PWM_LEVEL) { @@ -77,43 +70,12 @@ repeating_pid_handler(__unused struct repeating_timer *t) temp = MIN_PWM_LEVEL + 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("temp: %f\n", temp); - // printf("speed: %f cm/s\n", g_motor_right.speed.current_cms); - // printf("distance: %f cm\n", g_motor_right.speed.distance_cm); + set_wheel_speed((uint32_t)temp, car_strut->p_right_motor); - return true; -} - -bool -repeating_i_handler(__unused struct repeating_timer *t) -{ - static float integral = 0.0f; - - if (!g_use_pid) - { - integral = 0.0f; // reset once disabled - return true; - } - - float control_signal = compute_i_controller(&integral); - - float temp = (float)g_motor_right.pwm.level + control_signal * 0.05f; - - if (temp > MAX_PWM_LEVEL) - { - temp = MAX_PWM_LEVEL; - } - - if (temp <= MIN_PWM_LEVEL) - { - temp = MIN_PWM_LEVEL; - } - - set_wheel_speed((uint32_t)temp, &g_motor_right); + printf("speed: %f cm/s\n", car_strut->p_right_motor->speed.current_cms); + printf("distance: %f cm\n", car_strut->p_right_motor->speed.distance_cm); return true; } diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index be0072e..8c8da52 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -19,7 +19,7 @@ h_wheel_sensor_isr_handler(void) gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL); BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(g_motor_left.sem, + xSemaphoreGiveFromISR(g_left_sem, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } @@ -29,7 +29,7 @@ h_wheel_sensor_isr_handler(void) gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL); BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(g_motor_right.sem, + xSemaphoreGiveFromISR(g_right_sem, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } @@ -53,8 +53,8 @@ monitor_wheel_speed_task(void *pvParameters) for (;;) { - if ((xSemaphoreTake(p_motor->sem, pdMS_TO_TICKS(100)) - == pdTRUE) && (g_use_pid == true)) + if ((xSemaphoreTake(*p_motor->p_sem, pdMS_TO_TICKS(100)) + == pdTRUE) && (p_motor->p_pid->use_pid == true)) { curr_time = time_us_64(); elapsed_time = curr_time - prev_time; @@ -91,15 +91,15 @@ set_wheel_speed(uint32_t pwm_level, motor_t * motor) * @param pwm_level The pwm_level of the wheels, from 0 to 99 */ void -set_wheel_speed_synced(uint32_t pwm_level) +set_wheel_speed_synced(uint32_t pwm_level, car_struct_t *car_strut) { if (pwm_level > MAX_PWM_LEVEL) { pwm_level = MAX_PWM_LEVEL; } - set_wheel_speed(pwm_level, &g_motor_left); - set_wheel_speed(pwm_level, &g_motor_right); + set_wheel_speed(pwm_level, car_strut->p_left_motor); + set_wheel_speed(pwm_level, car_strut->p_right_motor); } ///*! diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index 5661284..230b854 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -5,26 +5,25 @@ #include "motor_pid.h" void -motor_control_task(__unused void *params) +motor_control_task(void *params) { + car_struct_t *car_struct = (car_struct_t *)params; for (;;) { set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed(90u, &g_motor_left); - set_wheel_speed(90u, &g_motor_right); + set_wheel_speed_synced(90u, car_struct); vTaskDelay(pdMS_TO_TICKS(10000)); revert_wheel_direction(); - set_wheel_speed(90u, &g_motor_left); - set_wheel_speed(90u, &g_motor_right); + set_wheel_speed_synced(90u, car_struct); vTaskDelay(pdMS_TO_TICKS(10000)); } } void -launch() +launch(car_struct_t *car_strut) { // isr to detect right motor slot gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true); @@ -38,8 +37,8 @@ launch() // PID timer struct repeating_timer pid_timer; - add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer); -// add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer); + add_repeating_timer_ms(-50, repeating_pid_handler, car_strut, &pid_timer); + // add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer); // Left wheel // @@ -47,7 +46,7 @@ launch() xTaskCreate(monitor_wheel_speed_task, "monitor_left_wheel_speed_task", configMINIMAL_STACK_SIZE, - (void *)&g_motor_left, + (void *)car_strut->p_left_motor, WHEEL_SPEED_PRIO, &h_monitor_left_wheel_speed_task_handle); @@ -57,7 +56,7 @@ launch() xTaskCreate(monitor_wheel_speed_task, "monitor_wheel_speed_task", configMINIMAL_STACK_SIZE, - (void *)&g_motor_right, + (void *)car_strut->p_right_motor, WHEEL_SPEED_PRIO, &h_monitor_right_wheel_speed_task_handle); @@ -66,7 +65,7 @@ launch() xTaskCreate(motor_control_task, "motor_turning_task", configMINIMAL_STACK_SIZE, - NULL, + (void *)car_strut, WHEEL_CONTROL_PRIO, &h_motor_turning_task_handle); @@ -81,9 +80,37 @@ main(void) sleep_ms(4000); printf("Test started!\n"); - motor_init(); + motor_pid_t g_pid = { + .kp_value = 600.f, + .ki_value = 66.67f, + .kd_value = 1350.f, + .use_pid = true, + }; - launch(); + motor_t g_motor_left = { + .pwm.level = 0u, + .pwm.channel = PWM_CHAN_A, + .speed.distance_cm = 0.0f, + .p_sem = &g_left_sem, + .p_pid = &g_pid, + }; + + motor_t g_motor_right = { + .pwm.level = 0u, + .pwm.channel = PWM_CHAN_B, + .speed.distance_cm = 0.0f, + .p_sem = &g_right_sem, + .p_pid = &g_pid, + }; + + car_struct_t car_struct = { + .p_left_motor = &g_motor_left, + .p_right_motor = &g_motor_right, + }; + + motor_init(&car_struct); + + launch(&car_struct); // for(;;); From e9511eb8cef9c214020e0cc67ab52ca8af6bedd8 Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Thu, 9 Nov 2023 12:57:26 +0800 Subject: [PATCH 22/23] updated vtask and init --- frtos/config/motor_config.h | 14 ++++--- frtos/motor/motor_direction.h | 4 +- frtos/motor/motor_init.h | 45 +++++++++++---------- frtos/motor/motor_pid.h | 8 ++-- frtos/motor/motor_speed.h | 2 +- frtos/motor/motor_test.c | 73 ++++++++++++----------------------- 6 files changed, 64 insertions(+), 82 deletions(-) diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index dbc411e..6aafd4f 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -85,10 +85,11 @@ typedef struct */ typedef struct { - motor_speed_t speed; - motor_pwm_t pwm; - SemaphoreHandle_t *p_sem; - motor_pid_t *p_pid; + motor_speed_t speed; + motor_pwm_t pwm; + SemaphoreHandle_t * p_sem; + bool * use_pid; + } motor_t; typedef struct @@ -103,8 +104,9 @@ SemaphoreHandle_t g_right_sem; typedef struct { - motor_t *p_left_motor; - motor_t *p_right_motor; + motor_t * p_left_motor; + motor_t * p_right_motor; + motor_pid_t * p_pid; } car_struct_t; diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index 6989302..8107d29 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -85,7 +85,7 @@ spin_to_yaw(float target_yaw, car_struct_t *car_struct) set_wheel_speed_synced(80u, car_struct); - car_struct->p_right_motor->p_pid->use_pid = false; + car_struct->p_pid->use_pid = false; for (;;) { @@ -98,7 +98,7 @@ spin_to_yaw(float target_yaw, car_struct_t *car_struct) } } - car_struct->p_right_motor->p_pid->use_pid = true; + 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 03db765..3b7aad3 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -19,29 +19,32 @@ #include "motor_config.h" -//motor_t g_motor_left = { .pwm.level = 0u, -// .pwm.channel = PWM_CHAN_A, -// .speed.distance_cm = 0.0f }; -// -//// classic ziegler nichols method -//// Ku = 1000, Tu = 0.9s, interval = 0.05s -//// Kp = 0.6 * Ku = 600 -//// Ki = 2 * Kp * 0.05 / Tu = 66.67 -//// Kd = Kp * Tu * 0.05 / 8 = 1350 -//motor_t g_motor_right = { .pwm.level = 0u, -// .pwm.channel = PWM_CHAN_B, -// .speed.distance_cm = 0.0f, -// .p_pid.kp_value = 600.f, -// .p_pid.ki_value = 66.67f, -// .p_pid.kd_value = 1350.f,}; - void -motor_init(car_struct_t * car_struct) +motor_init(car_struct_t *car_struct) { // Semaphore g_left_sem = xSemaphoreCreateBinary(); g_right_sem = xSemaphoreCreateBinary(); + car_struct->p_pid->use_pid = true; + car_struct->p_pid->kp_value = 600.f; + car_struct->p_pid->ki_value = 66.67f; + car_struct->p_pid->kd_value = 1350.f; + + // initialize the car_struct + car_struct->p_left_motor->pwm.level = 0u; + car_struct->p_left_motor->pwm.channel = PWM_CHAN_A; + car_struct->p_left_motor->speed.distance_cm = 0.0f; + car_struct->p_left_motor->p_sem = &g_left_sem; + car_struct->p_left_motor->use_pid = &car_struct->p_pid->use_pid; + + car_struct->p_right_motor->pwm.level = 0u; + car_struct->p_right_motor->pwm.channel = PWM_CHAN_B; + car_struct->p_right_motor->speed.distance_cm = 0.0f; + car_struct->p_right_motor->p_sem = &g_right_sem; + car_struct->p_right_motor->use_pid = &car_struct->p_pid->use_pid; + + // Initialize speed pins as inputs gpio_init(SPEED_PIN_RIGHT); gpio_init(SPEED_PIN_LEFT); gpio_set_dir(SPEED_PIN_RIGHT, GPIO_IN); @@ -62,10 +65,10 @@ motor_init(car_struct_t * car_struct) gpio_set_function(PWM_PIN_LEFT, GPIO_FUNC_PWM); gpio_set_function(PWM_PIN_RIGHT, GPIO_FUNC_PWM); - car_struct->p_left_motor->pwm.slice_num = pwm_gpio_to_slice_num - (PWM_PIN_LEFT); - car_struct->p_right_motor->pwm.slice_num = pwm_gpio_to_slice_num - (PWM_PIN_RIGHT); + car_struct->p_left_motor->pwm.slice_num + = pwm_gpio_to_slice_num(PWM_PIN_LEFT); + car_struct->p_right_motor->pwm.slice_num + = pwm_gpio_to_slice_num(PWM_PIN_RIGHT); // NOTE: PWM clock is 125MHz for raspberrypi pico w by default diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index b56acd2..62a9ff7 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -31,9 +31,9 @@ compute_pid(float *integral, float *prev_error, car_struct_t *car_struct) float derivative = error - *prev_error; float control_signal - = car_struct->p_right_motor->p_pid->kp_value * error - + car_struct->p_right_motor->p_pid->ki_value * (*integral) - + car_struct->p_right_motor->p_pid->kd_value * derivative; + = car_struct->p_pid->kp_value * error + + car_struct->p_pid->ki_value * (*integral) + + car_struct->p_pid->kd_value * derivative; *prev_error = error; @@ -48,7 +48,7 @@ repeating_pid_handler(struct repeating_timer *t) static float integral = 0.0f; static float prev_error = 0.0f; - if (!car_strut->p_right_motor->p_pid->use_pid) + if (!car_strut->p_pid->use_pid) { return true; } diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index 8c8da52..881bb24 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -54,7 +54,7 @@ monitor_wheel_speed_task(void *pvParameters) for (;;) { if ((xSemaphoreTake(*p_motor->p_sem, pdMS_TO_TICKS(100)) - == pdTRUE) && (p_motor->p_pid->use_pid == true)) + == pdTRUE) && (*p_motor->use_pid == true)) { curr_time = time_us_64(); elapsed_time = curr_time - prev_time; diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index 230b854..7a45243 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -23,30 +23,15 @@ motor_control_task(void *params) } void -launch(car_struct_t *car_strut) +launch(car_struct_t *car_struct, void *isr_handler) { - // 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); - - // 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); - - irq_set_enabled(IO_IRQ_BANK0, true); - - // PID timer - struct repeating_timer pid_timer; - add_repeating_timer_ms(-50, repeating_pid_handler, car_strut, &pid_timer); - // add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer); - // 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, - (void *)car_strut->p_left_motor, + (void *)car_struct->p_left_motor, WHEEL_SPEED_PRIO, &h_monitor_left_wheel_speed_task_handle); @@ -56,7 +41,7 @@ launch(car_struct_t *car_strut) xTaskCreate(monitor_wheel_speed_task, "monitor_wheel_speed_task", configMINIMAL_STACK_SIZE, - (void *)car_strut->p_right_motor, + (void *)car_struct->p_right_motor, WHEEL_SPEED_PRIO, &h_monitor_right_wheel_speed_task_handle); @@ -65,11 +50,19 @@ launch(car_struct_t *car_strut) xTaskCreate(motor_control_task, "motor_turning_task", configMINIMAL_STACK_SIZE, - (void *)car_strut, + (void *)car_struct, WHEEL_CONTROL_PRIO, &h_motor_turning_task_handle); - vTaskStartScheduler(); + // 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); + + // 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); + + irq_set_enabled(IO_IRQ_BANK0, true); } int @@ -80,39 +73,23 @@ main(void) sleep_ms(4000); printf("Test started!\n"); - motor_pid_t g_pid = { - .kp_value = 600.f, - .ki_value = 66.67f, - .kd_value = 1350.f, - .use_pid = true, - }; + motor_t g_motor_right; + motor_t g_motor_left; + motor_pid_t g_pid; - motor_t g_motor_left = { - .pwm.level = 0u, - .pwm.channel = PWM_CHAN_A, - .speed.distance_cm = 0.0f, - .p_sem = &g_left_sem, - .p_pid = &g_pid, - }; - - motor_t g_motor_right = { - .pwm.level = 0u, - .pwm.channel = PWM_CHAN_B, - .speed.distance_cm = 0.0f, - .p_sem = &g_right_sem, - .p_pid = &g_pid, - }; - - car_struct_t car_struct = { - .p_left_motor = &g_motor_left, - .p_right_motor = &g_motor_right, - }; + car_struct_t car_struct = { .p_right_motor = &g_motor_right, + .p_left_motor = &g_motor_left, + .p_pid = &g_pid }; motor_init(&car_struct); - launch(&car_struct); + launch(&car_struct, &h_wheel_sensor_isr_handler); - // for(;;); + // PID timer + struct repeating_timer pid_timer; + add_repeating_timer_ms(-50, repeating_pid_handler, &car_struct, &pid_timer); + + vTaskStartScheduler(); return (0); } \ No newline at end of file From 107a142ef68eb6843762b243d4135adfdbb5314a Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Thu, 9 Nov 2023 12:58:10 +0800 Subject: [PATCH 23/23] remove debug prints --- frtos/motor/motor_pid.h | 9 --------- 1 file changed, 9 deletions(-) diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index 62a9ff7..dd858e6 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -24,8 +24,6 @@ compute_pid(float *integral, float *prev_error, car_struct_t *car_struct) float error = car_struct->p_left_motor->speed.distance_cm - car_struct->p_right_motor->speed.distance_cm; - printf("%f\n", error); - *integral += error; float derivative = error - *prev_error; @@ -55,8 +53,6 @@ repeating_pid_handler(struct repeating_timer *t) float control_signal = compute_pid(&integral, &prev_error, car_strut); - printf("control: %f\n", control_signal); - float temp = (float)car_strut->p_right_motor->pwm.level + control_signal * 0.05f; @@ -70,13 +66,8 @@ repeating_pid_handler(struct repeating_timer *t) temp = MIN_PWM_LEVEL + 1u; } - printf("temp: %f\n", temp); - set_wheel_speed((uint32_t)temp, car_strut->p_right_motor); - printf("speed: %f cm/s\n", car_strut->p_right_motor->speed.current_cms); - printf("distance: %f cm\n", car_strut->p_right_motor->speed.distance_cm); - return true; }