devoalda
/
INF2004_Project
mirror of https://github.com/L4ncelot-R/freeRTOS-car.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'L4ncelot-R:main' into main

This commit is contained in:
OldManSteve 2023-11-09 13:02:23 +08:00 committed by GitHub
parent c959946071 107a142ef6
commit 21ef7e8892
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
26 changed files with 1463 additions and 458 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
frtos/CMakeLists.txt
View File

@ -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) 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) LWIP_SOCKET=1 # we need the socket API (generally this would be in your lwipopts.h)
PING_USE_SOCKETS=1 PING_USE_SOCKETS=1
PICO_MAX_SHARED_IRQ_HANDLERS=5
) )
target_include_directories(rtos_car PRIVATE target_include_directories(rtos_car PRIVATE
${CMAKE_CURRENT_LIST_DIR}/config ${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}/line_sensor
${CMAKE_CURRENT_LIST_DIR}/ultrasonic_sensor ${CMAKE_CURRENT_LIST_DIR}/ultrasonic_sensor
${CMAKE_CURRENT_LIST_DIR}/frontend ${CMAKE_CURRENT_LIST_DIR}/frontend
${CMAKE_CURRENT_LIST_DIR}/magnetometer
) )
target_link_libraries(rtos_car target_link_libraries(rtos_car
pico_cyw43_arch_lwip_sys_freertos pico_cyw43_arch_lwip_sys_freertos

19
frtos/barcode_sensor/CMakeLists.txt Normal file
View File

@ -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)

124
frtos/barcode_sensor/barcode_sensor.h Normal file
View File

@ -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);
}

97
frtos/barcode_sensor/barcode_sensor_init.h Normal file
View File

@ -0,0 +1,97 @@
/* Initialise the barcode sensor */
#ifndef BARCODE_SENSOR_INIT_H
#define BARCODE_SENSOR_INIT_H
#include <stdio.h>
#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 */

48
frtos/barcode_sensor/barcode_sensor_test.c Normal file
View File

@ -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);
}

1
frtos/config/line_sensor_config.h
View File

@ -7,6 +7,7 @@
#define LEFT_SENSOR_PIN ( 26 ) #define LEFT_SENSOR_PIN ( 26 )
#define RIGHT_SENSOR_PIN ( 27 ) #define RIGHT_SENSOR_PIN ( 27 )
#define BARCODE_SENSOR_PIN ( 22 )
/* Map */ /* Map */

13
frtos/config/magnetometer_config.h
View File

@ -7,14 +7,19 @@
#define DIRECTION_READ_DELAY ( 100 ) #define DIRECTION_READ_DELAY ( 100 )
#define ALPHA ( 0.01f ) // Complementary #define NUM_READINGS ( 10 ) // Number of readings to
// Filter Constant // take before
// calculating
// direction
//#define ALPHA ( 0.1f ) // Low Pass Filter
// Coefficient
// LSM303DLHC temperature compensation coefficients // LSM303DLHC temperature compensation coefficients
#define SCALE_Z ( 1.0f ) // Scale for Z-axis #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 // temperature for
// calibration // calibration

88
frtos/config/motor_config.h
View File

