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

updated motor with testing codes with line sensor

This commit is contained in:
Richie 2023-10-31 21:52:09 +08:00
parent 398ef2b8ca
commit 4137d4f350
8 changed files with 276 additions and 216 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
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

68
frtos/config/motor_config.h
View File

@ -3,44 +3,48 @@
#define MOTOR_CONFIG_H
// ENA and ENB on the L298N
#define PWM_PIN_RIGHT 1U // chanel B
#define PWM_PIN_LEFT 0U // chanel A
#define PWM_PIN_RIGHT 1U // chanel B
#define PWM_PIN_LEFT 0U // chanel A
// IN1, IN2, IN3, IN4 on the L298N
#define DIRECTION_PIN_RIGHT_IN1 11U
#define DIRECTION_PIN_RIGHT_IN2 12U
#define DIRECTION_PIN_RIGHT_IN1 11U
#define DIRECTION_PIN_RIGHT_IN2 12U
#define DIRECTION_PIN_LEFT_IN3 19U
#define DIRECTION_PIN_LEFT_IN4 20U
#define DIRECTION_PIN_LEFT_IN3 19U
#define DIRECTION_PIN_LEFT_IN4 20U
#define DIRECTION_RIGHT_FORWARD (1U << DIRECTION_PIN_RIGHT_IN2)
#define DIRECTION_RIGHT_BACKWARD (1U << DIRECTION_PIN_RIGHT_IN1)
#define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4)
#define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3)
#define DIRECTION_RIGHT_FORWARD (1U << DIRECTION_PIN_RIGHT_IN2)
#define DIRECTION_RIGHT_BACKWARD (1U << DIRECTION_PIN_RIGHT_IN1)
#define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4)
#define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3)
#define DIRECTION_FORWARD (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_FORWARD)
#define DIRECTION_BACKWARD (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_BACKWARD)
#define DIRECTION_LEFT (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_BACKWARD)
#define DIRECTION_RIGHT (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_FORWARD)
#define DIRECTION_FORWARD (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_FORWARD)
#define DIRECTION_BACKWARD (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_BACKWARD)
#define DIRECTION_LEFT (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_BACKWARD)
#define DIRECTION_RIGHT (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_FORWARD)
#define SPEED_PIN_RIGHT 15U
#define SPEED_PIN_LEFT 16U
#define SPEED_PIN_RIGHT 15U
#define SPEED_PIN_LEFT 16U
#define PWM_CLK_DIV 250.f
#define PWM_WRAP 5000U
#define PWM_CLK_DIV 250.f
#define PWM_WRAP 5000U
#define MAX_SPEED 4900U
#define MIN_SPEED 0U // To be changed
#define MAX_SPEED 4900U
#define MIN_SPEED 0U // To be changed
#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
#define WHEEL_CONTROL_PRIO (tskIDLE_PRIORITY + 1UL)
#define WHEEL_PID_PRIO (tskIDLE_PRIORITY + 1UL)
/*!
* @brief Structure for the motor speed parameters
* @param target_speed_cms Target speed in cm/s
* @param current_speed_cms Current speed in cm/s
* @param distance_cm Distance travelled in cm
*/
typedef struct {
float current_cms;
float distance_cm;
typedef struct
{
float current_cms;
float distance_cm;
} motor_speed_t;
/*!
@ -49,7 +53,8 @@ typedef struct {
* @param pwm_channel PWM channel, either A or B
* @param pwm_level PWM level, from 0 to 5000
*/
typedef struct {
typedef struct
{
uint slice_num;
uint channel;
uint16_t level;
@ -61,7 +66,8 @@ typedef struct {
* @param pid_ki Integral gain
* @param pid_kd Derivative gain
*/
typedef struct {
typedef struct
{
float kp_value;
float ki_value;
float kd_value;
@ -74,11 +80,19 @@ typedef struct {
* @param pwm Motor PWM parameters
* @param pid Motor PID parameters
*/
typedef struct {
typedef struct
{
motor_speed_t speed;
SemaphoreHandle_t sem;
motor_pwm_t pwm;
motor_pid_t pid;
} motor_t;
typedef struct
{
float starting_distance_cm;
float distance_to_travel_cm;
volatile bool is_running;
} distance_to_stop_t;
#endif /* MOTOR_CONFIG_H */

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)

107
frtos/motor/motor_direction.h
View File

@ -27,47 +27,88 @@ set_wheel_direction(uint32_t direction)
gpio_set_mask(direction);
}
/*!
* @brief Turn the wheel, must set the priority higher than the motor PID task
* @param direction The direction of the wheel
* @param direction_after The direction of the wheel after turning
* @param speed_after The speed of the wheel after turning
*/
void
turn_wheel(uint32_t direction)
turn_left_90()
{
set_wheel_speed(0u);
vTaskDelay(pdMS_TO_TICKS(1000));
float initial_right = g_motor_right.speed.distance_cm;
float initial_left = g_motor_left.speed.distance_cm;
set_wheel_direction(direction);
set_wheel_speed(3500u);
set_wheel_direction(DIRECTION_RIGHT_FORWARD);
set_wheel_speed(3500u, &g_motor_right);
float initial = g_motor_right.speed.distance_cm;
for (;;)
{
// gap between wheels = 11.3cm, to turn 90 degrees, need to travel
// 11.3 * pi / 4 = 8.9cm
if (g_motor_left.speed.distance_cm - initial_left >= 6.8f)
{
g_motor_left.pwm.level = 0;
pwm_set_chan_level(g_motor_left.pwm.slice_num,
g_motor_left.pwm.channel,
g_motor_left.pwm.level);
}
if (g_motor_right.speed.distance_cm - initial_right >= 6.8f)
{
g_motor_right.pwm.level = 0;
pwm_set_chan_level(g_motor_right.pwm.slice_num,
g_motor_right.pwm.channel,
g_motor_right.pwm.level);
}
if (g_motor_left.pwm.level == 0u && g_motor_right.pwm.level == 0u)
if (g_motor_right.speed.distance_cm - initial >= 17.f)
{
set_wheel_speed(0u, &g_motor_right);
break;
}
vTaskDelay(pdMS_TO_TICKS(5));
}
vTaskDelay(pdMS_TO_TICKS(1000));
g_motor_right.speed.distance_cm = initial;
g_motor_left.speed.distance_cm = initial;
}
void
turn_right_90()
{
set_wheel_direction(DIRECTION_LEFT_FORWARD);
set_wheel_speed(3500u, &g_motor_left);
float initial = g_motor_left.speed.distance_cm;
for (;;)
{
if (g_motor_left.speed.distance_cm - initial >= 17.f)
{
set_wheel_speed(0u, &g_motor_left);
break;
}
vTaskDelay(pdMS_TO_TICKS(5));
}
vTaskDelay(pdMS_TO_TICKS(1000));
g_motor_left.speed.distance_cm = initial;
g_motor_right.speed.distance_cm = initial;
}
void
spin_left_90()
{
set_wheel_direction(DIRECTION_LEFT);
set_wheel_speed_synced(3500u);
float initial = g_motor_left.speed.distance_cm;
for (;;)
{
if (g_motor_left.speed.distance_cm - initial >= 7.5f)
{
set_wheel_speed_synced(0u);
break;
}
vTaskDelay(pdMS_TO_TICKS(5));
}
vTaskDelay(pdMS_TO_TICKS(1000));
g_motor_left.speed.distance_cm = initial;
g_motor_right.speed.distance_cm = initial;
}
void
spin_right_90()
{
set_wheel_direction(DIRECTION_RIGHT);
set_wheel_speed_synced(3500u);
float initial = g_motor_right.speed.distance_cm;
for (;;)
{
if (g_motor_right.speed.distance_cm - initial >= 7.5f)
{
set_wheel_speed_synced(0u);
break;
}
vTaskDelay(pdMS_TO_TICKS(5));
}
vTaskDelay(pdMS_TO_TICKS(1000));
g_motor_right.speed.distance_cm = initial;
g_motor_left.speed.distance_cm = initial;
}

70
frtos/motor/motor_pid.h
View File

@ -5,7 +5,7 @@
* @author Richie
*/
#include "motor_init.h"
// #include "magnetometer_init.h"
/*!
* @brief Compute the control signal using PID controller
@ -21,6 +21,8 @@ compute_pid(float *integral, float *prev_error)
float error
= g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm;
printf("%f,\n", error);
*integral += error;
float derivative = error - *prev_error;
@ -34,44 +36,38 @@ compute_pid(float *integral, float *prev_error)
return control_signal;
}
void
motor_pid_task(__unused void *p_param)
bool
repeating_pid_handler(__unused struct repeating_timer *t)
{
float integral = 0.0f;
float prev_error = 0.0f;
static float integral = 0.0f;
static float prev_error = 0.0f;
for (;;)
if ((g_motor_left.pwm.level == 0u) || (g_motor_right.pwm.level == 0u))
{
if (g_motor_left.pwm.level == 0u)
{
g_motor_right.pwm.level = 0;
pwm_set_chan_level(g_motor_right.pwm.slice_num,
g_motor_right.pwm.channel,
g_motor_right.pwm.level);
vTaskDelay(pdMS_TO_TICKS(50));
continue;
}
float control_signal = compute_pid(&integral, &prev_error);
float temp = (float) g_motor_right.pwm.level + control_signal * 0.05f;
if (temp > MAX_SPEED)
{
temp = MAX_SPEED;
}
if (temp < MIN_SPEED)
{
temp = MIN_SPEED;
}
g_motor_right.pwm.level = (uint16_t) temp;
pwm_set_chan_level(g_motor_right.pwm.slice_num,
g_motor_right.pwm.channel,
g_motor_right.pwm.level);
vTaskDelay(pdMS_TO_TICKS(50));
return true;
}
float control_signal = compute_pid(&integral, &prev_error);
float temp = (float)g_motor_right.pwm.level + control_signal * 0.05f;
if (temp > MAX_SPEED)
{
temp = MAX_SPEED;
}
if (temp < MIN_SPEED)
{
temp = MIN_SPEED + 1u;
}
g_motor_right.pwm.level = (uint16_t)temp;
pwm_set_chan_level(g_motor_right.pwm.slice_num,
g_motor_right.pwm.channel,
g_motor_right.pwm.level);
printf("speed: %f cm/s\n", g_motor_right.speed.current_cms);
printf("distance: %f cm\n", g_motor_right.speed.distance_cm);
return true;
}

46
frtos/motor/motor_speed.h
View File

@ -74,6 +74,26 @@ monitor_wheel_speed_task(void *pvParameters)
}
}
void
set_wheel_speed(uint32_t pwm_level, motor_t * motor)
{
motor->pwm.level = pwm_level;
pwm_set_chan_level(motor->pwm.slice_num,
motor->pwm.channel,
motor->pwm.level);
}
/*!
* @brief Set the speed of the wheels
* @param pwm_level The pwm_level of the wheels, from 0 to 5000
*/
void
set_wheel_speed_synced(uint32_t pwm_level)
{
set_wheel_speed(pwm_level, &g_motor_left);
set_wheel_speed(pwm_level, &g_motor_right);
}
/*!
* @brief Set the distance to travel before stopping, must be called after
@ -84,34 +104,16 @@ void
distance_to_stop(float distance_cm)
{
float initial = g_motor_right.speed.distance_cm;
printf("initial: %f\n", initial);
for (;;)
{
if (g_motor_right.speed.distance_cm - initial >= distance_cm)
{
g_motor_right.pwm.level = 0;
g_motor_left.pwm.level = 0;
set_wheel_speed_synced(0u);
break;
}
// vTaskDelay(pdMS_TO_TICKS(50));
vTaskDelay(pdMS_TO_TICKS(10));
}
vTaskDelay(pdMS_TO_TICKS(1000));
g_motor_right.speed.distance_cm = g_motor_left.speed.distance_cm;
}
/*!
* @brief Set the speed of the wheels
* @param pwm_level The pwm_level of the wheels, from 0 to 5000
*/
void
set_wheel_speed(uint32_t pwm_level)
{
g_motor_right.pwm.level = pwm_level;
g_motor_left.pwm.level = pwm_level;
pwm_set_chan_level(g_motor_right.pwm.slice_num,
g_motor_right.pwm.channel,
g_motor_right.pwm.level);
pwm_set_chan_level(g_motor_left.pwm.slice_num,
g_motor_left.pwm.channel,
g_motor_left.pwm.level);
}

44
frtos/motor/motor_test.c
View File

@ -4,37 +4,6 @@
#include "motor_direction.h"
#include "motor_pid.h"
#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 2UL)
#define WHEEL_CONTROL_PRIO (tskIDLE_PRIORITY + 2UL)
#define WHEEL_PID_PRIO (tskIDLE_PRIORITY + 2UL)
static void
motor_control_task(__unused void *p_param)
{
for (;;)
{
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed(3000u);
distance_to_stop(30);
set_wheel_direction(DIRECTION_BACKWARD);
set_wheel_speed(3000u);
distance_to_stop(30);
turn_wheel(DIRECTION_LEFT);
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed(3000u);
distance_to_stop(30);
turn_wheel(DIRECTION_RIGHT);
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed(3000u);
distance_to_stop(30);
set_wheel_speed(0u);
vTaskDelay(pdMS_TO_TICKS(3000));
}
}
void
launch()
@ -49,8 +18,9 @@ launch()
irq_set_enabled(IO_IRQ_BANK0, true);
// set_wheel_direction(DIRECTION_FORWARD);
// set_wheel_speed(3000);
// PID timer
struct repeating_timer pid_timer;
add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer);
// Left wheel
//
@ -72,14 +42,6 @@ launch()
WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle);
TaskHandle_t h_motor_pid_right_task_handle = NULL;
xTaskCreate(motor_pid_task,
"motor_pid_task",
configMINIMAL_STACK_SIZE,
(void *)&g_motor_right,
WHEEL_PID_PRIO,
&h_motor_pid_right_task_handle);
// control task
TaskHandle_t h_motor_turning_task_handle = NULL;
xTaskCreate(motor_control_task,

154
frtos/rtos_car.c
View File

@ -14,46 +14,90 @@
#include "motor_speed.h"
#include "motor_direction.h"
#include "motor_pid.h"
#include "line_sensor.h"
#include "ultrasonic_sensor.h"
#define READ_LEFT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
#define READ_RIGHT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
#define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
#define READ_RIGHT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
#define READ_BARCODE_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
#define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
#define READ_RIGHT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
#define READ_BARCODE_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 3UL)
#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 3UL)
#define DISTANCE_TASK_PRIORITY (tskIDLE_PRIORITY + 4UL)
#define DISTANCE_TASK_PRIORITY (tskIDLE_PRIORITY + 4UL)
/* Common Car State Structure (TODO: TBC)*/
//static car_state_t g_car_state;
// static car_state_t g_car_state;
static void
motor_control_task(__unused void *p_param)
{
static bool left = false;
static bool right = false;
for (;;)
{
state_t state = gpio_get(LEFT_SENSOR_PIN);
printf("state: %d\n", state);
if (state == 1)
{
if (!left)
{
spin_left_90();
left = true;
}
else if (!right)
{
spin_right_90();
spin_right_90();
right = true;
}
else
{
spin_right_90();
left = false;
right = false;
}
}
else
{
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(3500u);
}
vTaskDelay(pdMS_TO_TICKS(5));
}
}
void
launch()
{
// // isr to detect left line sensor
// gpio_set_irq_enabled(LEFT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true);
// gpio_add_raw_irq_handler(LEFT_SENSOR_PIN, h_line_sensor_handler);
struct repeating_timer g_left_sensor_timer;
add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,
h_left_sensor_timer_handler,
NULL,
&g_left_sensor_timer);
// isr to detect right motor slot
gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true);
gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, h_wheel_sensor_isr_handler);
struct repeating_timer g_right_sensor_timer;
add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,
h_right_sensor_timer_handler,
NULL,
&g_right_sensor_timer);
// isr to detect left motor slot
gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true);
gpio_add_raw_irq_handler(SPEED_PIN_LEFT, h_wheel_sensor_isr_handler);
struct repeating_timer g_barcode_sensor_timer;
add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,
h_barcode_sensor_timer_handler,
NULL,
&g_barcode_sensor_timer);
irq_set_enabled(IO_IRQ_BANK0, true);
// // line sensor timer
// struct repeating_timer g_left_sensor_timer;
// add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,
// h_left_sensor_timer_handler,
// NULL,
// &g_left_sensor_timer);
// PID timer
struct repeating_timer pid_timer;
add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer);
TaskHandle_t h_monitor_left_sensor_task;
xTaskCreate(monitor_left_sensor_task,
@ -63,55 +107,53 @@ launch()
READ_LEFT_SENSOR_PRIO,
&h_monitor_left_sensor_task);
TaskHandle_t h_monitor_right_sensor_task;
xTaskCreate(monitor_right_sensor_task,
"Monitor Right Sensor Task",
// Left wheel
//
TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_wheel_speed_task,
"monitor_left_wheel_speed_task",
configMINIMAL_STACK_SIZE,
NULL,
READ_RIGHT_SENSOR_PRIO,
&h_monitor_right_sensor_task);
TaskHandle_t h_monitor_barcode_sensor_task;
xTaskCreate(monitor_barcode_sensor_task,
"Monitor Barcode Sensor Task",
configMINIMAL_STACK_SIZE,
NULL,
READ_BARCODE_SENSOR_PRIO,
&h_monitor_barcode_sensor_task);
(void *)&g_motor_left,
WHEEL_SPEED_PRIO,
&h_monitor_left_wheel_speed_task_handle);
TaskHandle_t h_monitor_direction_task;
xTaskCreate(monitor_direction_task,
"Monitor Direction Task",
// Right wheel
//
TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_wheel_speed_task,
"monitor_wheel_speed_task",
configMINIMAL_STACK_SIZE,
(void *)&g_motor_right,
WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle);
// control task
TaskHandle_t h_motor_turning_task_handle = NULL;
xTaskCreate(motor_control_task,
"motor_turning_task",
configMINIMAL_STACK_SIZE,
NULL,
DIRECTION_TASK_PRIORITY,
&h_monitor_direction_task);
TaskHandle_t h_monitor_distance_task;
xTaskCreate(distance_task,
"Monitor Distance Task",
configMINIMAL_STACK_SIZE,
NULL,
DISTANCE_TASK_PRIORITY,
&h_monitor_distance_task);
WHEEL_CONTROL_PRIO,
&h_motor_turning_task_handle);
vTaskStartScheduler();
}
int
main (void)
main(void)
{
stdio_usb_init();
motor_init();
sleep_ms(4000);
printf("Test started!\n");
set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD);
motor_init();
line_sensor_setup();
init_ultrasonic();
//init_ultrasonic();
initialize_car_state(); // TODO: Could be common functionality, To confirm
//initialize_car_state(); // TODO: Could be common functionality, To confirm
// during Integration
launch();