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

use single slice number; change PID to match left pwm with right distance travelled.

This commit is contained in:
Richie 2023-11-21 20:24:32 +08:00
parent 7b55278eda
commit c7b9625983
6 changed files with 54 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
frtos/config/motor_config.h
View File

@ -6,6 +6,8 @@
#define PWM_PIN_RIGHT 1U // chanel B
#define PWM_PIN_LEFT 0U // chanel A
#define SLICE_NUM 0U
// IN1, IN2, IN3, IN4 on the L298N
#define DIRECTION_PIN_RIGHT_IN1 11U
#define DIRECTION_PIN_RIGHT_IN2 12U
@ -65,7 +67,6 @@ typedef struct
*/
typedef struct
{
uint slice_num;
uint channel;
uint16_t level;
} motor_pwm_t;

31
frtos/motor/motor_direction.h
View File

@ -49,7 +49,8 @@ revert_wheel_direction()
* @param range acceptable range
* @return true if the current direction is within the range of the target
*/
bool check_direction(float current_direction, float target_direction, float range)
bool
check_direction(float current_direction, float target_direction, float range)
{
// Normalize directions to be within 0 to 360 degrees
current_direction = fmod(current_direction, 360.0f);
@ -67,6 +68,34 @@ bool check_direction(float current_direction, float target_direction, float rang
return false;
}
void
turn_by_distance(uint32_t direction,
float distance,
uint32_t pwm_level,
car_struct_t *pp_car_struct)
{
pp_car_struct->p_pid->use_pid = false;
set_wheel_direction(direction);
distance_to_stop(pp_car_struct, distance);
set_wheel_direction(DIRECTION_MASK);
set_wheel_speed_synced(0u, pp_car_struct);
}
/*!
* @brief Diameter 12.5cm; circumference 39.269908169872416cm,
* 10 degree = circumference / 36 = 1.090831882496456cm
*/
void
turn_by_10_degree(uint32_t direction,
uint32_t pwm_level,
car_struct_t *pp_car_struct)
{
turn_by_distance(direction, 1.090831882496456f, pwm_level, pp_car_struct);
}
/*!
* @brief Spin the car to a certain yaw specifically
* @param direction The direction to turn or spin

14
frtos/motor/motor_init.h
View File

@ -76,24 +76,16 @@ motor_init(car_struct_t *car_struct)
gpio_set_function(PWM_PIN_LEFT, GPIO_FUNC_PWM);
gpio_set_function(PWM_PIN_RIGHT, GPIO_FUNC_PWM);
car_struct->p_left_motor->pwm.slice_num
= pwm_gpio_to_slice_num(PWM_PIN_LEFT);
car_struct->p_right_motor->pwm.slice_num
= pwm_gpio_to_slice_num(PWM_PIN_RIGHT);
// NOTE: PWM clock is 125MHz for raspberrypi pico w by default
// 125MHz / 50 = 2500kHz
pwm_set_clkdiv(car_struct->p_left_motor->pwm.slice_num, PWM_CLK_DIV);
pwm_set_clkdiv(car_struct->p_right_motor->pwm.slice_num, PWM_CLK_DIV);
pwm_set_clkdiv(SLICE_NUM, PWM_CLK_DIV);
// L289N can accept up to 40kHz
// 2500kHz / 100 = 25kHz
pwm_set_wrap(car_struct->p_left_motor->pwm.slice_num, (PWM_WRAP - 1U));
pwm_set_wrap(car_struct->p_right_motor->pwm.slice_num, (PWM_WRAP - 1U));
pwm_set_wrap(SLICE_NUM, (PWM_WRAP - 1U));
pwm_set_enabled(car_struct->p_left_motor->pwm.slice_num, true);
pwm_set_enabled(car_struct->p_right_motor->pwm.slice_num, true);
pwm_set_enabled(SLICE_NUM, true);
}
/*!

21
frtos/motor/motor_pid.h
View File

@ -18,16 +18,18 @@
float
compute_pid(float *integral, float *prev_error, car_struct_t *car_struct)
{
float error = car_struct->p_left_motor->speed.distance_cm
- car_struct->p_right_motor->speed.distance_cm;
// float error = car_struct->p_left_motor->speed.distance_cm
// - car_struct->p_right_motor->speed.distance_cm;
float error = car_struct->p_right_motor->speed.distance_cm
- car_struct->p_left_motor->speed.distance_cm;
printf("error: %f\n", error);
*integral += error;
float derivative = error - *prev_error;
float control_signal
= car_struct->p_pid->kp_value * error
float control_signal = car_struct->p_pid->kp_value * error
+ car_struct->p_pid->ki_value * (*integral)
+ car_struct->p_pid->kd_value * derivative;
@ -36,7 +38,6 @@ compute_pid(float *integral, float *prev_error, car_struct_t *car_struct)
return control_signal;
}
/*!
* @brief Repeating timer handler for the PID controller
* @param ppp_timer The repeating timer
@ -57,8 +58,12 @@ repeating_pid_handler(struct repeating_timer *ppp_timer)
float control_signal = compute_pid(&integral, &prev_error, car_strut);
// float temp
// = (float)car_strut->p_right_motor->pwm.level + control_signal *
// 0.05f;
float temp
= (float)car_strut->p_right_motor->pwm.level + control_signal * 0.05f;
= (float)car_strut->p_left_motor->pwm.level + control_signal * 0.05f;
if (temp > MAX_PWM_LEVEL)
{
@ -70,8 +75,8 @@ repeating_pid_handler(struct repeating_timer *ppp_timer)
temp = MIN_PWM_LEVEL;
}
set_wheel_speed((uint32_t)temp, car_strut->p_right_motor);
// set_wheel_speed((uint32_t)temp, car_strut->p_right_motor);
set_wheel_speed((uint32_t)temp, car_strut->p_left_motor);
return true;
}

4
frtos/motor/motor_speed.h
View File

@ -91,7 +91,7 @@ set_wheel_speed(uint32_t pwm_level, motor_t *p_motor)
p_motor->pwm.level = pwm_level;
pwm_set_chan_level(
p_motor->pwm.slice_num, p_motor->pwm.channel, p_motor->pwm.level);
SLICE_NUM, p_motor->pwm.channel, p_motor->pwm.level);
}
/*!
@ -102,7 +102,7 @@ set_wheel_speed(uint32_t pwm_level, motor_t *p_motor)
void
set_wheel_speed_synced(uint32_t pwm_level, car_struct_t *pp_car_strut)
{
set_wheel_speed(pwm_level, pp_car_strut->p_left_motor);
set_wheel_speed(MAX_PWM_LEVEL, pp_car_strut->p_left_motor);
set_wheel_speed(pwm_level, pp_car_strut->p_right_motor);
}

11
frtos/motor/motor_test.c
View File

@ -7,15 +7,8 @@ motor_control_task(void *params)
car_struct_t *car_struct = (car_struct_t *)params;
for (;;)
{
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct);
vTaskDelay(pdMS_TO_TICKS(10000));
revert_wheel_direction();
set_wheel_speed_synced(90u, car_struct);
vTaskDelay(pdMS_TO_TICKS(10000));
turn_by_10_degree(RIGHT, 90u, car_struct);
vTaskDelay(pdMS_TO_TICKS(1000));
}
}