@ -3,39 +3,50 @@
#define MOTOR_CONFIG_H #define MOTOR_CONFIG_H
// ENA and ENB on the L298N // ENA and ENB on the L298N
#define PWM_PIN_RIGHT 1U // chanel B #define PWM_PIN_RIGHT 1U // chanel B
#define PWM_PIN_LEFT 0U // chanel A #define PWM_PIN_LEFT 0U // chanel A
// IN1, IN2, IN3, IN4 on the L298N // IN1, IN2, IN3, IN4 on the L298N
#define DIRECTION_PIN_RIGHT_IN1 11U #define DIRECTION_PIN_RIGHT_IN1 11U
#define DIRECTION_PIN_RIGHT_IN2 12U #define DIRECTION_PIN_RIGHT_IN2 12U
#define DIRECTION_PIN_LEFT_IN3 19U #define DIRECTION_PIN_LEFT_IN3 19U
#define DIRECTION_PIN_LEFT_IN4 20U #define DIRECTION_PIN_LEFT_IN4 20U
#define DIRECTION_RIGHT_FORWARD (1U << DIRECTION_PIN_RIGHT_IN2) #define DIRECTION_RIGHT_FORWARD (1U << DIRECTION_PIN_RIGHT_IN2)
#define DIRECTION_RIGHT_BACKWARD (1U << DIRECTION_PIN_RIGHT_IN1) #define DIRECTION_RIGHT_BACKWARD (1U << DIRECTION_PIN_RIGHT_IN1)
#define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4) #define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4)
#define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3) #define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3)
#define SPEED_PIN_RIGHT 15U #define DIRECTION_FORWARD (DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD)
#define SPEED_PIN_LEFT 16U #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 DIRECTION_MASK (DIRECTION_FORWARD | DIRECTION_BACKWARD)
#define PWM_WRAP 5000U
#define MAX_SPEED 4900U #define SPEED_PIN_RIGHT 15U
#define MIN_SPEED 0U // To be changed #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 * @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 current_speed_cms Current speed in cm/s
* @param distance_cm Distance travelled in cm * @param distance_cm Distance travelled in cm
*/ */
typedef struct { typedef struct
float current_cms; {
float distance_cm; float current_cms;
float distance_cm;
} motor_speed_t; } motor_speed_t;
/*! /*!
@ -44,7 +55,8 @@ typedef struct {
* @param pwm_channel PWM channel, either A or B * @param pwm_channel PWM channel, either A or B
* @param pwm_level PWM level, from 0 to 5000 * @param pwm_level PWM level, from 0 to 5000
*/ */
typedef struct { typedef struct
{
uint slice_num; uint slice_num;
uint channel; uint channel;
uint16_t level; uint16_t level;
@ -56,7 +68,9 @@ typedef struct {
* @param pid_ki Integral gain * @param pid_ki Integral gain
* @param pid_kd Derivative gain * @param pid_kd Derivative gain
*/ */
typedef struct { typedef struct
{
bool use_pid;
float kp_value; float kp_value;
float ki_value; float ki_value;
float kd_value; float kd_value;
@ -69,11 +83,31 @@ typedef struct {
* @param pwm Motor PWM parameters * @param pwm Motor PWM parameters
* @param pid Motor PID parameters * @param pid Motor PID parameters
*/ */
typedef struct { typedef struct
motor_speed_t speed; {
SemaphoreHandle_t sem; motor_speed_t speed;
motor_pwm_t pwm; motor_pwm_t pwm;
motor_pid_t pid; SemaphoreHandle_t * p_sem;
bool * use_pid;
} motor_t; } 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 */ #endif /* MOTOR_CONFIG_H */

4
frtos/config/ultrasonic_sensor_config.h
View File

@ -3,8 +3,8 @@
/* ADC Configuration */ /* ADC Configuration */
#define TRIG_PIN ( 0 ) #define TRIG_PIN ( 2 )
#define ECHO_PIN ( 1 ) #define ECHO_PIN ( 3 )
#define ULTRASONIC_SENSOR_READ_DELAY ( 500 ) #define ULTRASONIC_SENSOR_READ_DELAY ( 500 )

43
frtos/line_sensor/line_sensor.h
View File

@ -6,6 +6,7 @@
#include "line_sensor_init.h" #include "line_sensor_init.h"
/** /**
* @brief Monitor the left sensor * @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 * @brief Monitor the direction and Oritentation of the car
* *
@ -113,6 +150,7 @@ void
monitor_direction_task(__unused void *params) { monitor_direction_task(__unused void *params) {
state_t left_state; state_t left_state;
state_t right_state; state_t right_state;
state_t barcode_state;
for (;;) for (;;)
{ {
@ -126,6 +164,11 @@ monitor_direction_task(__unused void *params) {
&right_state, &right_state,
sizeof(state_t), sizeof(state_t),
portMAX_DELAY); portMAX_DELAY);
xMessageBufferReceive(barcode_sensor_msg_buffer,
&barcode_state,
sizeof(state_t),
portMAX_DELAY);
// g_car_state.current_direction = (left_state << 1) | right_state; // g_car_state.current_direction = (left_state << 1) | right_state;

103
frtos/line_sensor/line_sensor_init.h
View File

@ -23,55 +23,59 @@
static TickType_t lastEdgeTimeLeft = 0; static TickType_t lastEdgeTimeLeft = 0;
static TickType_t lastEdgeTimeRight = 0; static TickType_t lastEdgeTimeRight = 0;
static TickType_t lastBarcodeTime = 0;
typedef enum { // Unused, useful for readability typedef enum { // Unused, useful for readability
LINE_DETECTED = 0, LINE_DETECTED = 0,
LINE_NOT_DETECTED = 1, LINE_NOT_DETECTED = 1,
} state_t; } state_t;
typedef enum { //typedef enum {
ERROR = 0, // ERROR = 0,
RIGHT = 1, // RIGHT = 1,
LEFT = 2, // LEFT = 2,
FORWARD = 3 // FORWARD = 3
} direction_t; //} direction_t;
typedef enum { //typedef enum {
NORTH = 0, // NORTH = 0,
EAST = 1, // EAST = 1,
SOUTH = 2, // SOUTH = 2,
WEST = 3, // WEST = 3,
} orientation_t; //} orientation_t;
typedef struct { //typedef struct {
u_int8_t x; // Current x coordinate // uint8_t x; // Current x coordinate
u_int8_t y; // Current y coordinate // uint8_t y; // Current y coordinate
direction_t current_direction; // Current direction (forward, left, right) // direction_t current_direction; // Current direction (forward, left, right)
orientation_t orientation; // Current orientation (N, E, S, W) // orientation_t orientation; // Current orientation (N, E, S, W)
} car_state_t; //} car_state_t;
// Semaphore // Semaphore
SemaphoreHandle_t g_left_sensor_sem = NULL; SemaphoreHandle_t g_left_sensor_sem = NULL;
SemaphoreHandle_t g_right_sensor_sem = NULL; SemaphoreHandle_t g_right_sensor_sem = NULL;
SemaphoreHandle_t g_barcode_sensor_sem = NULL;
// Queue // Queue
static MessageBufferHandle_t left_sensor_msg_buffer; // Left Sensor Buffer static MessageBufferHandle_t left_sensor_msg_buffer; // Left Sensor Buffer
static MessageBufferHandle_t right_sensor_msg_buffer; // Right 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 right_sensor_triggered = pdFALSE;
static volatile BaseType_t left_sensor_triggered = pdFALSE; static volatile BaseType_t left_sensor_triggered = pdFALSE;
static volatile BaseType_t barcode_sensor_triggered = pdFALSE;
// Car State Struct //// Car State Struct
static car_state_t g_car_state; //static car_state_t g_car_state;
//
static car_state_t initialize_car_state() { //static car_state_t initialize_car_state() {
g_car_state.x = MAP_SIZE >> 1; // g_car_state.x = MAP_SIZE >> 1;
g_car_state.y = MAP_SIZE >> 1; // g_car_state.y = MAP_SIZE >> 1;
g_car_state.current_direction = FORWARD; // g_car_state.current_direction = FORWARD;
g_car_state.orientation = NORTH; // g_car_state.orientation = NORTH;
//
return g_car_state; // return g_car_state;
} //}
/** /**
* @brief Setup the Line Sensor * @brief Setup the Line Sensor
@ -82,15 +86,17 @@ static inline void
line_sensor_setup() { line_sensor_setup() {
g_left_sensor_sem = xSemaphoreCreateBinary(); g_left_sensor_sem = xSemaphoreCreateBinary();
g_right_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_init_mask(mask);
gpio_set_dir_in_masked(mask); gpio_set_dir_in_masked(mask);
left_sensor_msg_buffer = xMessageBufferCreate(30); left_sensor_msg_buffer = xMessageBufferCreate(30);
right_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; 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) { void h_line_sensor_handler(void) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE; BaseType_t xHigherPriorityTaskWoken = pdFALSE;
TickType_t currentTicks = xTaskGetTickCount(); 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); portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
} }

29
frtos/line_sensor/line_sensor_test.c
View File

@ -3,6 +3,7 @@
#define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) #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 READ_RIGHT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 3UL) #define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 3UL)
@ -10,14 +11,18 @@ void
launch() launch()
{ {
// isr to detect left line sensor // isr to detect left line sensor
// gpio_set_irq_enabled(LEFT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, 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); gpio_add_raw_irq_handler(LEFT_SENSOR_PIN, h_line_sensor_handler);
//
// // isr to detect right line sensor // isr to detect right line sensor
// gpio_set_irq_enabled(RIGHT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true); gpio_set_irq_enabled(RIGHT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true);
// gpio_add_raw_irq_handler(RIGHT_SENSOR_PIN, h_line_sensor_handler); gpio_add_raw_irq_handler(RIGHT_SENSOR_PIN, h_line_sensor_handler);
//
// irq_set_enabled(IO_IRQ_BANK0, true); // 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; struct repeating_timer g_left_sensor_timer;
add_repeating_timer_ms(LINE_SENSOR_READ_DELAY, add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,
@ -47,6 +52,14 @@ launch()
READ_RIGHT_SENSOR_PRIO, READ_RIGHT_SENSOR_PRIO,
&h_monitor_right_sensor_task); &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; // TaskHandle_t h_monitor_direction_task;
// xTaskCreate(monitor_direction_task, // xTaskCreate(monitor_direction_task,
// "Monitor Direction Task", // "Monitor Direction Task",

240
frtos/magnetometer/magnetometer_direction.h
View File

@ -24,6 +24,7 @@
#define MAGNETOMETER_DIRECTION_H #define MAGNETOMETER_DIRECTION_H
#include "magnetometer_init.h" #include "magnetometer_init.h"
#include "map.h"
/** /**
* @brief Roll Calculation with Accelerometer Data * @brief Roll Calculation with Accelerometer Data
@ -55,7 +56,7 @@ calculate_pitch(int16_t acceleration[3]) {
*/ */
static inline float static inline float
calculate_yaw_magnetometer(int16_t magnetometer[3]) { 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 * @param yaw_mag Yaw calculated from Magnetometer Data
* @return yaw Yaw calculated from Complementary Filter * @return yaw Yaw calculated from Complementary Filter
*/ */
static inline float //static inline float
calculate_yaw_complementary(float yaw_acc, float yaw_mag) { //calculate_yaw_complementary(float yaw_acc, float yaw_mag) {
return ALPHA * yaw_acc + (1 - ALPHA) * yaw_mag; // return ALPHA * yaw_acc + (1 - ALPHA) * yaw_mag;
} //}
/** /**
* @brief Compensate the magnetometer readings for temperature * @brief Compensate the magnetometer readings for temperature
@ -78,11 +79,11 @@ calculate_yaw_complementary(float yaw_acc, float yaw_mag) {
float float
compensate_magnetometer(float yaw_mag, int16_t temperature) { compensate_magnetometer(float yaw_mag, int16_t temperature) {
// Calculate temperature difference from the reference 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 // Apply temperature compensation to each axis using macros
float compensated_yaw_mag = 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 // Apply scale and offset corrections using macros
compensated_yaw_mag = (compensated_yaw_mag - OFFSET_Z) * SCALE_Z; 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 static inline float
adjust_yaw(float yaw) { 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 * the compass direction enum
* 45.0 = 360 / 8, used to calculate the compass direction from * 45.0 = 360 / 8, used to calculate the compass direction from
* the orientation (0 - 7) * the orientation (0 - 7)
* @param yaw Yaw calculated from Complementary Filter * @param yaw Yaw calculated
* @return Compass Direction * @return Compass Direction
*/ */
static inline compass_direction_t static inline compass_direction_t
calculate_compass_direction(float yaw) { calculate_compass_direction(float yaw) {
int orientation = (int) ((yaw + 22.5) / 45.0) % 8; // 8 compass directions if (yaw >= 337.5 || yaw < 22.5)
switch (orientation)
{ {
case 0: return NORTH;
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;
} }
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 * @param magnetometer Magnetometer Data
*/ */
static void static void
read_direction(int16_t acceleration[3], read_direction(int16_t acceleration[3], int16_t magnetometer[3]) {
int16_t magnetometer[3],
int16_t temperature[1]) {
float roll = calculate_roll(acceleration); float roll = calculate_roll(acceleration);
float pitch = calculate_pitch(acceleration); float pitch = calculate_pitch(acceleration);
float yaw_mag = calculate_yaw_magnetometer(magnetometer); float yaw_mag = calculate_yaw_magnetometer(magnetometer);
yaw_mag = adjust_yaw(yaw_mag);
// Apply temperature compensation to the magnetometer data // Apply temperature compensation to the magnetometer data
float compensated_mag_yaw = compensate_magnetometer(yaw_mag, // float compensated_mag_yaw = compensate_magnetometer(yaw_mag,
temperature[0]); // temperature[0]);
// compensated_mag_yaw = adjust_yaw(compensated_mag_yaw);
float yaw_acc = atan2(acceleration[1], acceleration[0]) * (180.0 / M_PI); // float yaw_acc = atan2(acceleration[1], acceleration[0]) * (180.0f / M_PI);
float yaw = calculate_yaw_complementary(yaw_acc, compensated_mag_yaw); // 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 * @param params
*/ */
void print_orientation_data() { 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.roll,
g_direction.pitch, g_direction.pitch,
g_direction.yaw 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) { void monitor_direction_task(__unused void *params) {
for (;;) for (;;)
{ {
if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE) if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE)
{ {
int16_t magnetometer[3]; updateDirection();
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);
} }
} }
} }
#endif #endif

200
frtos/magnetometer/magnetometer_init.h
View File

@ -32,13 +32,204 @@ SemaphoreHandle_t g_direction_sem = NULL;
direction_t g_direction = { direction_t g_direction = {
.roll = 0, .roll = 0,
.pitch = 0, .pitch = 0,
// .heading = 0,
.yaw = 0, .yaw = 0,
.orientation = NORTH, .orientation = NORTH,
.roll_angle = LEFT, .roll_angle = LEFT,
.pitch_angle = UP .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, &reg, 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) * @brief Initialise the LSM303DLHC sensor (Accelerometer and Magnetometer)
* @details * @details
@ -105,8 +296,12 @@ magnetometer_init()
LSM303DLHC_init(); LSM303DLHC_init();
// initial_calibration();
// sleep_ms(3000);
printf("Magnetometer Initialised\n");
// Semaphore // 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); portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
return true; return true;
} }
#endif #endif

96
frtos/magnetometer/magnetometer_read.h
View File

@ -10,101 +10,5 @@
#include "magnetometer_init.h" #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, &reg, 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 #endif

7
frtos/magnetometer/magnetometer_test.c
View File

@ -2,6 +2,7 @@
#include "magnetometer_init.h" #include "magnetometer_init.h"
#include "magnetometer_read.h" #include "magnetometer_read.h"
#include "magnetometer_direction.h" #include "magnetometer_direction.h"
#include "map.h"
#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 1UL) #define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 1UL)
@ -30,9 +31,11 @@ main (void)
{ {
stdio_usb_init(); 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(); magnetometer_init();
launch(); launch();

128
frtos/magnetometer/map.h Normal file
View File

@ -0,0 +1,128 @@
//
// Created by junwei on 31/10/23.
//
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#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

2
frtos/motor/CMakeLists.txt
View File

@ -4,12 +4,14 @@ target_link_libraries(motor_test
pico_cyw43_arch_lwip_sys_freertos pico_cyw43_arch_lwip_sys_freertos
pico_stdlib pico_stdlib
pico_lwip_iperf pico_lwip_iperf
hardware_i2c
FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap
hardware_pwm hardware_pwm
) )
target_include_directories(motor_test PRIVATE target_include_directories(motor_test PRIVATE
../config ../config
../magnetometer
) )
pico_enable_stdio_usb(motor_test 1) pico_enable_stdio_usb(motor_test 1)

93
frtos/motor/motor_direction.h
View File

@ -4,7 +4,13 @@
* @author Richie * @author Richie
*/ */
#ifndef MOTOR_DIRECTION_H
#define MOTOR_DIRECTION_H
#include "motor_init.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 * @brief Set the direction of the wheels; can use bitwise OR to set both
@ -19,28 +25,81 @@
void void
set_wheel_direction(uint32_t direction) set_wheel_direction(uint32_t direction)
{ {
static const uint32_t mask gpio_put_masked(DIRECTION_MASK, 0U);
= DIRECTION_LEFT_FORWARD | DIRECTION_LEFT_BACKWARD
| DIRECTION_RIGHT_FORWARD | DIRECTION_RIGHT_BACKWARD;
gpio_put_masked(mask, 0U);
gpio_set_mask(direction); gpio_set_mask(direction);
} }
/*! /*!
* @brief Set the speed of the wheels * @brief Set the direction of the wheel to opposite direction using bit mask
* @param speed The speed of the wheels, from 0 to 5000
*/ */
void void
set_wheel_speed(uint32_t speed) revert_wheel_direction()
{ {
g_motor_right.pwm.level = speed; uint32_t current_direction = gpio_get_all();
g_motor_left.pwm.level = speed;
pwm_set_chan_level(g_motor_right.pwm.slice_num, uint32_t reverted_direction = current_direction ^ DIRECTION_MASK;
g_motor_right.pwm.channel,
g_motor_right.pwm.level); gpio_put_masked(DIRECTION_MASK, 0U);
pwm_set_chan_level(g_motor_left.pwm.slice_num, gpio_set_mask(reverted_direction & DIRECTION_MASK);
g_motor_left.pwm.channel, }
g_motor_left.pwm.level);
} 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 */

59
frtos/motor/motor_init.h
View File

@ -19,24 +19,32 @@
#include "motor_config.h" #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 void
motor_init(void) motor_init(car_struct_t *car_struct)
{ {
// Semaphore // Semaphore
g_motor_left.sem = xSemaphoreCreateBinary(); g_left_sem = xSemaphoreCreateBinary();
g_motor_right.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_RIGHT);
gpio_init(SPEED_PIN_LEFT); gpio_init(SPEED_PIN_LEFT);
gpio_set_dir(SPEED_PIN_RIGHT, GPIO_IN); 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_LEFT, GPIO_FUNC_PWM);
gpio_set_function(PWM_PIN_RIGHT, 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); car_struct->p_left_motor->pwm.slice_num
g_motor_right.pwm.slice_num = pwm_gpio_to_slice_num(PWM_PIN_RIGHT); = 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 // 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(car_struct->p_left_motor->pwm.slice_num, PWM_CLK_DIV);
pwm_set_clkdiv(g_motor_right.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 // L289N can accept up to 40kHz
pwm_set_wrap(g_motor_left.pwm.slice_num, (PWM_WRAP - 1U)); // 2500kHz / 100 = 25kHz
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(car_struct->p_left_motor->pwm.slice_num, true);
pwm_set_enabled(g_motor_right.pwm.slice_num, true); pwm_set_enabled(car_struct->p_right_motor->pwm.slice_num, true);
} }
#endif /* MOTOR_INIT_H */ #endif /* MOTOR_INIT_H */

85
frtos/motor/motor_pid.h
View File

@ -5,7 +5,10 @@
* @author Richie * @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 * @brief Compute the control signal using PID controller
@ -16,62 +19,56 @@
* @return The control signal * @return The control signal
*/ */
float float
compute_pid(float *integral, float *prev_error) compute_pid(float *integral, float *prev_error, car_struct_t *car_struct)
{ {
float error float error = car_struct->p_left_motor->speed.distance_cm
= g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm; - car_struct->p_right_motor->speed.distance_cm;
*integral += error; *integral += error;
float derivative = error - *prev_error; float derivative = error - *prev_error;
float control_signal = g_motor_right.pid.kp_value * error float control_signal
+ g_motor_right.pid.ki_value * (*integral) = car_struct->p_pid->kp_value * error
+ g_motor_right.pid.kd_value * derivative; + car_struct->p_pid->ki_value * (*integral)
+ car_struct->p_pid->kd_value * derivative;
*prev_error = error; *prev_error = error;
return control_signal; return control_signal;
} }
void bool
motor_pid_task(__unused void *p_param) repeating_pid_handler(struct repeating_timer *t)
{ {
float integral = 0.0f; car_struct_t *car_strut = (car_struct_t *)t->user_data;
float prev_error = 0.0f;
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) return true;
{
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));
} }
}
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 */

63
frtos/motor/motor_speed.h
View File

@ -3,6 +3,8 @@
* @brief monitor and update the speed of the wheels * @brief monitor and update the speed of the wheels
* @author Richie * @author Richie
*/ */
#ifndef MOTOR_SPEED_H
#define MOTOR_SPEED_H
#include "motor_init.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); gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL);
BaseType_t xHigherPriorityTaskWoken = pdFALSE; BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_motor_left.sem, xSemaphoreGiveFromISR(g_left_sem,
&xHigherPriorityTaskWoken); &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(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); gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL);
BaseType_t xHigherPriorityTaskWoken = pdFALSE; BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_motor_right.sem, xSemaphoreGiveFromISR(g_right_sem,
&xHigherPriorityTaskWoken); &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
} }
@ -51,8 +53,8 @@ monitor_wheel_speed_task(void *pvParameters)
for (;;) for (;;)
{ {
if (xSemaphoreTake(p_motor->sem, pdMS_TO_TICKS(100)) if ((xSemaphoreTake(*p_motor->p_sem, pdMS_TO_TICKS(100))
== pdTRUE) == pdTRUE) && (*p_motor->use_pid == true))
{ {
curr_time = time_us_64(); curr_time = time_us_64();
elapsed_time = curr_time - prev_time; elapsed_time = curr_time - prev_time;
@ -72,4 +74,55 @@ monitor_wheel_speed_task(void *pvParameters)
p_motor->speed.current_cms = 0.f; p_motor->speed.current_cms = 0.f;
} }
} }
} }
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 */

