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-25 12:11:57 +08:00 committed by GitHub
parent 7900df09e0 bb9450a5aa
commit 9ee17b5a98
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
18 changed files with 347 additions and 298 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
README.md
View File

@ -44,6 +44,9 @@ Motor module consist of 4 components:
## Line Sensor
The Line sensor package is in `frtos/line_sensor`, and its configuration is in `frtos/config/line_sensor_config.h`. It
contains the drivers and FreeRTOS tasks to read the line sensor data and update the car's obstruction struct.
## Magnetometer
The magnetometer used is the [LSM303DLHC](https://www.st.com/resource/en/datasheet/lsm303dlhc.pdf) from STMicroelectronics.

108
frtos/barcode_sensor/barcode_sensor.h
View File

@ -1,6 +1,7 @@
/* Barcode sensor */
#include "barcode_sensor_init.h"
#include <string.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
@ -8,6 +9,9 @@
// Define the barcode sensor timer
static struct repeating_timer barcode_sensor_timer;
// Define the barcode data array
static uint32_t barcode_data_array[MAX_BARCODES];
static int barcode_data_index = 0;
/**
* @brief Decode a Code 39 barcode
@ -15,9 +19,9 @@ static struct repeating_timer barcode_sensor_timer;
* 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
* @return Decoded value as a character
*/
int code39_decode(uint32_t barcode_data) {
char code39_decode(uint32_t barcode_data) {
// Define the binary representations of Code 39 characters
const uint32_t code39_characters[] = {
0b001001001, // 0
@ -30,18 +34,50 @@ int code39_decode(uint32_t barcode_data) {
0b001010011, // 7
0b001011101, // 8
0b001111001, // 9
// Add more character representations as needed
0b001010101, // A
0b001010111, // B
0b001011101, // C
0b001101101, // D
0b001110101, // E
0b001110111, // F
0b001111101, // G
0b001101011, // H
0b001011011, // I
0b001010011, // J
0b001101111, // K
0b001111011, // L
0b001110011, // M
0b001110001, // N
0b001101001, // O
0b001011001, // P
0b001010001, // Q
0b001001101, // R
0b001001111, // S
0b001001011, // T
0b001111011, // U
0b001111101, // V
0b001110101, // W
0b001010111, // X
0b001011111, // Y
0b001101101, // Z
0b001110011, // -
0b001011001, // .
0b001101111, // space
0b001111111, // $
0b001010011, // /
0b001010001, // +
0b001001001, // %
};
// Compare the barcode data to known Code 39 character representations
for (int i = 0; i < 10; i++) {
for (int i = 0; i < 44; i++) {
if (barcode_data == code39_characters[i]) {
return i; // Return the decoded value (0-9)
return (char)(i + 48); // Return the decoded value as a character
}
}
// If the barcode data does not match any known character, return -1 to indicate an error
return -1;
// If the barcode data does not match any known character, return '?' to indicate an error
return '?';
}
@ -61,7 +97,6 @@ void monitor_barcode_sensor_task(void *params) {
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++) {
@ -78,23 +113,39 @@ void monitor_barcode_sensor_task(void *params) {
bar_width = 0; // Reset the bar width measurement
}
barcode_data |= (1u << i);
barcode_data_array[barcode_data_index] |= (1u << i);
}
}
printf("Barcode Data (binary): %09b\n", barcode_data);
printf("Barcode Data (binary): %09b\n", barcode_data_array[barcode_data_index]);
// Decode the barcode data
int decoded_value = code39_decode(barcode_data);
barcode_data_index++;
if (decoded_value != -1) {
printf("Decoded Value: %d\n", decoded_value);
// Store or process the decoded value as needed
if (barcode_data_index >= MAX_BARCODES) {
// Decode the barcode data
for (int i = 0; i < MAX_BARCODES; i++) {
int ones_count = 0;
uint32_t barcode_data = barcode_data_array[i];
// 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");
// Count the number of ones in the barcode data
for (int j = 0; j < 9; j++) {
if ((barcode_data >> j) & 1) {
ones_count++;
}
}
char decoded_value = code39_decode(barcode_data);
printf("Decoded Value: %c\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(char), 0);
}
// Reset the barcode data array and index
memset(barcode_data_array, 0, sizeof(barcode_data_array));
barcode_data_index = 0;
}
// Reset the flag
@ -103,22 +154,3 @@ void monitor_barcode_sensor_task(void *params) {
}
}
}
/**
* @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);
}

4
frtos/barcode_sensor/barcode_sensor_init.h
View File

@ -2,6 +2,7 @@
#ifndef BARCODE_SENSOR_INIT_H
#define BARCODE_SENSOR_INIT_H
#define DEBOUNCE_DELAY_MS 100
#include <stdio.h>
#include "pico/stdlib.h"
@ -13,9 +14,10 @@
#include "message_buffer.h"
#include "semphr.h"
#include "barcode_sensor_config.h"
#include "line_sensor_config.h"
#include "line_sensor_init.h"
// Set barcode time to 0
static TickType_t lastBarcodeTime = 0;

2
frtos/barcode_sensor/barcode_sensor_test.c
View File

@ -40,7 +40,7 @@ main (void)
printf("Test started!\n");
barcode_sensor_setup();
initialize_car_state();
// initialize_car_state();
launch();

9
frtos/config/line_sensor_config.h
View File

@ -3,9 +3,10 @@
/* ADC Configuration */
#define LINE_SENSOR_READ_DELAY ( 100 )
#define LINE_SENSOR_READ_DELAY (100)
#define LEFT_SENSOR_PIN ( 26 )
#define RIGHT_SENSOR_PIN ( 27 )
#define LEFT_SENSOR_PIN (26)
#define RIGHT_SENSOR_PIN (27)
#define BARCODE_SENSOR_PIN (22)
#endif //CONFIG_H
#endif // CONFIG_H

35
frtos/line_sensor/line_sensor_init.h
View File

@ -31,7 +31,8 @@ SemaphoreHandle_t g_left_sensor_sem = NULL;
static inline void
line_sensor_init(car_struct_t *p_car_struct)
{
p_car_struct->obs->left_sensor_detected, p_car_struct->obs->left_sensor_detected = false;
p_car_struct->obs->left_sensor_detected,
p_car_struct->obs->left_sensor_detected = false;
g_left_sensor_sem = xSemaphoreCreateBinary();
@ -40,7 +41,6 @@ line_sensor_init(car_struct_t *p_car_struct)
// Initialise 3 GPIO pins and set them to input
gpio_init_mask(mask);
gpio_set_dir_in_masked(mask);
}
/**
@ -48,35 +48,37 @@ line_sensor_init(car_struct_t *p_car_struct)
* @param rt
* @return True (To keep the timer running)
*/
bool h_left_sensor_timer_handler(repeating_timer_t *repeatingTimer) {
bool
h_left_sensor_timer_handler(repeating_timer_t *repeatingTimer)
{
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_left_sensor_sem,
&xHigherPriorityTaskWoken);
xSemaphoreGiveFromISR(g_left_sensor_sem, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
return true;
}
void
monitor_line_sensor_task(void *pvParameters) {
monitor_line_sensor_task(void *pvParameters)
{
volatile obs_t *p_obs = NULL;
p_obs = (obs_t *) pvParameters;
p_obs = (obs_t *)pvParameters;
for (;;)
{
// if (xSemaphoreTake(g_left_sensor_sem, portMAX_DELAY) == pdTRUE)
// {
// Set the flag to notify the task
p_obs->left_sensor_detected = gpio_get(LEFT_SENSOR_PIN);
p_obs->right_sensor_detected = gpio_get(RIGHT_SENSOR_PIN);
// printf("Left Sensor: %d\n", p_obs->line_detected);
vTaskDelay(pdMS_TO_TICKS(LINE_SENSOR_READ_DELAY));
// }
// Set the flag to notify the task
p_obs->left_sensor_detected = gpio_get(LEFT_SENSOR_PIN);
p_obs->right_sensor_detected = gpio_get(RIGHT_SENSOR_PIN);
vTaskDelay(pdMS_TO_TICKS(LINE_SENSOR_READ_DELAY));
}
}
/**
* @brief Initialise the tasks for the line sensor
* @param car_struct The car struct
*/
void
line_tasks_init(car_struct_t *car_struct)
{
@ -89,5 +91,4 @@ line_tasks_init(car_struct_t *car_struct)
&h_monitor_line_sensor_task);
}
#endif /* LINE_SENSOR_INIT_H */

3
frtos/line_sensor/line_sensor_test.c
View File

@ -2,7 +2,6 @@
#include "line_sensor_init.h"
#include "car_config.h"
int
main(void)
{
@ -10,7 +9,7 @@ main(void)
obs_t obs;
car_struct_t car_struct = {.obs = &obs};
car_struct_t car_struct = { .obs = &obs };
sleep_ms(2000);

100
frtos/line_sensor/line_sensor_unused.c
View File

@ -1,100 +0,0 @@
#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/adc.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"
#include "line_sensor.h"
#include "string.h"
//const float conversionFactor = 3.3f / (1 << 12);
volatile u_int8_t map[MAP_SIZE][MAP_SIZE] = {0};
/**
* @brief Update the map based on the car's state
*
* @param car_state The current car state
*/
static inline void
update_map(car_state_t car_state) {
if (car_state.x >= 0 && car_state.x < MAP_SIZE &&
car_state.y >= 0 && car_state.y < MAP_SIZE) {
map[car_state.x][car_state.y] = 1;
}
}
/**
* @brief Handle forward movement of the car
*
* @param car_state The current car state
*/
static void
handle_forward_movement(car_state_t *car_state) {
printf("FORWARD, ");
// TODO: Check car's actual forward movement
switch (car_state->orientation) {
case NORTH:
printf("NORTH\n");
car_state->y++;
break;
case EAST:
printf("EAST\n");
car_state->x++;
break;
case SOUTH:
printf("SOUTH\n");
car_state->y--;
break;
case WEST:
printf("WEST\n");
car_state->x--;
break;
}
}
/**
* @brief Handle a right turn of the car
*
* Note: Bitwise AND with 0x03 to ensure that the orientation
* is always between 0 and 3
* @param car_state The current car state
*/
static inline void
handle_right_turn(car_state_t *car_state) {
car_state->orientation = (car_state->orientation + 1) & 0x03;
}
/**
* @brief Handle a left turn of the car
*
* @param car_state The current car state
*/
static inline void
handle_left_turn(car_state_t *car_state) {
car_state->orientation = (car_state->orientation - 1) & 0x03;
}
/**
* @brief Print the map to the console
*
* This function will print the map to the console
*/
void
print_map() {
// Invert the map, 0,0 is at the bottom left
for (int i = MAP_SIZE - 1; i >= 0; i --)
{
for (int j = 0; j < MAP_SIZE; j ++)
{
printf("%d ", map[j][i]);
}
printf("\n");
}
}

2
frtos/magnetometer/LSM303DLHC_register.h
View File

@ -28,8 +28,6 @@
#define LSM303_TEMP_OUT_H_M 0x31
#define LSM303_TEMP_OUT_L_M 0x32
#define ACCEL_ADDR 0x19
#define MAG_ADDR 0x1E

69
frtos/magnetometer/magnetometer_direction.h
View File

@ -9,11 +9,15 @@
* The yaw is calculated using the magnetometer and accelerometer data
* The roll, pitch and yaw are combined to calculate the direction
* of the car with a complementary filter and compensating for the
* temperature.
* temperature. (existing)
*
* The complementary filter is used to combine the accelerometer
* and magnetometer data (yaw) to calculate the direction of the car
*
* Complementary Filter was used previously, but it was not accurate
* enough. The yaw calculated from the magnetometer data was used
* instead.
*
* Source:
* https://www.nxp.com/docs/en/application-note/AN3461.pdf
* https://ahrs.readthedocs.io/en/latest/filters/complementary.html
@ -80,7 +84,8 @@ calculate_yaw_magnetometer(int16_t magnetometer[3])
* @return Compensated Yaw
*/
float
compensate_magnetometer(float yaw_mag, int16_t temperature) {
compensate_magnetometer(float yaw_mag, int16_t temperature)
{
// Calculate temperature difference from the reference temperature
uint delta_temp = temperature - TEMPERATURE_OFFSET;
@ -190,11 +195,11 @@ calculate_compass_direction(float yaw)
* @param compass_direction Compass Direction
*/
static inline void
update_orientation_data(float roll,
float pitch,
float yaw,
compass_direction_t compass_direction,
volatile direction_t *g_direction)
update_orientation_data(float roll,
float pitch,
float yaw,
compass_direction_t compass_direction,
volatile direction_t *g_direction)
{
g_direction->roll = roll;
g_direction->roll_angle = (roll > 0) ? LEFT : RIGHT;
@ -211,8 +216,8 @@ update_orientation_data(float roll,
* @param magnetometer Magnetometer Data
*/
static void
read_direction(int16_t acceleration[3],
int16_t magnetometer[3],
read_direction(int16_t acceleration[3],
int16_t magnetometer[3],
volatile direction_t *g_direction)
{
@ -301,7 +306,7 @@ print_roll_and_pitch(angle_t roll_angle, angle_t pitch_angle)
}
void
updateDirection(volatile direction_t * g_direction)
updateDirection(volatile direction_t *g_direction)
{
int16_t magnetometer[3];
int16_t accelerometer[3];
@ -317,53 +322,13 @@ updateDirection(volatile direction_t * g_direction)
read_direction(accelerometer, magnetometer, g_direction);
print_orientation_data(*g_direction);
//
// 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(void *pvParameters)
{
volatile direction_t *p_direction = NULL;
p_direction = (direction_t *) pvParameters;
p_direction = (direction_t *)pvParameters;
for (;;)
{
@ -379,7 +344,7 @@ magnetometer_tasks_init(car_struct_t *car_struct)
xTaskCreate(monitor_direction_task,
"Direction Task",
configMINIMAL_STACK_SIZE,
(void *) car_struct->p_direction,
(void *)car_struct->p_direction,
PRIO,
&h_direction_task);
}

16
frtos/magnetometer/magnetometer_test.c
View File

@ -3,7 +3,7 @@
#include "magnetometer_direction.h"
#include "map.h"
#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 1UL)
#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 1UL)
void
launch()
@ -21,16 +21,16 @@ launch()
}
int
main (void)
main(void)
{
stdio_usb_init();
direction_t direction;
car_struct_t car_struct = {.p_direction = &direction};
car_struct_t car_struct = { .p_direction = &direction };
int grid_rows = 10; // Define the number of rows in your grid
int grid_cols = 10; // Define the number of columns in your grid
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);
@ -39,13 +39,13 @@ main (void)
magnetometer_init(&car_struct);
// printf("Magnetometer initialized!\n");
// printf("Magnetometer initialized!\n");
magnetometer_tasks_init(&car_struct);
vTaskStartScheduler();
// launch();
// launch();
return(0);
return (0);
}

85
frtos/magnetometer/map.h
View File

@ -9,29 +9,33 @@
#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
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 *
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)
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)
}
}
@ -39,23 +43,32 @@ Grid *create_grid(int rows, int cols) {
}
// 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) {
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) {
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
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++) {
void
destroy_grid(Grid *grid)
{
for (int i = 0; i < grid->rows; i++)
{
free(grid->data[i]);
}
free(grid->data);
@ -64,12 +77,15 @@ void destroy_grid(Grid *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) {
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) {
switch (orientation)
{
case NORTH:
offset_y = 1;
break;
@ -105,24 +121,27 @@ void update_map(int orientation, int cur_x, int cur_y) {
mark_cell(car_path_grid, cur_x + offset_x, cur_y + offset_y);
}
// Function to print the map
void print_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++) {
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;
// }
// case false:
// printf("0 ");
// break;
// case true:
// printf("1 ");
// break;
// }
}
printf("\n");
}
}
#endif //TEST_PROJECT_MAP_H
#endif // TEST_PROJECT_MAP_H

6
frtos/motor/motor_init.h
View File

@ -38,9 +38,9 @@ motor_init(car_struct_t *car_struct)
g_right_sem = xSemaphoreCreateBinary();
car_struct->p_pid->use_pid = true;
car_struct->p_pid->kp_value = 60.f;
car_struct->p_pid->ki_value = 0.f;
car_struct->p_pid->kd_value = 135.f;
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;

2
frtos/motor/motor_pid.h
View File

@ -67,7 +67,7 @@ repeating_pid_handler(struct repeating_timer *ppp_timer)
if (temp <= MIN_PWM_LEVEL)
{
temp = MIN_PWM_LEVEL + 1u;
temp = MIN_PWM_LEVEL;
}
set_wheel_speed((uint32_t)temp, car_strut->p_right_motor);

2
frtos/motor/motor_test.c
View File

@ -47,7 +47,7 @@ main(void)
"motor_turning_task",
configMINIMAL_STACK_SIZE,
(void *)&car_struct,
WHEEL_CONTROL_PRIO,
1,
&h_motor_turning_task_handle);
// PID timer

71
frtos/rtos_car.c
View File

@ -19,6 +19,11 @@ check_line_touch(void *params)
| (car_struct->obs->right_sensor_detected);
}
/**
* @brief Check if the car is on the line or if there is an obstacle
* @param params The car_struct
* @return 1 if there is an obstacle, 0 otherwise
*/
bool
check_collision(void *params)
{
@ -34,6 +39,8 @@ void
motor_control_task(void *params)
{
car_struct_t *car_struct = (car_struct_t *)params;
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct);
for (;;)
{
@ -41,24 +48,19 @@ motor_control_task(void *params)
switch (temp)
{
default:
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct);
distance_to_stop(car_struct, 50.f);
vTaskDelay(pdMS_TO_TICKS(3000));
break;
case 0b01:
car_struct->p_right_motor->speed.current_cms
+= SLOT_DISTANCE_CM * 1000.f;
car_struct->p_right_motor->speed.distance_cm
-= SLOT_DISTANCE_CM;
break;
case 0b10:
car_struct->p_right_motor->speed.current_cms
-= SLOT_DISTANCE_CM * 1000.f;
car_struct->p_right_motor->speed.distance_cm
+= SLOT_DISTANCE_CM;
break;
case 0b11:
// set_wheel_direction(DIRECTION_MASK);
// set_wheel_speed_synced(0u, car_struct);
// vTaskDelay(pdMS_TO_TICKS(1000));
// turn(DIRECTION_LEFT, 90u, 90u, car_struct);
turn(DIRECTION_LEFT, 90u, 90u, car_struct);
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct);
break;
}
@ -78,11 +80,12 @@ obs_task(void *params)
{
if (car_struct->obs->ultrasonic_detected)
{
// turn(DIRECTION_LEFT, 180u, 90u, car_struct);
// set_wheel_direction(DIRECTION_FORWARD);
// set_wheel_speed_synced(90u, car_struct);
// turn(DIRECTION_LEFT, 130u, 90u, car_struct);
// set_wheel_direction(DIRECTION_FORWARD);
// set_wheel_speed_synced(90u, car_struct);
//
revert_wheel_direction();
distance_to_stop(car_struct, 25.f);
distance_to_stop(car_struct, 20.f);
}
vTaskDelay(pdMS_TO_TICKS(50));
}
@ -96,14 +99,14 @@ turn_task(void *params)
for (;;)
{
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct);
set_wheel_speed_synced(89u, car_struct);
distance_to_stop(car_struct, 50.f);
distance_to_stop(car_struct, 20.f);
vTaskDelay(pdMS_TO_TICKS(1000));
// turn_to_yaw(DIRECTION_LEFT, 230.f, 80u, car_struct);
turn(DIRECTION_RIGHT, 50.f, 90u, car_struct);
turn(DIRECTION_RIGHT, 80.f, 90u, car_struct);
vTaskDelay(pdMS_TO_TICKS(1000));
}
}
@ -177,23 +180,23 @@ main(void)
sleep_ms(1000u);
// control task
// TaskHandle_t h_motor_turning_task_handle = NULL;
// xTaskCreate(motor_control_task,
// "motor_turning_task",
// configMINIMAL_STACK_SIZE,
// (void *)&car_struct,
// PRIO,
// &h_motor_turning_task_handle);
// control task
// TaskHandle_t h_motor_turning_task_handle = NULL;
// xTaskCreate(motor_control_task,
// "motor_turning_task",
// configMINIMAL_STACK_SIZE,
// (void *)&car_struct,
// PRIO,
// &h_motor_turning_task_handle);
// obs task
TaskHandle_t h_obs_task_handle = NULL;
xTaskCreate(obs_task,
"obs_task",
configMINIMAL_STACK_SIZE,
(void *)&car_struct,
PRIO,
&h_obs_task_handle);
// TaskHandle_t h_obs_task_handle = NULL;
// xTaskCreate(obs_task,
// "obs_task",
// configMINIMAL_STACK_SIZE,
// (void *)&car_struct,
// PRIO,
// &h_obs_task_handle);
// turn task
TaskHandle_t h_turn_task_handle = NULL;

126
frtos/rtos_car_line.c Normal file
View File

@ -0,0 +1,126 @@
#include "line_sensor_init.h"
#include "ultrasonic_sensor.h"
#include "car_config.h"
#include "motor_init.h"
/*!
* @brief Check if the car is on the line
* @param params
* @return
*/
uint8_t
check_line_touch(void *params)
{
car_struct_t *car_struct = (car_struct_t *)params;
return (car_struct->obs->left_sensor_detected << 1)
| (car_struct->obs->right_sensor_detected);
}
void
motor_control_task(void *params)
{
car_struct_t *car_struct = (car_struct_t *)params;
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct);
for (;;)
{
uint8_t temp = check_line_touch(car_struct);
switch (temp)
{
default:
break;
case 0b01: // right
car_struct->p_right_motor->speed.distance_cm -=
SLOT_DISTANCE_CM;
break;
case 0b10: //left
car_struct->p_right_motor->speed.distance_cm +=
SLOT_DISTANCE_CM;
break;
case 0b11:
revert_wheel_direction();
distance_to_stop(car_struct, 17.f);
turn(DIRECTION_LEFT, 90u, 90u, car_struct);
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct);
break;
}
vTaskDelay(pdMS_TO_TICKS(50));
}
}
void
h_main_irq_handler(void)
{
if (gpio_get_irq_event_mask(SPEED_PIN_LEFT) & GPIO_IRQ_EDGE_FALL)
{
gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL);
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_left_sem, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
if (gpio_get_irq_event_mask(SPEED_PIN_RIGHT) & GPIO_IRQ_EDGE_FALL)
{
gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL);
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_right_sem, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
int
main(void)
{
stdio_usb_init();
obs_t obs;
motor_t motor_right;
motor_t motor_left;
motor_pid_t pid;
direction_t direction;
car_struct_t car_struct = { .p_right_motor = &motor_right,
.p_left_motor = &motor_left,
.p_pid = &pid,
.obs = &obs,
.p_direction = &direction };
// line
line_sensor_init(&car_struct);
line_tasks_init(&car_struct);
printf("Line sensor initialized!\n");
// motor
motor_init(&car_struct);
motor_tasks_init(&car_struct, &h_main_irq_handler);
printf("Motor initialized!\n");
sleep_ms(1000u);
// control task
TaskHandle_t h_motor_turning_task_handle = NULL;
xTaskCreate(motor_control_task,
"motor_turning_task",
configMINIMAL_STACK_SIZE,
(void *)&car_struct,
PRIO,
&h_motor_turning_task_handle);
// PID timer
struct repeating_timer pid_timer;
add_repeating_timer_ms(-50, repeating_pid_handler, &car_struct, &pid_timer);
vTaskStartScheduler();
return (0);
}

2
frtos/ultrasonic_sensor/ultrasonic_init.h
View File

@ -18,7 +18,7 @@ check_obstacle(void *pvParameters)
while (true)
{ // Put trigger pin high for 10us
gpio_put(TRIG_PIN, 1);
sleep_us(10);
vTaskDelay(1);
gpio_put(TRIG_PIN, 0);
// Wait for echo pin to go high