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:
Devoalda 2023-11-08 14:11:11 +08:00 committed by GitHub
parent f65c0ab189 f284f972d2
commit f78fb7973d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 180 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
frtos/config/motor_config.h
View File

@ -18,6 +18,11 @@
#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_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD)
#define DIRECTION_BACKWARD (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_BACKWARD)
#define DIRECTION_MASK (DIRECTION_FORWARD | DIRECTION_BACKWARD)
#define SPEED_PIN_RIGHT 15U #define SPEED_PIN_RIGHT 15U
#define SPEED_PIN_LEFT 16U #define SPEED_PIN_LEFT 16U

150
frtos/motor/motor_direction.h
View File

@ -80,80 +80,80 @@ turn_right_90()
g_motor_right.speed.distance_cm = initial; g_motor_right.speed.distance_cm = initial;
} }
void //void
spin_left_90() //spin_left_90()
{ //{
set_wheel_direction(DIRECTION_LEFT); // set_wheel_direction(DIRECTION_LEFT);
//
// set_wheel_speed_synced(90u);
//
// 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;
//}
set_wheel_speed_synced(90u); //void
//spin_right_90()
//{
// set_wheel_direction(DIRECTION_RIGHT);
//
// set_wheel_speed_synced(90u);
//
// 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;
//}
float initial = g_motor_left.speed.distance_cm; //void
for (;;) //spin_to_yaw(float target_yaw)
{ //{
if (g_motor_left.speed.distance_cm - initial >= 7.5f) // float initial_yaw = g_direction.yaw;
{ //
set_wheel_speed_synced(0u); // // if it will to turn more than 180 degrees, turn the other way
break; // if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw < 180.f)
} // || ((target_yaw < initial_yaw) && (initial_yaw - target_yaw >= 180.f)))
vTaskDelay(pdMS_TO_TICKS(5)); // {
} // set_wheel_direction(DIRECTION_LEFT);
vTaskDelay(pdMS_TO_TICKS(1000)); // }
g_motor_left.speed.distance_cm = initial; // else if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw >= 180.f)
g_motor_right.speed.distance_cm = initial; // || ((target_yaw < initial_yaw)
} // && (initial_yaw - target_yaw < 180.f)))
// {
void // set_wheel_direction(DIRECTION_RIGHT);
spin_right_90() // }
{ //
set_wheel_direction(DIRECTION_RIGHT); // set_wheel_speed_synced(90u);
//
set_wheel_speed_synced(90u); // g_use_pid = false;
//
float initial = g_motor_right.speed.distance_cm; // for (;;)
for (;;) // {
{ // if (initial_yaw == target_yaw)
if (g_motor_right.speed.distance_cm - initial >= 7.5f) // {
{ // set_wheel_speed_synced(0u);
set_wheel_speed_synced(0u); // break;
break; // }
} // vTaskDelay(pdMS_TO_TICKS(5));
vTaskDelay(pdMS_TO_TICKS(5)); // }
} // g_use_pid = true;
vTaskDelay(pdMS_TO_TICKS(1000)); //}
g_motor_right.speed.distance_cm = initial;
g_motor_left.speed.distance_cm = initial;
}
void
spin_to_yaw(float target_yaw)
{
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_LEFT);
}
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_RIGHT);
}
set_wheel_speed_synced(90u);
g_use_pid = false;
for (;;)
{
if (initial_yaw == target_yaw)
{
set_wheel_speed_synced(0u);
break;
}
vTaskDelay(pdMS_TO_TICKS(5));
}
g_use_pid = true;
}

47
frtos/motor/motor_pid.h
View File

@ -36,6 +36,19 @@ compute_pid(float *integral, float *prev_error)
return control_signal; return control_signal;
} }
float
compute_i_controller(float *integral)
{
float error
= g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm;
printf("%f\n", error);
*integral += error;
return g_motor_right.pid.ki_value * (*integral);
}
bool bool
repeating_pid_handler(__unused struct repeating_timer *t) repeating_pid_handler(__unused struct repeating_timer *t)
{ {
@ -66,8 +79,38 @@ repeating_pid_handler(__unused struct repeating_timer *t)
g_motor_right.pwm.channel, g_motor_right.pwm.channel,
g_motor_right.pwm.level); g_motor_right.pwm.level);
// printf("speed: %f cm/s\n", g_motor_right.speed.current_cms); // printf("speed: %f cm/s\n", g_motor_right.speed.current_cms);
// printf("distance: %f cm\n", g_motor_right.speed.distance_cm); // printf("distance: %f cm\n", g_motor_right.speed.distance_cm);
return true;
}
bool
repeating_i_handler(__unused struct repeating_timer *t)
{
static float integral = 0.0f;
if (!g_use_pid)
{
integral = 0.0f; // reset once disabled
return true;
}
float control_signal = compute_i_controller(&integral);
float temp = (float)g_motor_right.pwm.level + control_signal * 0.05f;
if (temp > MAX_PWM_LEVEL)
{
temp = MAX_PWM_LEVEL;
}
if (temp <= MIN_PWM_LEVEL)
{
temp = MIN_PWM_LEVEL;
}
set_wheel_speed((uint32_t)temp, &g_motor_right);
return true; return true;
} }

49
frtos/motor/motor_speed.h
View File

@ -73,3 +73,52 @@ 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)
//{
// if (pwm_level > MAX_PWM_LEVEL)
// {
// pwm_level = MAX_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
// * 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;
//}

7
frtos/motor/motor_test.c
View File

@ -10,12 +10,14 @@ motor_control_task(__unused void *params)
for (;;) for (;;)
{ {
set_wheel_direction(DIRECTION_FORWARD); set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90); set_wheel_speed(90u, &g_motor_left);
set_wheel_speed(90u, &g_motor_right);
vTaskDelay(pdMS_TO_TICKS(10000)); vTaskDelay(pdMS_TO_TICKS(10000));
revert_wheel_direction(); revert_wheel_direction();
set_wheel_speed_synced(90); set_wheel_speed(90u, &g_motor_left);
set_wheel_speed(90u, &g_motor_right);
vTaskDelay(pdMS_TO_TICKS(10000)); vTaskDelay(pdMS_TO_TICKS(10000));
} }
@ -37,6 +39,7 @@ launch()
// PID timer // PID timer
struct repeating_timer pid_timer; struct repeating_timer pid_timer;
add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer); add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer);
// add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer);
// Left wheel // Left wheel
// //