79
frtos/motor/motor_test.c
View File

@ -4,55 +4,65 @@
#include "motor_direction.h" #include "motor_direction.h"
#include "motor_pid.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 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 // Left wheel
// //
TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_wheel_speed_task, xTaskCreate(monitor_wheel_speed_task,
"monitor_left_wheel_speed_task", "monitor_left_wheel_speed_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
(void *)&g_motor_left, (void *)car_struct->p_left_motor,
WHEEL_SPEED_PRIO, WHEEL_SPEED_PRIO,
&h_monitor_left_wheel_speed_task_handle); &h_monitor_left_wheel_speed_task_handle);
// Right wheel // Right wheel
// //
TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL; TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_wheel_speed_task, xTaskCreate(monitor_wheel_speed_task,
"monitor_wheel_speed_task", "monitor_wheel_speed_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
(void *)&g_motor_right, (void *)car_struct->p_right_motor,
WHEEL_SPEED_PRIO, WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle); &h_monitor_right_wheel_speed_task_handle);
TaskHandle_t h_motor_pid_right_task_handle = NULL; // control task
xTaskCreate(motor_pid_task, TaskHandle_t h_motor_turning_task_handle = NULL;
"motor_pid_task", xTaskCreate(motor_control_task,
"motor_turning_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
(void *)&g_motor_right, (void *)car_struct,
WHEEL_SPEED_PRIO, WHEEL_CONTROL_PRIO,
&h_motor_pid_right_task_handle); &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 int
@ -63,12 +73,23 @@ main(void)
sleep_ms(4000); sleep_ms(4000);
printf("Test started!\n"); printf("Test started!\n");
motor_init(); motor_t g_motor_right;
set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD); 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); return (0);
} }

