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

add direction control

This commit is contained in:
Richie 2023-10-30 23:20:15 +08:00
parent f176192dce
commit ed516b5ce4
2 changed files with 80 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
frtos/motor/motor_direction.h
View File

@ -28,19 +28,46 @@ set_wheel_direction(uint32_t direction)
}
/*!
* @brief Set the speed of the wheels
* @param speed The speed of the wheels, from 0 to 5000
* @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
set_wheel_speed(uint32_t speed)
turn_wheel(uint32_t direction)
{
g_motor_right.pwm.level = speed;
g_motor_left.pwm.level = speed;
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;
pwm_set_chan_level(g_motor_right.pwm.slice_num,
g_motor_right.pwm.channel,
g_motor_right.pwm.level);
set_wheel_direction(direction);
set_wheel_speed(3500u);
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)
{
break;
}
}
vTaskDelay(pdMS_TO_TICKS(1000));
}

48
frtos/motor/motor_test.c
View File

@ -4,7 +4,37 @@
#include "motor_direction.h"
#include "motor_pid.h"
#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
#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()
@ -19,8 +49,8 @@ launch()
irq_set_enabled(IO_IRQ_BANK0, true);
// Set wheel speed
set_wheel_speed(3500);
// set_wheel_direction(DIRECTION_FORWARD);
// set_wheel_speed(3000);
// Left wheel
//
@ -32,7 +62,6 @@ launch()
WHEEL_SPEED_PRIO,
&h_monitor_left_wheel_speed_task_handle);
// Right wheel
//
TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL;
@ -48,9 +77,17 @@ launch()
"motor_pid_task",
configMINIMAL_STACK_SIZE,
(void *)&g_motor_right,
WHEEL_SPEED_PRIO,
WHEEL_PID_PRIO,
&h_motor_pid_right_task_handle);
// control task
TaskHandle_t h_motor_turning_task_handle = NULL;
xTaskCreate(motor_control_task,
"motor_turning_task",
configMINIMAL_STACK_SIZE,
NULL,
WHEEL_CONTROL_PRIO,
&h_motor_turning_task_handle);
vTaskStartScheduler();
}
@ -64,7 +101,6 @@ main(void)
printf("Test started!\n");
motor_init();
set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD);
launch();