54 lines
1.8 KiB
C
54 lines
1.8 KiB
C
#include "chassis_control.h"
|
||
#include "CAN_receive.h"
|
||
|
||
static pid_t pid_motor1;
|
||
static pid_t pid_motor2;
|
||
static pid_t pid_motor3;
|
||
static pid_t pid_motor4;
|
||
|
||
static int16_t target_speed_m1;
|
||
static int16_t target_speed_m2;
|
||
static int16_t target_speed_m3;
|
||
static int16_t target_speed_m4;
|
||
|
||
/**
|
||
* @brief 初始化底盘电机的PID
|
||
* 从左到右依次是：PID作用模块，KP,KI,KD,电机最大输出，电机最大限制输出
|
||
*/
|
||
void chassis_control_init(void)
|
||
{
|
||
pid_init(&pid_motor1, 15.0f, 0.1f, 2.0f, 16384.0f, 1000.0f);//底盘电机1
|
||
pid_init(&pid_motor2, 15.0f, 0.1f, 2.0f, 16384.0f, 1000.0f);//底盘电机2
|
||
pid_init(&pid_motor3, 15.0f, 0.1f, 2.0f, 16384.0f, 1000.0f);//底盘电机3
|
||
pid_init(&pid_motor4, 15.0f, 0.1f, 2.0f, 16384.0f, 1000.0f);//底盘电机4
|
||
}
|
||
|
||
/**
|
||
* @brief 控制底盘电机的转速
|
||
*/
|
||
void chassis_control_task(void)
|
||
{
|
||
const motor_measure_t *m1 = get_chassis_motor_measure_point(0);
|
||
const motor_measure_t *m2 = get_chassis_motor_measure_point(1);
|
||
const motor_measure_t *m3 = get_chassis_motor_measure_point(2);
|
||
const motor_measure_t *m4 = get_chassis_motor_measure_point(3);
|
||
|
||
//pid运算
|
||
int16_t cur1 = (int16_t)pid_calc(&pid_motor1, target_speed_m1, m1->speed_rpm);
|
||
int16_t cur2 = (int16_t)pid_calc(&pid_motor2, target_speed_m2, m2->speed_rpm);
|
||
int16_t cur3 = (int16_t)pid_calc(&pid_motor3, target_speed_m3, m3->speed_rpm);
|
||
int16_t cur4 = (int16_t)pid_calc(&pid_motor4, target_speed_m4, m4->speed_rpm);
|
||
|
||
CAN_cmd_chassis(cur1, cur2, cur3, cur4);
|
||
}
|
||
|
||
/**
|
||
* @brief 设置电机转速的电流值，范围（0~16384）
|
||
*/
|
||
void chassis_set_speed(int16_t m1, int16_t m2, int16_t m3, int16_t m4)
|
||
{
|
||
target_speed_m1 = m1;
|
||
target_speed_m2 = m2;
|
||
target_speed_m3 = m3;
|
||
target_speed_m4 = m4;
|
||
} |