168
frtos/rtos_car.c
View File

@ -14,90 +14,158 @@
#include "motor_speed.h" #include "motor_speed.h"
#include "motor_direction.h" #include "motor_direction.h"
#include "motor_pid.h"
#include "line_sensor.h" // #include "line_sensor.h"
#include "ultrasonic_sensor.h" #include "ultrasonic_sensor.h"
#define READ_LEFT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) // #define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
#define READ_RIGHT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) // #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 DISTANCE_TASK_PRIORITY (tskIDLE_PRIORITY + 4UL)
#define READ_RIGHT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 3UL)
#define DISTANCE_TASK_PRIORITY (tskIDLE_PRIORITY + 4UL)
/* Common Car State Structure (TODO: TBC)*/ /* 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 void
launch() 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; // isr for ultrasonic
add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,
h_left_sensor_timer_handler,
NULL,
&g_left_sensor_timer);
struct repeating_timer g_right_sensor_timer; // isr to detect right motor slot
add_repeating_timer_ms(LINE_SENSOR_READ_DELAY, gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true);
h_right_sensor_timer_handler, gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, h_wheel_sensor_isr_handler);
NULL,
&g_right_sensor_timer);
TaskHandle_t h_monitor_left_sensor_task; // isr to detect left motor slot
xTaskCreate(monitor_left_sensor_task, gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true);
"Monitor Left Sensor Task", 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, configMINIMAL_STACK_SIZE,
NULL, NULL,
READ_LEFT_SENSOR_PRIO, WHEEL_CONTROL_PRIO,
&h_monitor_left_sensor_task); &h_motor_turning_task_handle);
TaskHandle_t h_monitor_right_sensor_task; // ultra
xTaskCreate(monitor_right_sensor_task, TaskHandle_t disttask;
"Monitor Right Sensor Task", xTaskCreate(distance_task,
"TestDistThread",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
NULL, NULL,
READ_RIGHT_SENSOR_PRIO, 1,
&h_monitor_right_sensor_task); &disttask);
TaskHandle_t h_monitor_direction_task; // magnetometer
xTaskCreate(monitor_direction_task, // struct repeating_timer g_direction_timer;
"Monitor Direction Task", // add_repeating_timer_ms(DIRECTION_READ_DELAY,
configMINIMAL_STACK_SIZE, // h_direction_timer_handler,
NULL, // NULL,
DIRECTION_TASK_PRIORITY, // &g_direction_timer);
&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);
vTaskStartScheduler(); vTaskStartScheduler();
} }
int int
main (void) main(void)
{ {
stdio_usb_init(); stdio_usb_init();
sleep_ms(4000);
printf("Test started!\n");
motor_init(); 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(); init_ultrasonic();
printf("ultraman init");
initialize_car_state(); // TODO: Could be common functionality, To confirm // initialize_car_state(); // TODO: Could be common functionality, To
// during Integration // confirm
// during Integration
launch(); launch();
return (0); return (0);

5
frtos/ultrasonic_sensor/CMakeLists.txt
View File

@ -6,12 +6,17 @@ add_executable(
target_link_libraries( target_link_libraries(
ultrasonic_sensor ultrasonic_sensor
hardware_adc hardware_adc
pico_cyw43_arch_lwip_sys_freertos
pico_stdlib pico_stdlib
pico_lwip_iperf
hardware_i2c
FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap
hardware_pwm
) )
target_include_directories(ultrasonic_sensor target_include_directories(ultrasonic_sensor
PRIVATE ../config PRIVATE ../config
../motor
) )
pico_enable_stdio_usb(ultrasonic_sensor 1) pico_enable_stdio_usb(ultrasonic_sensor 1)

125
frtos/ultrasonic_sensor/ultrasonic_sensor.h
View File

@ -1,82 +1,95 @@
/** /**
* @file ultrasonic_sensor.h * @file ultrasonic_sensor.h
* @brief Monitor the distance between the car and the wall * @brief Monitor the distance between the car and the wall
* @author Poon Xiang Yuan * @author Poon Xiang Yuan
*/ */
#ifndef ULTRASONIC_SENSOR_H #ifndef ULTRASONIC_SENSOR_H
#define ULTRASONIC_SENSOR_H #define ULTRASONIC_SENSOR_H
#include "ultrasonic_init.h" #include "ultrasonic_init.h"
#include "motor_speed.h"
volatile uint32_t start_time; // volatile uint32_t start_time;
volatile uint32_t end_time; // volatile uint32_t end_time;
volatile bool echo_rising = false; volatile bool echo_rising = false;
float float
KalmanFilter(float U) KalmanFilter(float U)
{ {
static float R = 10; // noise convariance can be 10, higher better smooth static float R = 10; // noise convariance can be 10, higher better smooth
static float H = 1; // Measurement Map scalar static float H = 1; // Measurement Map scalar
static float Q = 10; // initial estimated convariance static float Q = 10; // initial estimated convariance
static float P = 0; // initial error covariance static float P = 0; // initial error covariance
static float U_hat = 0; // initial estimated state static float U_hat = 0; // initial estimated state
static float K = 0; // initial Kalman gain static float K = 0; // initial Kalman gain
// Predict // Predict
// //
K = P * H / (H * P * H + R); // Update Kalman gain K = P * H / (H * P * H + R); // Update Kalman gain
U_hat = U_hat + K * (U - H * U_hat); // Update estimated state U_hat = U_hat + K * (U - H * U_hat); // Update estimated state
// Update error covariance // Update error covariance
// //
P = (1 - K * H) * P + Q; P = (1 - K * H) * P + Q;
return U_hat; return U_hat;
} }
void // void
echo_handler() // echo_handler()
{ // {
if (gpio_get(ECHO_PIN)) // if (gpio_get(ECHO_PIN))
{ // {
start_time = time_us_32(); // start_time = time_us_32();
echo_rising = true; // echo_rising = true;
} // }
else // else
{ // {
end_time = time_us_32(); // end_time = time_us_32();
echo_rising = false; // echo_rising = false;
} // }
} // }
void void
distance_task(__unused void *params) distance_task(__unused void *params)
{ {
while (true) while (true)
{ {
vTaskDelay(1000); vTaskDelay(1000);
gpio_put(TRIG_PIN, 1); gpio_put(TRIG_PIN, 1);
sleep_us(10); sleep_us(10);
gpio_put(TRIG_PIN, 0); gpio_put(TRIG_PIN, 0);
while (echo_rising) while (gpio_get(ECHO_PIN) == 0)
{ tight_loop_contents();
tight_loop_contents();
}
// Calculate the distance (in centimeters) // Measure the pulse width (time taken for the echo to return)
uint32_t pulse_duration = end_time - start_time; uint32_t start_time = time_us_32();
float distance = (pulse_duration * 0.034 / 2) * 1.125; // Speed of sound in cm/us while (gpio_get(ECHO_PIN) == 1)
tight_loop_contents();
uint32_t end_time = time_us_32();
// printf("Distance: %.2f cm\n", distance); // Calculate the distance (in centimeters)
printf("Kalman Filtered Distance: %.2f cm\n", KalmanFilter(distance)); 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("Distance: %.2f cm\n", distance);
{ // printf("Kalman Filtered Distance: %.2f cm\n", KalmanFilter(distance));
printf("Collision Imminent!\n");
} 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 */