224 lines
7.9 KiB
C
224 lines
7.9 KiB
C
/**
|
|
* @file chassis.c
|
|
* @author NeoZeng neozng1@hnu.edu.cn
|
|
* @brief 底盘应用,负责接收robot_cmd的控制命令并根据命令进行运动学解算,得到输出
|
|
* 注意底盘采取右手系,对于平面视图,底盘纵向运动的正前方为x正方向;横向运动的右侧为y正方向
|
|
*
|
|
* @version 0.1
|
|
* @date 2022-12-04
|
|
*
|
|
* @copyright Copyright (c) 2022
|
|
*
|
|
*/
|
|
|
|
#include "chassis.h"
|
|
#include "robot_def.h"
|
|
#include "super_cap.h"
|
|
#include "dji_motor.h"
|
|
#include "message_center.h"
|
|
#include "referee_task.h"
|
|
|
|
#include "general_def.h"
|
|
#include "bsp_dwt.h"
|
|
#include "referee_UI.h"
|
|
#include "arm_math.h"
|
|
|
|
|
|
|
|
/* 根据robot_def.h中的macro自动计算的参数 */
|
|
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距
|
|
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f) // 半轮距
|
|
#define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长
|
|
|
|
#ifdef ONE_BOARD
|
|
static Publisher_t *chassis_pub; // 用于发布底盘的数据
|
|
static Subscriber_t *chassis_sub; // 用于订阅底盘的控制命令
|
|
#endif
|
|
static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令
|
|
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据
|
|
|
|
static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
|
|
|
|
|
|
/* 用于自旋变速策略的时间变量 */
|
|
// static float t;
|
|
|
|
/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
|
|
static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘
|
|
static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
|
|
|
|
void ChassisInit()
|
|
{
|
|
// 四个轮子的参数一样,改tx_id和反转标志位即可
|
|
Motor_Init_Config_s chassis_motor_config = {
|
|
.can_init_config.can_handle = &hcan1,
|
|
.controller_param_init_config = {
|
|
.angle_PID = {
|
|
.Kp = 50,
|
|
.Ki = 150,
|
|
.Kd = 0.3,
|
|
.IntegralLimit = 1000,
|
|
.Derivative_LPF_RC = 0.008,
|
|
.Output_LPF_RC = 0.005,
|
|
.CoefA= 10.0f,
|
|
.CoefB = 3.0f,
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement |PID_ChangingIntegrationRate | PID_DerivativeFilter | PID_OutputFilter,
|
|
.MaxOut = 1000,
|
|
},
|
|
.speed_PID = {
|
|
.Kp = 12,
|
|
.Ki = 0,
|
|
.Kd = 0.08,
|
|
.IntegralLimit = 16000,
|
|
.Derivative_LPF_RC = 0.008,
|
|
.Output_LPF_RC = 0.001,
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_OutputFilter,
|
|
.MaxOut = 16000,
|
|
},
|
|
|
|
},
|
|
.controller_setting_init_config = {
|
|
.angle_feedback_source = MOTOR_FEED,
|
|
.speed_feedback_source = MOTOR_FEED,
|
|
.outer_loop_type = ANGLE_LOOP, // 设置为开环，电机设定值由下面的功率控制设定，不走普通的pid
|
|
.close_loop_type = SPEED_LOOP | ANGLE_LOOP, // 角度环和速度环
|
|
},
|
|
.motor_type = GM6020_NEW,
|
|
};
|
|
// @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
|
|
// 使用功率控制的电机需要使用PowerControlInit()函数初始化,因为电机的控制方式不同
|
|
// chassis_motor_config.can_init_config.tx_id = 1;
|
|
// chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
|
// motor_lf = DJIMotorInit(&chassis_motor_config);
|
|
|
|
// chassis_motor_config.can_init_config.tx_id = 2;
|
|
// chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
|
// motor_rf = DJIMotorInit(&chassis_motor_config);
|
|
|
|
chassis_motor_config.can_init_config.tx_id = 1;
|
|
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
|
// motor_lb = DJIMotorInit(&chassis_motor_config);
|
|
Motor_Init_Config_s pitch_motor_config = {
|
|
.can_init_config.can_handle = &hcan1,
|
|
.can_init_config.tx_id = 2, // 俯仰电机的ID
|
|
.controller_param_init_config = {
|
|
.angle_PID = {
|
|
.Kp = 30,
|
|
.Ki = 80,
|
|
.Kd = 0.2,
|
|
.IntegralLimit = 1000,
|
|
.Derivative_LPF_RC = 0.008,
|
|
.Output_LPF_RC = 0.005,
|
|
.CoefA= 20.0f,
|
|
.CoefB = 10.0f,
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement |PID_ChangingIntegrationRate | PID_DerivativeFilter | PID_OutputFilter,
|
|
.MaxOut = 1000,
|
|
},
|
|
.speed_PID = {
|
|
.Kp = 8,
|
|
.Ki = 0,
|
|
.Kd = 0.02,
|
|
.IntegralLimit = 16000,
|
|
.Derivative_LPF_RC = 0.008,
|
|
.Output_LPF_RC = 0.001,
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_OutputFilter,
|
|
.MaxOut = 16000,
|
|
},
|
|
|
|
},
|
|
.controller_setting_init_config = {
|
|
.angle_feedback_source = MOTOR_FEED,
|
|
.speed_feedback_source = MOTOR_FEED,
|
|
.outer_loop_type = ANGLE_LOOP, // 设置为开环，电机设定值由下面的功率控制设定，不走普通的pid
|
|
.close_loop_type = SPEED_LOOP | ANGLE_LOOP, // 角度环和速度环
|
|
},
|
|
.motor_type = GM6020_NEW,
|
|
};
|
|
// motor_rb = DJIMotorInit(&pitch_motor_config);
|
|
|
|
chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
|
|
chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
|
|
}
|
|
|
|
#define LF_CENTER 1
|
|
#define RF_CENTER 1
|
|
#define LB_CENTER 1
|
|
#define RB_CENTER 1
|
|
/**
|
|
* @brief 计算每个轮毂电机的输出,正运动学解算
|
|
* 用宏进行预替换减小开销,运动解算具体过程参考教程
|
|
*/
|
|
static void MecanumCalculate()
|
|
{
|
|
vt_lb = chassis_vx;
|
|
vt_rb = chassis_cmd_recv.wz;
|
|
}
|
|
|
|
/**
|
|
* @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
|
|
*
|
|
*/
|
|
static void LimitChassisOutput()
|
|
{
|
|
// 完成功率限制后进行电机参考输入设定
|
|
DJIMotorSetRef(motor_lb, vt_lb);
|
|
DJIMotorSetRef(motor_rb, vt_rb);
|
|
}
|
|
|
|
/**
|
|
* @brief 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算
|
|
* 对于双板的情况,考虑增加来自底盘板IMU的数据
|
|
*
|
|
*/
|
|
static void EstimateSpeed()
|
|
{
|
|
// 根据电机速度和陀螺仪的角速度进行解算,还可以利用加速度计判断是否打滑(如果有)
|
|
// chassis_feedback_data.vx vy wz =
|
|
// ...
|
|
}
|
|
|
|
/* 机器人底盘控制核心任务 */
|
|
void ChassisTask()
|
|
{
|
|
// 后续增加没收到消息的处理(双板的情况)
|
|
// 获取新的控制信息
|
|
#ifdef ONE_BOARD
|
|
SubGetMessage(chassis_sub, &chassis_cmd_recv);
|
|
#endif
|
|
#ifdef CHASSIS_BOARD
|
|
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
|
|
#endif // CHASSIS_BOARD
|
|
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
|
|
{ // 如果出现重要模块离线或遥控器设置为急停,让电机停止
|
|
DJIMotorStop(motor_lb);
|
|
DJIMotorStop(motor_rb);
|
|
}
|
|
else
|
|
{ // 正常工作
|
|
|
|
DJIMotorEnable(motor_rb);
|
|
DJIMotorEnable(motor_lb);
|
|
}
|
|
|
|
// 根据控制模式设定旋转速度
|
|
switch (chassis_cmd_recv.chassis_mode)
|
|
{
|
|
default:
|
|
break;
|
|
}
|
|
chassis_vx = chassis_cmd_recv.vx;
|
|
|
|
// 根据控制模式进行正运动学解算,计算底盘输出
|
|
MecanumCalculate();
|
|
|
|
// 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
|
|
LimitChassisOutput();
|
|
|
|
// 推送反馈消息
|
|
#ifdef ONE_BOARD
|
|
PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);
|
|
#endif
|
|
#ifdef CHASSIS_BOARD
|
|
CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data);
|
|
#endif // CHASSIS_BOARD
|
|
} |