diff --git a/frtos/CMakeLists.txt b/frtos/CMakeLists.txt index 6aa36ca..28447a9 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 @@ -22,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/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 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/config/magnetometer_config.h b/frtos/config/magnetometer_config.h index 2996d79..1128376 100644 --- a/frtos/config/magnetometer_config.h +++ b/frtos/config/magnetometer_config.h @@ -7,14 +7,19 @@ #define DIRECTION_READ_DELAY ( 100 ) -#define ALPHA ( 0.01f ) // 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 -#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/config/motor_config.h b/frtos/config/motor_config.h index 267d672..6aafd4f 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -3,39 +3,50 @@ #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 SPEED_PIN_RIGHT 15U -#define SPEED_PIN_LEFT 16U +#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 PWM_CLK_DIV 250.f -#define PWM_WRAP 5000U +#define DIRECTION_MASK (DIRECTION_FORWARD | DIRECTION_BACKWARD) -#define MAX_SPEED 4900U -#define MIN_SPEED 0U // To be changed +#define SPEED_PIN_RIGHT 15U +#define SPEED_PIN_LEFT 16U + +#define PWM_CLK_DIV 50.f +#define PWM_WRAP 100U + +#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) /*! * @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; /*! @@ -44,7 +55,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; @@ -56,7 +68,9 @@ typedef struct { * @param pid_ki Integral gain * @param pid_kd Derivative gain */ -typedef struct { +typedef struct +{ + bool use_pid; float kp_value; float ki_value; float kd_value; @@ -69,11 +83,31 @@ typedef struct { * @param pwm Motor PWM parameters * @param pid Motor PID parameters */ -typedef struct { - motor_speed_t speed; - SemaphoreHandle_t sem; - motor_pwm_t pwm; - motor_pid_t pid; +typedef struct +{ + motor_speed_t speed; + motor_pwm_t pwm; + SemaphoreHandle_t * p_sem; + bool * use_pid; + } motor_t; +typedef struct +{ + float starting_distance_cm; + float distance_to_travel_cm; + volatile bool is_running; +} distance_to_stop_t; + +SemaphoreHandle_t g_left_sem; +SemaphoreHandle_t g_right_sem; + +typedef struct +{ + motor_t * p_left_motor; + motor_t * p_right_motor; + motor_pid_t * p_pid; + +} car_struct_t; + #endif /* MOTOR_CONFIG_H */ \ No newline at end of file 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.h b/frtos/line_sensor/line_sensor.h index ff6191d..9aa8970 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 * @@ -101,6 +102,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 * @@ -113,6 +150,7 @@ void monitor_direction_task(__unused void *params) { state_t left_state; state_t right_state; + state_t barcode_state; for (;;) { @@ -126,6 +164,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..b15447a 100644 --- a/frtos/line_sensor/line_sensor_init.h +++ b/frtos/line_sensor/line_sensor_init.h @@ -23,55 +23,59 @@ static TickType_t lastEdgeTimeLeft = 0; static TickType_t lastEdgeTimeRight = 0; +static TickType_t lastBarcodeTime = 0; typedef enum { // Unused, useful for readability LINE_DETECTED = 0, 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; 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; - -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 @@ -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 eff8ddf..a60e625 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) @@ -10,14 +11,18 @@ 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 right line sensor -// gpio_set_irq_enabled(RIGHT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true); -// gpio_add_raw_irq_handler(RIGHT_SENSOR_PIN, h_line_sensor_handler); -// -// irq_set_enabled(IO_IRQ_BANK0, true); + 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 right line sensor + 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; add_repeating_timer_ms(LINE_SENSOR_READ_DELAY, @@ -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/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h index c7d7189..e21ca74 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); } /** @@ -64,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 @@ -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,17 @@ 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; + } + + if (yaw > 360) + { + yaw -= 360; + } + + return yaw; } /** @@ -106,33 +117,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; +// } } /** @@ -161,26 +225,33 @@ 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); 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); } /** @@ -192,7 +263,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 @@ -251,34 +323,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 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..f704de8 100644 --- a/frtos/magnetometer/magnetometer_init.h +++ b/frtos/magnetometer/magnetometer_init.h @@ -32,13 +32,204 @@ SemaphoreHandle_t g_direction_sem = NULL; direction_t g_direction = { .roll = 0, .pitch = 0, -// .heading = 0, .yaw = 0, .orientation = NORTH, .roll_angle = LEFT, .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 @@ -105,8 +296,12 @@ magnetometer_init() LSM303DLHC_init(); +// initial_calibration(); + +// sleep_ms(3000); + printf("Magnetometer Initialised\n"); // Semaphore - g_direction_sem = xSemaphoreCreateBinary(); + // g_direction_sem = xSemaphoreCreateBinary(); } /** @@ -123,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 fde4b0b..8f8911b 100644 --- a/frtos/magnetometer/magnetometer_read.h +++ b/frtos/magnetometer/magnetometer_read.h @@ -10,101 +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 - * @param magnetometer Magnetometer Data - */ -static inline void -read_magnetometer(int16_t magnetometer[3]) { - uint8_t buffer[6]; - - 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 - - magnetometer[1] = (int16_t) (buffer[2] << 8 | buffer[3]); //yMag - - magnetometer[2] = (int16_t) (buffer[4] << 8 | buffer[5]); //zMag -} - -/** - * @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 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 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 f81273f..8107d29 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -4,7 +4,13 @@ * @author Richie */ +#ifndef MOTOR_DIRECTION_H +#define MOTOR_DIRECTION_H + #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 @@ -19,28 +25,81 @@ 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 speed of the wheels - * @param speed The speed of the wheels, from 0 to 5000 + * @brief Set the direction of the wheel to opposite direction using bit mask */ void -set_wheel_speed(uint32_t speed) +revert_wheel_direction() { - g_motor_right.pwm.level = speed; - g_motor_left.pwm.level = speed; + uint32_t current_direction = gpio_get_all(); - 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 + uint32_t reverted_direction = current_direction ^ DIRECTION_MASK; + + gpio_put_masked(DIRECTION_MASK, 0U); + gpio_set_mask(reverted_direction & DIRECTION_MASK); +} + +bool +check_direction(float current_direction, float target_direction, float range) +{ + // Normalize directions to be within 0 to 360 degrees + current_direction = fmod(current_direction, 360.0f); + if (current_direction < 0) + current_direction += 360.0f; + + 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)) + { + return true; + } + return false; +} + +void +spin_to_yaw(float target_yaw, car_struct_t *car_struct) +{ + 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_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_LEFT); + } + + set_wheel_speed_synced(80u, car_struct); + + car_struct->p_pid->use_pid = false; + + for (;;) + { + updateDirection(); + if (check_direction(g_direction.yaw, target_yaw, 1)) + { + set_wheel_direction(DIRECTION_MASK); + set_wheel_speed_synced(0u, car_struct); + break; + } + } + + car_struct->p_pid->use_pid = true; + vTaskDelay(pdMS_TO_TICKS(50)); +} + +#endif /* MOTOR_DIRECTION_H */ \ No newline at end of file diff --git a/frtos/motor/motor_init.h b/frtos/motor/motor_init.h index 56cb390..3b7aad3 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -19,24 +19,32 @@ #include "motor_config.h" -motor_t g_motor_left = { .pwm.level = 0u, - .pwm.channel = PWM_CHAN_A, - .speed.distance_cm = 0.0f }; - -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,}; - 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(); + 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); @@ -57,21 +65,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 / 250 = 500kHz - pwm_set_clkdiv(g_motor_left.pwm.slice_num, PWM_CLK_DIV); - pwm_set_clkdiv(g_motor_right.pwm.slice_num, PWM_CLK_DIV); + // 125MHz / 50 = 2500kHz + 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); - // have them to be 500kHz / 5000 = 100Hz - pwm_set_wrap(g_motor_left.pwm.slice_num, (PWM_WRAP - 1U)); - pwm_set_wrap(g_motor_right.pwm.slice_num, (PWM_WRAP - 1U)); + // L289N can accept up to 40kHz + // 2500kHz / 100 = 25kHz + 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 c741ef6..dd858e6 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -5,7 +5,10 @@ * @author Richie */ -#include "motor_init.h" +#ifndef MOTOR_PID_H +#define MOTOR_PID_H + +// #include "magnetometer_init.h" /*! * @brief Compute the control signal using PID controller @@ -16,62 +19,56 @@ * @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; *integral += 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_pid->kp_value * error + + car_struct->p_pid->ki_value * (*integral) + + car_struct->p_pid->kd_value * derivative; *prev_error = error; return control_signal; } -void -motor_pid_task(__unused void *p_param) +bool +repeating_pid_handler(struct repeating_timer *t) { - float integral = 0.0f; - float prev_error = 0.0f; + car_struct_t *car_strut = (car_struct_t *)t->user_data; - for (;;) + static float integral = 0.0f; + static float prev_error = 0.0f; + + if (!car_strut->p_pid->use_pid) { - 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; } -} \ No newline at end of file + + float control_signal = compute_pid(&integral, &prev_error, car_strut); + + float temp + = (float)car_strut->p_right_motor->pwm.level + control_signal * 0.05f; + + if (temp > MAX_PWM_LEVEL) + { + temp = MAX_PWM_LEVEL; + } + + if (temp <= MIN_PWM_LEVEL) + { + temp = MIN_PWM_LEVEL + 1u; + } + + set_wheel_speed((uint32_t)temp, car_strut->p_right_motor); + + return true; +} + +#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 520fbee..881bb24 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" @@ -17,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); } @@ -27,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); } @@ -51,8 +53,8 @@ monitor_wheel_speed_task(void *pvParameters) for (;;) { - if (xSemaphoreTake(p_motor->sem, pdMS_TO_TICKS(100)) - == pdTRUE) + if ((xSemaphoreTake(*p_motor->p_sem, pdMS_TO_TICKS(100)) + == pdTRUE) && (*p_motor->use_pid == true)) { curr_time = time_us_64(); elapsed_time = curr_time - prev_time; @@ -72,4 +74,55 @@ 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, car_struct_t *car_strut) +{ + if (pwm_level > MAX_PWM_LEVEL) + { + pwm_level = MAX_PWM_LEVEL; + } + + set_wheel_speed(pwm_level, car_strut->p_left_motor); + set_wheel_speed(pwm_level, car_strut->p_right_motor); +} + +///*! +// * @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; +//} + +#endif /* MOTOR_SPEED_H */ diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index a5f16ff..7a45243 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -4,55 +4,65 @@ #include "motor_direction.h" #include "motor_pid.h" -#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) +void +motor_control_task(void *params) +{ + car_struct_t *car_struct = (car_struct_t *)params; + for (;;) + { + set_wheel_direction(DIRECTION_FORWARD); + set_wheel_speed_synced(90u, car_struct); + + vTaskDelay(pdMS_TO_TICKS(10000)); + + revert_wheel_direction(); + set_wheel_speed_synced(90u, car_struct); + + vTaskDelay(pdMS_TO_TICKS(10000)); + } +} void -launch() +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); - - // Set wheel speed - set_wheel_speed(3500); - // 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 *)&g_motor_left, + (void *)car_struct->p_left_motor, WHEEL_SPEED_PRIO, &h_monitor_left_wheel_speed_task_handle); - // 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, + (void *)car_struct->p_right_motor, 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", + // control task + TaskHandle_t h_motor_turning_task_handle = NULL; + xTaskCreate(motor_control_task, + "motor_turning_task", configMINIMAL_STACK_SIZE, - (void *)&g_motor_right, - WHEEL_SPEED_PRIO, - &h_motor_pid_right_task_handle); + (void *)car_struct, + WHEEL_CONTROL_PRIO, + &h_motor_turning_task_handle); + // isr to detect right motor slot + gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true); + gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, isr_handler); - vTaskStartScheduler(); + // 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 @@ -63,12 +73,23 @@ main(void) sleep_ms(4000); printf("Test started!\n"); - motor_init(); - set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD); + motor_t g_motor_right; + motor_t g_motor_left; + motor_pid_t g_pid; - launch(); + car_struct_t car_struct = { .p_right_motor = &g_motor_right, + .p_left_motor = &g_motor_left, + .p_pid = &g_pid }; - // for(;;); + motor_init(&car_struct); + + launch(&car_struct, &h_wheel_sensor_isr_handler); + + // 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 diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c index f7989b3..2d0da53 100644 --- a/frtos/rtos_car.c +++ b/frtos/rtos_car.c @@ -14,90 +14,158 @@ #include "motor_speed.h" #include "motor_direction.h" +#include "motor_pid.h" -#include "line_sensor.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 DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 3UL) -#define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) -#define READ_RIGHT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) - -#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) +{ + vTaskDelay(1000); + +// set_wheel_direction(DIRECTION_FORWARD); +// set_wheel_speed_synced(90); +// vTaskDelay(1000); + +// 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); - 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 for ultrasonic - 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 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); - TaskHandle_t h_monitor_left_sensor_task; - xTaskCreate(monitor_left_sensor_task, - "Monitor Left Sensor Task", + // 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); + + // // 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); + + // 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 + // + TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; + xTaskCreate(monitor_wheel_speed_task, + "monitor_left_wheel_speed_task", + configMINIMAL_STACK_SIZE, + (void *)&g_motor_left, + WHEEL_SPEED_PRIO, + &h_monitor_left_wheel_speed_task_handle); + + // 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, - READ_LEFT_SENSOR_PRIO, - &h_monitor_left_sensor_task); + WHEEL_CONTROL_PRIO, + &h_motor_turning_task_handle); - TaskHandle_t h_monitor_right_sensor_task; - xTaskCreate(monitor_right_sensor_task, - "Monitor Right Sensor Task", + // ultra + TaskHandle_t disttask; + xTaskCreate(distance_task, + "TestDistThread", configMINIMAL_STACK_SIZE, NULL, - READ_RIGHT_SENSOR_PRIO, - &h_monitor_right_sensor_task); + 1, + &disttask); - TaskHandle_t h_monitor_direction_task; - xTaskCreate(monitor_direction_task, - "Monitor Direction 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); + // magnetometer + // struct repeating_timer g_direction_timer; + // add_repeating_timer_ms(DIRECTION_READ_DELAY, + // h_direction_timer_handler, + // NULL, + // &g_direction_timer); vTaskStartScheduler(); } int -main (void) +main(void) { stdio_usb_init(); + sleep_ms(4000); + printf("Test started!\n"); + motor_init(); + printf("motor init"); - set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD); + magnetometer_init(); + printf("magnet init"); - line_sensor_setup(); + // line_sensor_setup(); init_ultrasonic(); + printf("ultraman init"); - initialize_car_state(); // TODO: Could be common functionality, To confirm - // during Integration + // 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