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) NO_SYS=0 # don't want NO_SYS (generally this would be in your lwipopts.h)
LWIP_SOCKET=1 # we need the socket API (generally this would be in your lwipopts.h) LWIP_SOCKET=1 # we need the socket API (generally this would be in your lwipopts.h)
PING_USE_SOCKETS=1 PING_USE_SOCKETS=1
PICO_MAX_SHARED_IRQ_HANDLERS=5
) )
target_include_directories(rtos_car PRIVATE target_include_directories(rtos_car PRIVATE
${CMAKE_CURRENT_LIST_DIR}/config ${CMAKE_CURRENT_LIST_DIR}/config

68
frtos/config/motor_config.h
View File

@ -3,44 +3,48 @@
#define MOTOR_CONFIG_H #define MOTOR_CONFIG_H
// ENA and ENB on the L298N // ENA and ENB on the L298N
#define PWM_PIN_RIGHT 1U // chanel B #define PWM_PIN_RIGHT 1U // chanel B
#define PWM_PIN_LEFT 0U // chanel A #define PWM_PIN_LEFT 0U // chanel A
// IN1, IN2, IN3, IN4 on the L298N // IN1, IN2, IN3, IN4 on the L298N
#define DIRECTION_PIN_RIGHT_IN1 11U #define DIRECTION_PIN_RIGHT_IN1 11U
#define DIRECTION_PIN_RIGHT_IN2 12U #define DIRECTION_PIN_RIGHT_IN2 12U
#define DIRECTION_PIN_LEFT_IN3 19U #define DIRECTION_PIN_LEFT_IN3 19U
#define DIRECTION_PIN_LEFT_IN4 20U #define DIRECTION_PIN_LEFT_IN4 20U
#define DIRECTION_RIGHT_FORWARD (1U << DIRECTION_PIN_RIGHT_IN2) #define DIRECTION_RIGHT_FORWARD (1U << DIRECTION_PIN_RIGHT_IN2)
#define DIRECTION_RIGHT_BACKWARD (1U << DIRECTION_PIN_RIGHT_IN1) #define DIRECTION_RIGHT_BACKWARD (1U << DIRECTION_PIN_RIGHT_IN1)
#define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4) #define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4)
#define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3) #define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3)
#define DIRECTION_FORWARD (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_FORWARD) #define DIRECTION_FORWARD (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_FORWARD)
#define DIRECTION_BACKWARD (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_BACKWARD) #define DIRECTION_BACKWARD (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_BACKWARD)
#define DIRECTION_LEFT (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_BACKWARD) #define DIRECTION_LEFT (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_BACKWARD)
#define DIRECTION_RIGHT (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_FORWARD) #define DIRECTION_RIGHT (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_FORWARD)
#define SPEED_PIN_RIGHT 15U #define SPEED_PIN_RIGHT 15U
#define SPEED_PIN_LEFT 16U #define SPEED_PIN_LEFT 16U
#define PWM_CLK_DIV 250.f #define PWM_CLK_DIV 250.f
#define PWM_WRAP 5000U #define PWM_WRAP 5000U
#define MAX_SPEED 4900U #define MAX_SPEED 4900U
#define MIN_SPEED 0U // To be changed #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 * @brief Structure for the motor speed parameters
* @param target_speed_cms Target speed in cm/s
* @param current_speed_cms Current speed in cm/s * @param current_speed_cms Current speed in cm/s
* @param distance_cm Distance travelled in cm * @param distance_cm Distance travelled in cm
*/ */
typedef struct { typedef struct
float current_cms; {
float distance_cm; float current_cms;
float distance_cm;
} motor_speed_t; } motor_speed_t;
/*! /*!
@ -49,7 +53,8 @@ typedef struct {
* @param pwm_channel PWM channel, either A or B * @param pwm_channel PWM channel, either A or B
* @param pwm_level PWM level, from 0 to 5000 * @param pwm_level PWM level, from 0 to 5000
*/ */
typedef struct { typedef struct
{
uint slice_num; uint slice_num;
uint channel; uint channel;
uint16_t level; uint16_t level;
@ -61,7 +66,8 @@ typedef struct {
* @param pid_ki Integral gain * @param pid_ki Integral gain
* @param pid_kd Derivative gain * @param pid_kd Derivative gain
*/ */
typedef struct { typedef struct
{
float kp_value; float kp_value;
float ki_value; float ki_value;
float kd_value; float kd_value;
@ -74,11 +80,19 @@ typedef struct {
* @param pwm Motor PWM parameters * @param pwm Motor PWM parameters
* @param pid Motor PID parameters * @param pid Motor PID parameters
*/ */
typedef struct { typedef struct
{
motor_speed_t speed; motor_speed_t speed;
SemaphoreHandle_t sem; SemaphoreHandle_t sem;
motor_pwm_t pwm; motor_pwm_t pwm;
motor_pid_t pid; motor_pid_t pid;
} motor_t; } 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 */ #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_cyw43_arch_lwip_sys_freertos
pico_stdlib pico_stdlib
pico_lwip_iperf pico_lwip_iperf
hardware_i2c
FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap
hardware_pwm hardware_pwm
) )
target_include_directories(motor_test PRIVATE target_include_directories(motor_test PRIVATE
../config ../config
../magnetometer
) )
pico_enable_stdio_usb(motor_test 1) pico_enable_stdio_usb(motor_test 1)

107
frtos/motor/motor_direction.h
View File

@ -27,47 +27,88 @@ set_wheel_direction(uint32_t direction)
gpio_set_mask(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 void
turn_wheel(uint32_t direction) turn_left_90()
{ {
set_wheel_speed(0u); set_wheel_direction(DIRECTION_RIGHT_FORWARD);
vTaskDelay(pdMS_TO_TICKS(1000)); set_wheel_speed(3500u, &g_motor_right);
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);
float initial = g_motor_right.speed.distance_cm;
for (;;) for (;;)
{ {
// gap between wheels = 11.3cm, to turn 90 degrees, need to travel if (g_motor_right.speed.distance_cm - initial >= 17.f)
// 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)
{ {
set_wheel_speed(0u, &g_motor_right);
break; break;
} }
vTaskDelay(pdMS_TO_TICKS(5));
} }
vTaskDelay(pdMS_TO_TICKS(1000)); 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 * @author Richie
*/ */
#include "motor_init.h" // #include "magnetometer_init.h"
/*! /*!
* @brief Compute the control signal using PID controller * @brief Compute the control signal using PID controller
@ -21,6 +21,8 @@ compute_pid(float *integral, float *prev_error)
float error float error
= g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm; = g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm;
printf("%f,\n", error);
*integral += error; *integral += error;
float derivative = error - *prev_error; float derivative = error - *prev_error;
@ -34,44 +36,38 @@ compute_pid(float *integral, float *prev_error)
return control_signal; return control_signal;
} }
void bool
motor_pid_task(__unused void *p_param) repeating_pid_handler(__unused struct repeating_timer *t)
{ {
float integral = 0.0f; static float integral = 0.0f;
float prev_error = 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) return true;
{
g_motor_right.pwm.level = 0;
pwm_set_chan_level(g_motor_right.pwm.slice_num,
g_motor_right.pwm.channel,
g_motor_right.pwm.level);
vTaskDelay(pdMS_TO_TICKS(50));
continue;
}
float control_signal = compute_pid(&integral, &prev_error);
float temp = (float) g_motor_right.pwm.level + control_signal * 0.05f;
if (temp > MAX_SPEED)
{
temp = MAX_SPEED;
}
if (temp < MIN_SPEED)
{
temp = MIN_SPEED;
}
g_motor_right.pwm.level = (uint16_t) temp;
pwm_set_chan_level(g_motor_right.pwm.slice_num,
g_motor_right.pwm.channel,
g_motor_right.pwm.level);
vTaskDelay(pdMS_TO_TICKS(50));
} }
float control_signal = compute_pid(&integral, &prev_error);
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 * @brief Set the distance to travel before stopping, must be called after
@ -84,34 +104,16 @@ void
distance_to_stop(float distance_cm) distance_to_stop(float distance_cm)
{ {
float initial = g_motor_right.speed.distance_cm; float initial = g_motor_right.speed.distance_cm;
printf("initial: %f\n", initial);
for (;;) for (;;)
{ {
if (g_motor_right.speed.distance_cm - initial >= distance_cm) if (g_motor_right.speed.distance_cm - initial >= distance_cm)
{ {
g_motor_right.pwm.level = 0; set_wheel_speed_synced(0u);
g_motor_left.pwm.level = 0;
break; 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_direction.h"
#include "motor_pid.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 void
launch() launch()
@ -49,8 +18,9 @@ launch()
irq_set_enabled(IO_IRQ_BANK0, true); irq_set_enabled(IO_IRQ_BANK0, true);
// set_wheel_direction(DIRECTION_FORWARD); // PID timer
// set_wheel_speed(3000); struct repeating_timer pid_timer;
add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer);
// Left wheel // Left wheel
// //
@ -72,14 +42,6 @@ launch()
WHEEL_SPEED_PRIO, WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle); &h_monitor_right_wheel_speed_task_handle);
TaskHandle_t h_motor_pid_right_task_handle = NULL;
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 // control task
TaskHandle_t h_motor_turning_task_handle = NULL; TaskHandle_t h_motor_turning_task_handle = NULL;
xTaskCreate(motor_control_task, xTaskCreate(motor_control_task,

150
frtos/rtos_car.c
View File

@ -14,46 +14,90 @@
#include "motor_speed.h" #include "motor_speed.h"
#include "motor_direction.h" #include "motor_direction.h"
#include "motor_pid.h"
#include "line_sensor.h" #include "line_sensor.h"
#include "ultrasonic_sensor.h" #include "ultrasonic_sensor.h"
#define READ_LEFT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) #define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
#define READ_RIGHT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) #define READ_RIGHT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
#define READ_BARCODE_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
#define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL) #define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 3UL)
#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)
#define DISTANCE_TASK_PRIORITY (tskIDLE_PRIORITY + 4UL)
/* Common Car State Structure (TODO: TBC)*/ /* Common Car State Structure (TODO: TBC)*/
//static car_state_t g_car_state; // static car_state_t g_car_state;
static void
motor_control_task(__unused void *p_param)
{
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 void
launch() launch()
{ {
// // isr to detect left line sensor
// gpio_set_irq_enabled(LEFT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true);
// gpio_add_raw_irq_handler(LEFT_SENSOR_PIN, h_line_sensor_handler);
struct repeating_timer g_left_sensor_timer; // isr to detect right motor slot
add_repeating_timer_ms(LINE_SENSOR_READ_DELAY, gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true);
h_left_sensor_timer_handler, gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, h_wheel_sensor_isr_handler);
NULL,
&g_left_sensor_timer);
struct repeating_timer g_right_sensor_timer; // isr to detect left motor slot
add_repeating_timer_ms(LINE_SENSOR_READ_DELAY, gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true);
h_right_sensor_timer_handler, gpio_add_raw_irq_handler(SPEED_PIN_LEFT, h_wheel_sensor_isr_handler);
NULL,
&g_right_sensor_timer);
struct repeating_timer g_barcode_sensor_timer; irq_set_enabled(IO_IRQ_BANK0, true);
add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,
h_barcode_sensor_timer_handler, // // line sensor timer
NULL, // struct repeating_timer g_left_sensor_timer;
&g_barcode_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; TaskHandle_t h_monitor_left_sensor_task;
xTaskCreate(monitor_left_sensor_task, xTaskCreate(monitor_left_sensor_task,
@ -63,55 +107,53 @@ launch()
READ_LEFT_SENSOR_PRIO, READ_LEFT_SENSOR_PRIO,
&h_monitor_left_sensor_task); &h_monitor_left_sensor_task);
TaskHandle_t h_monitor_right_sensor_task; // Left wheel
xTaskCreate(monitor_right_sensor_task, //
"Monitor Right Sensor Task", TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_wheel_speed_task,
"monitor_left_wheel_speed_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
NULL, (void *)&g_motor_left,
READ_RIGHT_SENSOR_PRIO, WHEEL_SPEED_PRIO,
&h_monitor_right_sensor_task); &h_monitor_left_wheel_speed_task_handle);
TaskHandle_t h_monitor_barcode_sensor_task; // Right wheel
xTaskCreate(monitor_barcode_sensor_task, //
"Monitor Barcode Sensor Task", TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_wheel_speed_task,
"monitor_wheel_speed_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
NULL, (void *)&g_motor_right,
READ_BARCODE_SENSOR_PRIO, WHEEL_SPEED_PRIO,
&h_monitor_barcode_sensor_task); &h_monitor_right_wheel_speed_task_handle);
TaskHandle_t h_monitor_direction_task; // control task
xTaskCreate(monitor_direction_task, TaskHandle_t h_motor_turning_task_handle = NULL;
"Monitor Direction Task", xTaskCreate(motor_control_task,
"motor_turning_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
NULL, NULL,
DIRECTION_TASK_PRIORITY, WHEEL_CONTROL_PRIO,
&h_monitor_direction_task); &h_motor_turning_task_handle);
TaskHandle_t h_monitor_distance_task;
xTaskCreate(distance_task,
"Monitor Distance Task",
configMINIMAL_STACK_SIZE,
NULL,
DISTANCE_TASK_PRIORITY,
&h_monitor_distance_task);
vTaskStartScheduler(); vTaskStartScheduler();
} }
int int
main (void) main(void)
{ {
stdio_usb_init(); stdio_usb_init();
motor_init(); sleep_ms(4000);
printf("Test started!\n");
set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD); motor_init();
line_sensor_setup(); 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 // during Integration
launch(); launch();