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

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 RIGHT_SENSOR_PIN ( 27 )
#define BARCODE_SENSOR_PIN ( 22 )
/* Map */

13
frtos/config/magnetometer_config.h
View File

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

56
frtos/config/motor_config.h
View File

@ -18,22 +18,33 @@
#define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4)
#define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3)
#define DIRECTION_FORWARD (DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD)
#define DIRECTION_BACKWARD (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_BACKWARD)
#define DIRECTION_LEFT (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_FORWARD)
#define DIRECTION_RIGHT (DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_BACKWARD)
#define DIRECTION_MASK (DIRECTION_FORWARD | DIRECTION_BACKWARD)
#define SPEED_PIN_RIGHT 15U
#define SPEED_PIN_LEFT 16U
#define PWM_CLK_DIV 250.f
#define PWM_WRAP 5000U
#define PWM_CLK_DIV 50.f
#define PWM_WRAP 100U
#define MAX_SPEED 4900U
#define MIN_SPEED 0U // To be changed
#define MAX_PWM_LEVEL 99U
#define MIN_PWM_LEVEL 0U
#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
#define WHEEL_CONTROL_PRIO (tskIDLE_PRIORITY + 1UL)
#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 {
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 {
typedef struct
{
motor_speed_t speed;
SemaphoreHandle_t sem;
motor_pwm_t pwm;
motor_pid_t pid;
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 */

4
frtos/config/ultrasonic_sensor_config.h
View File

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

43
frtos/line_sensor/line_sensor.h
View File

@ -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 (;;)
{
@ -127,6 +165,11 @@ monitor_direction_task(__unused void *params) {
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;
// switch (g_car_state.current_direction)

103
frtos/line_sensor/line_sensor_init.h
View File

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

29
frtos/line_sensor/line_sensor_test.c
View File

@ -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",

218
frtos/magnetometer/magnetometer_direction.h
View File

@ -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;
}
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 monitor_direction_task(__unused void *params) {
for (;;)
{
if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE)
{
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);
read_direction(accelerometer, magnetometer);
// Temperature in degrees Celsius
printf("Temperature: %d\n", temperature[0]);
// printf("Temperature: %d\n", temperature[0]);
print_orientation_data();
printf("Direction: ");
// printf("Direction: ");
print_direction(g_direction.orientation);
// print_direction(g_direction.orientation);
print_roll_and_pitch(g_direction.roll_angle,
g_direction.pitch_angle);
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)
{
updateDirection();
}
}
}
#endif

200
frtos/magnetometer/magnetometer_init.h
View File

@ -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, &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)
* @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

96
frtos/magnetometer/magnetometer_read.h
View File

@ -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, &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

7
frtos/magnetometer/magnetometer_test.c
View File

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

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

91
frtos/motor/motor_direction.h
View File

@ -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);
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 */

59
frtos/motor/motor_init.h
View File

@ -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 */

63
frtos/motor/motor_pid.h
View File

@ -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;
return true;
}
float control_signal = compute_pid(&integral, &prev_error);
float control_signal = compute_pid(&integral, &prev_error, car_strut);
float temp = (float) g_motor_right.pwm.level + control_signal * 0.05f;
float temp
= (float)car_strut->p_right_motor->pwm.level + control_signal * 0.05f;
if (temp > MAX_SPEED)
if (temp > MAX_PWM_LEVEL)
{
temp = MAX_SPEED;
temp = MAX_PWM_LEVEL;
}
if (temp < MIN_SPEED)
if (temp <= MIN_PWM_LEVEL)
{
temp = MIN_SPEED;
temp = MIN_PWM_LEVEL + 1u;
}
g_motor_right.pwm.level = (uint16_t) temp;
set_wheel_speed((uint32_t)temp, car_strut->p_right_motor);
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;
}
#endif /* MOTOR_PID_H */

61
frtos/motor/motor_speed.h
View File

@ -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;
@ -73,3 +75,54 @@ monitor_wheel_speed_task(void *pvParameters)
}
}
}
void
set_wheel_speed(uint32_t pwm_level, motor_t * motor)
{
motor->pwm.level = pwm_level;
pwm_set_chan_level(motor->pwm.slice_num,
motor->pwm.channel,
motor->pwm.level);
}
/*!
* @brief Set the speed of the wheels
* @param pwm_level The pwm_level of the wheels, from 0 to 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_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);
}

164
frtos/rtos_car.c
View File

@ -14,89 +14,157 @@
#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 DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 3UL)
// #define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
// #define READ_RIGHT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
// #define READ_BARCODE_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
//
// #define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 3UL)
#define DISTANCE_TASK_PRIORITY (tskIDLE_PRIORITY + 4UL)
/* 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",
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",
configMINIMAL_STACK_SIZE,
NULL,
DIRECTION_TASK_PRIORITY,
&h_monitor_direction_task);
TaskHandle_t h_monitor_distance_task;
// ultra
TaskHandle_t disttask;
xTaskCreate(distance_task,
"Monitor Distance Task",
"TestDistThread",
configMINIMAL_STACK_SIZE,
NULL,
DISTANCE_TASK_PRIORITY,
&h_monitor_distance_task);
1,
&disttask);
// magnetometer
// struct repeating_timer g_direction_timer;
// add_repeating_timer_ms(DIRECTION_READ_DELAY,
// h_direction_timer_handler,
// NULL,
// &g_direction_timer);
vTaskStartScheduler();
}
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
// initialize_car_state(); // TODO: Could be common functionality, To
// confirm
// during Integration
launch();

5
frtos/ultrasonic_sensor/CMakeLists.txt
View File

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

67
frtos/ultrasonic_sensor/ultrasonic_sensor.h
View File

@ -1,16 +1,17 @@
/**
* @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 uint32_t start_time;
// volatile uint32_t end_time;
volatile bool echo_rising = false;
float
@ -35,20 +36,20 @@ KalmanFilter(float U)
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)
@ -61,22 +62,34 @@ distance_task(__unused void *params)
sleep_us(10);
gpio_put(TRIG_PIN, 0);
while (echo_rising)
{
while (gpio_get(ECHO_PIN) == 0)
tight_loop_contents();
}
// 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();
// 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
float distance
= (pulse_duration * 0.034 / 2); // Speed of sound in cm/us
// printf("Distance: %.2f cm\n", distance);
printf("Kalman Filtered Distance: %.2f cm\n", KalmanFilter(distance));
printf("Distance: %.2f cm\n", distance);
// printf("Kalman Filtered Distance: %.2f cm\n", KalmanFilter(distance));
if (distance < 5)
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 */