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

update PWM and PID value

This commit is contained in:
Richie 2023-11-06 23:46:47 +08:00
parent 389c4ecd92
commit c19b665e0b
5 changed files with 47 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
frtos/config/motor_config.h
View File

@ -28,11 +28,11 @@
#define SPEED_PIN_RIGHT 15U
#define SPEED_PIN_LEFT 16U
#define PWM_CLK_DIV 250.f
#define PWM_WRAP 5000U
#define PWM_CLK_DIV 50.f
#define PWM_WRAP 100U
#define MAX_SPEED 4900U
#define MIN_SPEED 0U // To be changed
#define MAX_PWM_LEVEL 99U
#define MIN_PWM_LEVEL 0U
#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
#define WHEEL_CONTROL_PRIO (tskIDLE_PRIORITY + 1UL)
@ -97,4 +97,6 @@ typedef struct
volatile bool is_running;
} distance_to_stop_t;
volatile bool g_use_pid = true;
#endif /* MOTOR_CONFIG_H */

16
frtos/motor/motor_init.h
View File

@ -23,12 +23,17 @@ motor_t g_motor_left = { .pwm.level = 0u,
.pwm.channel = PWM_CHAN_A,
.speed.distance_cm = 0.0f };
// classic ziegler nichols method
// Ku = 1000, Tu = 0.9s, interval = 0.05s
// Kp = 0.6 * Ku = 600
// Ki = 2 * Kp * 0.05 / Tu = 66.67
// Kd = Kp * Tu * 0.05 / 8 = 1350
motor_t g_motor_right = { .pwm.level = 0u,
.pwm.channel = PWM_CHAN_B,
.speed.distance_cm = 0.0f,
.pid.kp_value = 1000.f,
.pid.ki_value = 0.0f,
.pid.kd_value = 10000.0f,};
.pid.kp_value = 600.f,
.pid.ki_value = 66.67f,
.pid.kd_value = 1350.f,};
void
motor_init(void)
@ -62,11 +67,12 @@ motor_init(void)
// NOTE: PWM clock is 125MHz for raspberrypi pico w by default
// 125MHz / 250 = 500kHz
// 125MHz / 50 = 2500kHz
pwm_set_clkdiv(g_motor_left.pwm.slice_num, PWM_CLK_DIV);
pwm_set_clkdiv(g_motor_right.pwm.slice_num, PWM_CLK_DIV);
// have them to be 500kHz / 5000 = 100Hz
// L289N can accept up to 40kHz
// 2500kHz / 100 = 25kHz
pwm_set_wrap(g_motor_left.pwm.slice_num, (PWM_WRAP - 1U));
pwm_set_wrap(g_motor_right.pwm.slice_num, (PWM_WRAP - 1U));

16
frtos/motor/motor_pid.h
View File

@ -21,7 +21,7 @@ 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);
printf("%f\n", error);
*integral += error;
@ -42,7 +42,7 @@ repeating_pid_handler(__unused struct repeating_timer *t)
static float integral = 0.0f;
static float prev_error = 0.0f;
if ((g_motor_left.pwm.level == 0u) || (g_motor_right.pwm.level == 0u))
if (!g_use_pid)
{
return true;
}
@ -51,14 +51,14 @@ repeating_pid_handler(__unused struct repeating_timer *t)
float temp = (float)g_motor_right.pwm.level + control_signal * 0.05f;
if (temp > MAX_SPEED)
if (temp > MAX_PWM_LEVEL)
{
temp = MAX_SPEED;
temp = MAX_PWM_LEVEL;
}
if (temp < MIN_SPEED)
if (temp <= MIN_PWM_LEVEL)
{
temp = MIN_SPEED + 1u;
temp = MIN_PWM_LEVEL + 1u;
}
g_motor_right.pwm.level = (uint16_t)temp;
@ -66,8 +66,8 @@ repeating_pid_handler(__unused struct repeating_timer *t)
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);
// 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;
}

7
frtos/motor/motor_speed.h
View File

@ -86,11 +86,16 @@ set_wheel_speed(uint32_t pwm_level, motor_t * motor)
/*!
* @brief Set the speed of the wheels
* @param pwm_level The pwm_level of the wheels, from 0 to 5000
* @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);
}

16
frtos/motor/motor_test.c
View File

@ -4,6 +4,22 @@
#include "motor_direction.h"
#include "motor_pid.h"
void
motor_control_task(__unused void *params)
{
for (;;)
{
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90);
vTaskDelay(pdMS_TO_TICKS(10000));
revert_wheel_direction();
set_wheel_speed_synced(90);
vTaskDelay(pdMS_TO_TICKS(10000));
}
}
void
launch()