HFUT_Firmament_EC
/
sentry_chassis_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

简化了电机测量的命名,完成初版平衡底盘的功能编写,待测试方向和符号的正确性

This commit is contained in:
NeoZng 2023-04-14 22:26:33 +08:00
parent 9e364cbaaa
commit 1818edf117
14 changed files with 215 additions and 162 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

132
application/balance_chassis/balance.c
View File

@ -26,12 +26,8 @@ static attitude_t *imu_data;
static SuperCapInstance *super_cap; static SuperCapInstance *super_cap;
static referee_info_t *referee_data; // 裁判系统的数据会被不断更新 static referee_info_t *referee_data; // 裁判系统的数据会被不断更新
// 电机 // 电机
static HTMotorInstance *lf; static HTMotorInstance *lf, *rf, *lb, *rb;
static HTMotorInstance *rf; static LKMotorInstance *l_driven, *r_driven;
static HTMotorInstance *lb;
static HTMotorInstance *rb;
static LKMotorInstance *l_driven;
static LKMotorInstance *r_driven;
// 底盘板和云台板通信 // 底盘板和云台板通信
static CANCommInstance *chassis_comm; // 由于采用多板架构,即使使用C板也有空余串口,可以使用串口通信以获得更高的通信速率并降低阻塞 static CANCommInstance *chassis_comm; // 由于采用多板架构,即使使用C板也有空余串口,可以使用串口通信以获得更高的通信速率并降低阻塞
static Chassis_Ctrl_Cmd_s chassis_cmd_recv; static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
@ -40,27 +36,51 @@ static Chassis_Upload_Data_s chassis_feed_send;
static LinkNPodParam left_side, right_side; static LinkNPodParam left_side, right_side;
static PIDInstance swerving_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量 static PIDInstance swerving_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
static PIDInstance anti_crash_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上 static PIDInstance anti_crash_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上
static PIDInstance leg_length_pid; // 用PD模拟弹簧的传递函数,不需要积分项(弹簧是一个无积分的二阶系统),增益不可过大否则抗外界冲击响应时太"硬" static PIDInstance leg_length_pid; // 用PD模拟弹簧,不要积分(弹簧是无积分二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水平 static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水平
/* ↓↓↓分割出这些函数是为了提高可读性,使得阅读者的逻辑更加顺畅;但实际上这些函数都不长,可以以注释的形式直接加到BalanceTask里↓↓↓*/ /* ↓↓↓分割出这些函数是为了提高可读性,使得阅读者的逻辑更加顺畅;但实际上这些函数都不长,可以以注释的形式直接加到BalanceTask里↓↓↓*/
/** /**
* @brief 将电机和imu的数据组装为LinkNPodParam结构体 * @brief 将电机和imu的数据组装为LinkNPodParam结构体
* @note 由于两个连杆和轮子连接时,只有一个使用了轴承,另外一个直接和电机的定子连接,
* 所以轮子的转速和电机的转速不一致,因此和地面的速度应为电机转速减去杆的转速,得到的才是轮子的转速
* *
* @todo angle direction to be verified
*/ */
static void ParamAssemble() static void ParamAssemble()
{ { // 电机的角度是逆时针为正,左侧全部取反
left_side.phi1 = PI2 - LIMIT_LINK_RAD - lb->measure.total_angle;
left_side.phi4 = -lf->measure.total_angle + LIMIT_LINK_RAD;
left_side.phi1_w = -lb->measure.speed_rads;
left_side.phi4_w = -lf->measure.speed_rads;
left_side.wheel_dist = -l_driven->measure.total_angle / 360 * WHEEL_RADIUS * PI;
left_side.wheel_w = -l_driven->measure.speed_rads;
right_side.phi1 = PI2 - LIMIT_LINK_RAD + rb->measure.total_angle;
right_side.phi4 = rf->measure.total_angle + LIMIT_LINK_RAD;
right_side.phi1_w = rb->measure.speed_rads;
right_side.phi4_w = rf->measure.speed_rads;
right_side.wheel_dist = r_driven->measure.total_angle / 360 * WHEEL_RADIUS * PI;
right_side.wheel_w = r_driven->measure.speed_rads;
if(chassis_cmd_recv.wz != 0) // 若有转向指令,则使用IMU积分得到的位置,否则左右轮位置相反会产生阻碍转向的力矩
{
// left_side.wheel_dist = 0.5*imu_data->Accel[3];
// right_side.wheel_dist = 0.5*imu_data->Accel[3];
}
} }
/** /**
* @brief 根据关节角度和角速度,计算单杆长度和角度以及变化率 * @brief 根据关节角度和角速度,计算单杆长度和角度以及变化率
* @todo 测试两种的效果,留下其中一种;
* 若差分的效果好则不需要VELOCITY_DIFF_VMC内的代码
* *
* @param p 5连杆和腿的参数 * @param p 5连杆和腿的参数
*/ */
static void Link2Pod(LinkNPodParam *p) static void Link2Pod(LinkNPodParam *p)
{ // 拟将功能封装到vmc_project.h中 { // 拟将功能封装到vmc_project.h中
float xD, yD, xB, yB, BD, A0, B0, xC, yC, phi2; float xD, yD, xB, yB, BD, A0, B0, xC, yC, phi2t, phi5t;
xD = JOINT_DISTANCE + THIGH_LEN * arm_cos_f32(p->phi4); xD = JOINT_DISTANCE + THIGH_LEN * arm_cos_f32(p->phi4);
yD = THIGH_LEN * arm_sin_f32(p->phi4); yD = THIGH_LEN * arm_sin_f32(p->phi4);
xB = 0 + THIGH_LEN * arm_cos_f32(p->phi1); xB = 0 + THIGH_LEN * arm_cos_f32(p->phi1);
@ -68,20 +88,16 @@ static void Link2Pod(LinkNPodParam *p)
BD = powf(xD - xB, 2) + powf(yD - yB, 2); BD = powf(xD - xB, 2) + powf(yD - yB, 2);
A0 = 2 * CALF_LEN * (xD - xB); A0 = 2 * CALF_LEN * (xD - xB);
B0 = 2 * CALF_LEN * (yD - yB); B0 = 2 * CALF_LEN * (yD - yB);
p->phi2 = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD); phi2t = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD);
xC = xB + CALF_LEN * arm_cos_f32(p->phi2); xC = xB + CALF_LEN * arm_cos_f32(phi2t);
yC = yB + CALF_LEN * arm_sin_f32(p->phi2); yC = yB + CALF_LEN * arm_sin_f32(phi2t);
#ifdef ANGLE_DIFF_VMC
float p5t = atan2f(yC, xC - JOINT_DISTANCE / 2); // 避免重复计算 p->phi2 = phi2t;
float plt = Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2));
p->phi5_w = (p5t - p->phi5) / balance_dt;
p->pod_len_w = (plt - p->pod_len) / balance_dt;
p->phi5 = p5t;
p->pod_len = plt;
#endif
p->phi5 = atan2f(yC, xC - JOINT_DISTANCE / 2); p->phi5 = atan2f(yC, xC - JOINT_DISTANCE / 2);
p->pod_len = Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2)); p->pod_len = Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2));
p->phi3 = atan2(yC - yD, xC - xD); // 稍后用于计算VMC p->phi3 = atan2(yC - yD, xC - xD); // 稍后用于计算VMC
p->theta = p->phi5 - 0.5 * PI - imu_data->Pitch;
p->height = p->pod_len * arm_cos_f32(p->theta);
#ifdef VELOCITY_DIFF_VMC #ifdef VELOCITY_DIFF_VMC
float phi1_pred = p->phi1 + p->phi1_w * balance_dt; // 预测下一时刻的关节角度(利用关节角速度) float phi1_pred = p->phi1 + p->phi1_w * balance_dt; // 预测下一时刻的关节角度(利用关节角速度)
@ -94,24 +110,46 @@ static void Link2Pod(LinkNPodParam *p)
BD = powf(xD - xB, 2) + powf(yD - yB, 2); BD = powf(xD - xB, 2) + powf(yD - yB, 2);
A0 = 2 * CALF_LEN * (xD - xB); A0 = 2 * CALF_LEN * (xD - xB);
B0 = 2 * CALF_LEN * (yD - yB); B0 = 2 * CALF_LEN * (yD - yB);
phi2 = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD); // 不要用link->phi2,因为这里是预测的 phi2t = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD); // 不要用link->phi2,因为这里是预测的
xC = xB + CALF_LEN * arm_cos_f32(phi2); xC = xB + CALF_LEN * arm_cos_f32(phi2t);
yC = yB + CALF_LEN * arm_sin_f32(phi2); yC = yB + CALF_LEN * arm_sin_f32(phi2t);
phi5t = atan2f(yC, xC - JOINT_DISTANCE / 2);
// 差分计算腿长变化率和腿角速度 // 差分计算腿长变化率和腿角速度
p->pod_w = (atan2f(yC, xC - JOINT_DISTANCE / 2) - p->phi5) / balance_dt; p->phi2_w = (phi2t - p->phi2) / balance_dt; // 稍后用于修正轮速
p->pod_w = (phi5t - p->phi5) / balance_dt;
p->pod_v = (Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2)) - p->pod_len) / balance_dt; p->pod_v = (Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2)) - p->pod_len) / balance_dt;
#endif // VELOCITY_DIFF_VMC p->theta_w = (phi5t - 0.5 * PI - imu_data->Pitch - p->theta) / balance_dt;
p->height_v = p->pod_v * arm_cos_f32(p->theta) - p->pod_len * arm_sin_f32(p->theta) * p->theta_w; // 这很酷!PDE!
#endif
p->wheel_w = (p->wheel_w - p->phi2_w + imu_data->Gyro[3]) * WHEEL_RADIUS; // 修正轮速
} }
/** /**
* @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出 * @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出
* 由于反馈矩阵和控制矩阵都比较稀疏,故不使用矩阵库,避免非零项计算 * 由于反馈矩阵和控制矩阵都比较稀疏,故不使用矩阵库,避免非零项计算
*
* @note 计算得到的腿部力矩输出还要再综合运动控制系统补偿后映射为两个关节电机 * @note 计算得到的腿部力矩输出还要再综合运动控制系统补偿后映射为两个关节电机
* 而轮子的输出则只经过转向PID的反馈增益计算 * 而轮子的输出则只经过转向PID的反馈增益计算
* *
* @todo 确定使用查表还是多项式拟合
*
*/ */
static void CalcLQR(LinkNPodParam *p) static void CalcLQR(LinkNPodParam *p, float target_x)
{ {
float *gain_list = LookUpKgain(p->pod_len); // K11,K12... K21,K22... K26
float T[2]; // 0 T_wheel, 1 T_pod;
for (uint8_t i = 0; i < 2; i++)
{
T[i] = gain_list[i * 6 + 0] * -p->theta +
gain_list[i * 6 + 1] * -p->theta_w +
gain_list[i * 6 + 2] * (target_x - p->wheel_dist) +
gain_list[i * 6 + 3] * -p->wheel_w +
gain_list[i * 6 + 4] * -imu_data->Pitch +
gain_list[i * 6 + 5] * -imu_data->Gyro[3];
}
p->T_wheel = T[0];
p->T_pod = T[1];
} }
/** /**
@ -124,7 +162,7 @@ static void SynthesizeMotion()
left_side.T_pod += anti_crash_pid.Output; left_side.T_pod += anti_crash_pid.Output;
right_side.T_pod -= anti_crash_pid.Output; right_side.T_pod -= anti_crash_pid.Output;
// PIDCalculate(&swerving_pid, imu_data->Yaw, 0); // 对速度闭环还是使用角度增量闭环? PIDCalculate(&swerving_pid, imu_data->Yaw, chassis_cmd_recv.wz); // 对速度闭环还是使用角度增量闭环?
left_side.T_wheel -= swerving_pid.Output; left_side.T_wheel -= swerving_pid.Output;
right_side.T_wheel += swerving_pid.Output; right_side.T_wheel += swerving_pid.Output;
} }
@ -135,7 +173,7 @@ static void SynthesizeMotion()
*/ */
static void LegControl(LinkNPodParam *p, float target_length) static void LegControl(LinkNPodParam *p, float target_length)
{ {
p->F_pod += PIDCalculate(&leg_length_pid, p->pod_len, target_length); p->F_pod += PIDCalculate(&leg_length_pid, p->height, target_length);
} }
/** /**
@ -177,8 +215,14 @@ static void FlyDetect()
* @brief 功率限制,一般不需要 * @brief 功率限制,一般不需要
* *
*/ */
static void WattLimit(LinkNPodParam *p) static void WattLimit()
{ {
HTMotorSetRef(&lf, left_side.T_front);
HTMotorSetRef(&lb, left_side.T_back);
HTMotorSetRef(&rf, right_side.T_front);
HTMotorSetRef(&rb, right_side.T_back);
LKMotorSetRef(&l_driven, left_side.T_wheel);
LKMotorSetRef(&r_driven, right_side.T_wheel);
} }
void BalanceInit() void BalanceInit()
@ -320,13 +364,32 @@ void BalanceTask()
{ {
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chassis_comm); chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chassis_comm);
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
{
HTMotorStop(lf);
HTMotorStop(rf);
HTMotorStop(lb);
HTMotorStop(rb);
LKMotorStop(l_driven);
LKMotorStop(r_driven);
}
else
{
HTMotorEnable(lf);
HTMotorEnable(rf);
HTMotorEnable(lb);
HTMotorEnable(rb);
LKMotorEnable(l_driven);
LKMotorEnable(r_driven);
}
ParamAssemble(); // 参数组装,将电机和IMU的参数组装到一起 ParamAssemble(); // 参数组装,将电机和IMU的参数组装到一起
// 将五连杆映射成单杆 // 将五连杆映射成单杆
Link2Pod(&left_side); Link2Pod(&left_side);
Link2Pod(&right_side); Link2Pod(&right_side);
// 根据单杆计算处的角度和杆长,计算反馈增益 // 根据单杆计算处的角度和杆长,计算反馈增益
CalcLQR(&left_side); CalcLQR(&left_side, chassis_cmd_recv.vx); // @todo,需要确定速度or位置闭环
CalcLQR(&right_side); CalcLQR(&right_side, chassis_cmd_recv.vx);
// 腿长控制 // 腿长控制
LegControl(&left_side, 0); LegControl(&left_side, 0);
LegControl(&right_side, 0); LegControl(&right_side, 0);
@ -339,11 +402,10 @@ void BalanceTask()
VMCProject(&right_side); VMCProject(&right_side);
FlyDetect(); // 滞空检测 FlyDetect(); // 滞空检测
// 电机输出限幅
WattLimit(&left_side); WattLimit(); // 电机输出限幅
WattLimit(&right_side);
// code to go here... 裁判系统,UI,多机通信 // code to go here... 裁判系统,UI,多机通信
CANCommSend(chassis_comm, (uint8_t*)&chassis_feed_send); CANCommSend(chassis_comm, (uint8_t *)&chassis_feed_send);
} }

21
application/balance_chassis/balance.h
View File

@ -4,31 +4,30 @@
#define CALF_LEN 0.25f // 小腿 #define CALF_LEN 0.25f // 小腿
#define THIGH_LEN 0.15f // 大腿 #define THIGH_LEN 0.15f // 大腿
#define JOINT_DISTANCE 0.108f // 关节间距 #define JOINT_DISTANCE 0.108f // 关节间距
#define WHEEL_RADIUS 0.032f // 轮子半径
#define LIMIT_LINK_RAD 0.358 //
#define VELOCITY_DIFF_VMC //通过速度计算增量,然后通过差分计算腿长变化率和腿角速度 // 计算速度的方式(五连杆到单杆的映射)
#define VELOCITY_DIFF_VMC // 通过速度计算增量,然后通过差分计算腿长变化率和腿角速度
// #define ANGLE_DIFF_VMC //直接保存上一次的值,通过差分计算腿长变化率和腿角速度 // #define ANGLE_DIFF_VMC //直接保存上一次的值,通过差分计算腿长变化率和腿角速度
typedef struct typedef struct
{ {
// joint // joint
float phi1_w, phi4_w; float phi1_w, phi4_w, phi2_w; // phi2_w used for calc real wheel speed
float T_back, T_front; float T_back, T_front;
// link angle,phi1-ph5, phi5 is pod angle // link angle,phi1-ph5, phi5 is pod angle
float phi1, phi2, phi3, phi4, phi5; float phi1, phi2, phi3, phi4, phi5;
// wheel // wheel
float wheel_angle; float wheel_dist;
float wheel_w; float wheel_w;
float T_wheel; float T_wheel;
// pod // pod
float theta, theta_w; // 杆和垂直方向的夹角,为控制状态之一
float pod_len; float pod_len;
float pod_w; float height,height_v;
float pod_v; float pod_v, pod_w;
float F_pod; float F_pod, T_pod;
float T_pod;
} LinkNPodParam; } LinkNPodParam;
/** /**

2
application/balance_chassis/gain_table.h
View File

@ -19,7 +19,7 @@ void EnalbeInterpolation(void)
} }
/* 默认关闭插值,向下取整 */ /* 默认关闭插值,向下取整 */
float LookUpKgain(float leg_length) float* LookUpKgain(float leg_length)
{ {
} }

41
application/chassis/chassis.c
View File

@ -47,14 +47,10 @@ static Subscriber_t *chassis_sub; // 用于订阅底盘的控
static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令 static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据 static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据
// static referee_info_t *referee_data; // 裁判系统相关数据
static SuperCapInstance *cap; // 超级电容 static SuperCapInstance *cap; // 超级电容
static DJIMotorInstance *motor_lf; // left right forward back static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
static DJIMotorInstance *motor_rf;
static DJIMotorInstance *motor_lb;
static DJIMotorInstance *motor_rb;
/* 用于自旋变速策略的时间变量,后续考虑查表加速 */ /* 用于自旋变速策略的时间变量 */
// static float t; // static float t;
/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */ /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
@ -68,19 +64,19 @@ void ChassisInit()
.can_init_config.can_handle = &hcan1, .can_init_config.can_handle = &hcan1,
.controller_param_init_config = { .controller_param_init_config = {
.speed_PID = { .speed_PID = {
.Kp = 10,//4.5 .Kp = 10, // 4.5
.Ki = 0,//0 .Ki = 0, // 0
.Kd = 0,//0 .Kd = 0, // 0
.IntegralLimit = 3000, .IntegralLimit = 3000,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.MaxOut = 12000, .MaxOut = 12000,
}, },
.current_PID = { .current_PID = {
.Kp = 0.5,//0.4 .Kp = 0.5, // 0.4
.Ki = 0,//0 .Ki = 0, // 0
.Kd = 0, .Kd = 0,
.IntegralLimit = 3000, .IntegralLimit = 3000,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.MaxOut = 15000, .MaxOut = 15000,
}, },
}, },
@ -114,7 +110,6 @@ void ChassisInit()
// while (referee_data->GameRobotState.robot_id ==0); // while (referee_data->GameRobotState.robot_id ==0);
// Referee_Interactive_init(referee_data); // Referee_Interactive_init(referee_data);
SuperCap_Init_Config_s cap_conf = { SuperCap_Init_Config_s cap_conf = {
.can_config = { .can_config = {
.can_handle = &hcan2, .can_handle = &hcan2,
@ -145,10 +140,10 @@ void ChassisInit()
#endif // ONE_BOARD #endif // ONE_BOARD
} }
#define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD) #define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD) #define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD) #define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD) #define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
/** /**
* @brief 计算每个轮毂电机的输出,正运动学解算 * @brief 计算每个轮毂电机的输出,正运动学解算
* 用宏进行预替换减小开销,运动解算具体过程参考教程 * 用宏进行预替换减小开销,运动解算具体过程参考教程
@ -157,7 +152,7 @@ static void MecanumCalculate()
{ {
vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER; vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER;
vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER; vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER;
vt_lb = chassis_vx - chassis_vy -chassis_cmd_recv.wz * LB_CENTER; vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * LB_CENTER;
vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER; vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
} }
@ -224,10 +219,10 @@ void ChassisTask()
chassis_cmd_recv.wz = 0; chassis_cmd_recv.wz = 0;
break; break;
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
chassis_cmd_recv.wz = -1.5*chassis_cmd_recv.offset_angle*abs(chassis_cmd_recv.offset_angle); chassis_cmd_recv.wz = -1.5 * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
break; break;
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
chassis_cmd_recv.wz=4000; chassis_cmd_recv.wz = 4000;
break; break;
default: default:
break; break;
@ -236,8 +231,8 @@ void ChassisTask()
// 根据云台和底盘的角度offset将控制量映射到底盘坐标系上 // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
// 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东) // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
static float sin_theta, cos_theta; static float sin_theta, cos_theta;
cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD); cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD); sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta; chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta; chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;

31
application/gimbal/gimbal.c
View File

@ -8,15 +8,14 @@
#include "bmi088.h" #include "bmi088.h"
static attitude_t *gimba_IMU_data; // 云台IMU数据 static attitude_t *gimba_IMU_data; // 云台IMU数据
static DJIMotorInstance *yaw_motor; // yaw电机 static DJIMotorInstance *yaw_motor, *pitch_motor;
static DJIMotorInstance *pitch_motor; // pitch电机
static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd) static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd)
static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 static Subscriber_t *gimbal_sub; // cmd控制消息订阅者
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息
BMI088Instance* imu; BMI088Instance *imu;
void GimbalInit() void GimbalInit()
{ {
BMI088_Init_Config_s imu_config = { BMI088_Init_Config_s imu_config = {
@ -54,7 +53,7 @@ void GimbalInit()
.cali_mode = BMI088_CALIBRATE_ONLINE_MODE, .cali_mode = BMI088_CALIBRATE_ONLINE_MODE,
.work_mode = BMI088_BLOCK_PERIODIC_MODE, .work_mode = BMI088_BLOCK_PERIODIC_MODE,
}; };
imu=BMI088Register(&imu_config); imu = BMI088Register(&imu_config);
// gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
// YAW // YAW
Motor_Init_Config_s yaw_config = { Motor_Init_Config_s yaw_config = {
@ -64,26 +63,26 @@ void GimbalInit()
}, },
.controller_param_init_config = { .controller_param_init_config = {
.angle_PID = { .angle_PID = {
.Kp = 8, //8 .Kp = 8, // 8
.Ki = 0, .Ki = 0,
.Kd = 0, .Kd = 0,
.DeadBand = 0.1, .DeadBand = 0.1,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100, .IntegralLimit = 100,
.MaxOut = 500, .MaxOut = 500,
}, },
.speed_PID = { .speed_PID = {
.Kp = 50,//50 .Kp = 50, // 50
.Ki = 200,//200 .Ki = 200, // 200
.Kd = 0, .Kd = 0,
.Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 3000, .IntegralLimit = 3000,
.MaxOut = 20000, .MaxOut = 20000,
}, },
.other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle, .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
.other_speed_feedback_ptr=&gimba_IMU_data->Gyro[2], .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2],
}, },
.controller_setting_init_config = { .controller_setting_init_config = {
.angle_feedback_source = OTHER_FEED, .angle_feedback_source = OTHER_FEED,
@ -109,11 +108,11 @@ void GimbalInit()
.MaxOut = 500, .MaxOut = 500,
}, },
.speed_PID = { .speed_PID = {
.Kp=50,//50 .Kp = 50, // 50
.Ki =350,//350 .Ki = 350, // 350
.Kd =0,//0 .Kd = 0, // 0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit =2500, .IntegralLimit = 2500,
.MaxOut = 20000, .MaxOut = 20000,
}, },
.other_angle_feedback_ptr = &gimba_IMU_data->Pitch, .other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
@ -185,7 +184,7 @@ void GimbalTask()
// 设置反馈数据,主要是imu和yaw的ecd // 设置反馈数据,主要是imu和yaw的ecd
gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->motor_measure.angle_single_round; gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
// 推送消息 // 推送消息
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data); PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);

27
application/shoot/shoot.c
View File

@ -7,9 +7,7 @@
#include "general_def.h" #include "general_def.h"
/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */ /* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
static DJIMotorInstance *friction_l; // 左摩擦轮 static DJIMotorInstance *friction_l, *friction_r, *loader; // 拨盘电机
static DJIMotorInstance *friction_r; // 右摩擦轮
static DJIMotorInstance *loader; // 拨盘电机
// static servo_instance *lid; 需要增加弹舱盖 // static servo_instance *lid; 需要增加弹舱盖
static Publisher_t *shoot_pub; static Publisher_t *shoot_pub;
@ -29,16 +27,16 @@ void ShootInit()
}, },
.controller_param_init_config = { .controller_param_init_config = {
.speed_PID = { .speed_PID = {
.Kp = 0,//20 .Kp = 0, // 20
.Ki = 0,//1 .Ki = 0, // 1
.Kd = 0, .Kd = 0,
.Improve = PID_Integral_Limit, .Improve = PID_Integral_Limit,
.IntegralLimit = 10000, .IntegralLimit = 10000,
.MaxOut = 15000, .MaxOut = 15000,
}, },
.current_PID = { .current_PID = {
.Kp = 0,//0.7 .Kp = 0, // 0.7
.Ki = 0,//0.1 .Ki = 0, // 0.1
.Kd = 0, .Kd = 0,
.Improve = PID_Integral_Limit, .Improve = PID_Integral_Limit,
.IntegralLimit = 10000, .IntegralLimit = 10000,
@ -70,22 +68,22 @@ void ShootInit()
.controller_param_init_config = { .controller_param_init_config = {
.angle_PID = { .angle_PID = {
// 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降 // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
.Kp = 0,//10 .Kp = 0, // 10
.Ki = 0, .Ki = 0,
.Kd = 0, .Kd = 0,
.MaxOut = 200, .MaxOut = 200,
}, },
.speed_PID = { .speed_PID = {
.Kp = 0,//10 .Kp = 0, // 10
.Ki = 0,//1 .Ki = 0, // 1
.Kd = 0, .Kd = 0,
.Improve = PID_Integral_Limit, .Improve = PID_Integral_Limit,
.IntegralLimit = 5000, .IntegralLimit = 5000,
.MaxOut = 5000, .MaxOut = 5000,
}, },
.current_PID = { .current_PID = {
.Kp = 0,//0.7 .Kp = 0, // 0.7
.Ki = 0,//0.1 .Ki = 0, // 0.1
.Kd = 0, .Kd = 0,
.Improve = PID_Integral_Limit, .Improve = PID_Integral_Limit,
.IntegralLimit = 5000, .IntegralLimit = 5000,
@ -142,14 +140,14 @@ void ShootTask()
// 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射) // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用. case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环 DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
DJIMotorSetRef(loader, loader->motor_measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度 DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 150; // 完成1发弹丸发射的时间 dead_time = 150; // 完成1发弹丸发射的时间
break; break;
// 三连发,如果不需要后续可能删除 // 三连发,如果不需要后续可能删除
case LOAD_3_BULLET: case LOAD_3_BULLET:
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环 DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
DJIMotorSetRef(loader, loader->motor_measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发 DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 300; // 完成3发弹丸发射的时间 dead_time = 300; // 完成3发弹丸发射的时间
break; break;
@ -170,7 +168,6 @@ void ShootTask()
; // 未知模式,停止运行,检查指针越界,内存溢出等问题 ; // 未知模式,停止运行,检查指针越界,内存溢出等问题
} }
// 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启) // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
if (shoot_cmd_recv.friction_mode == FRICTION_ON) if (shoot_cmd_recv.friction_mode == FRICTION_ON)
{ {

6
modules/algorithm/user_lib.c
View File

@ -165,7 +165,7 @@ int float_rounding(float raw)
} }
// 三维向量归一化 // 三维向量归一化
float* Norm3d(float* v) float *Norm3d(float *v)
{ {
float len = Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); float len = Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
v[0] /= len; v[0] /= len;
@ -181,7 +181,7 @@ float NormOf3d(float *v)
} }
// 三维向量叉乘v1 x v2 // 三维向量叉乘v1 x v2
void Cross3d(float* v1, float* v2,float* res) void Cross3d(float *v1, float *v2, float *res)
{ {
res[0] = v1[1] * v2[2] - v1[2] * v2[1]; res[0] = v1[1] * v2[2] - v1[2] * v2[1];
res[1] = v1[2] * v2[0] - v1[0] * v2[2]; res[1] = v1[2] * v2[0] - v1[0] * v2[2];
@ -189,7 +189,7 @@ void Cross3d(float* v1, float* v2,float* res)
} }
// 三维向量点乘 // 三维向量点乘
float Dot3d(float* v1, float* v2) float Dot3d(float *v1, float *v2)
{ {
return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]; return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
} }

8
modules/general_def.h
View File

@ -3,15 +3,15 @@
// 一些module的通用数值型定义,注意条件macro兼容,一些宏可能在math.h中已经定义过了 // 一些module的通用数值型定义,注意条件macro兼容,一些宏可能在math.h中已经定义过了
#ifndef PI #ifndef PI
#define PI 3.1415926535f #define PI 3.1415926535f
#endif // !PI #endif
#define PI2 (PI * 2.0f) // 2 pi #define PI2 (PI * 2.0f) // 2 pi
#define RAD_2_ANGLE 57.2957795f // 180/pi #define RAD_2_DEGREE 57.2957795f // 180/pi
#define ANGLE_2_RAD 0.01745329252f // pi/180 #define DEGREE_2_RAD 0.01745329252f // pi/180
#define RPM_2_ANGLE_PER_SEC 6.0f // ×360°/60sec #define RPM_2_ANGLE_PER_SEC 6.0f // ×360°/60sec
#define RPM_2_RAD_PER_SEC 0.104719755f // ×2pi/60sec
#endif // !GENERAL_DEF_H #endif // !GENERAL_DEF_H

15
modules/motor/DJImotor/dji_motor.c
View File

@ -124,7 +124,7 @@ static void DecodeDJIMotor(CANInstance *_instance)
// 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址 // 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
// _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针 // _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针
uint8_t *rxbuff = _instance->rx_buff; uint8_t *rxbuff = _instance->rx_buff;
DJI_Motor_Measure_s *measure = &(((DJIMotorInstance *)_instance->id)->motor_measure); // measure要多次使用,保存指针减小访存开销 DJI_Motor_Measure_s *measure = &(((DJIMotorInstance *)_instance->id)->measure); // measure要多次使用,保存指针减小访存开销
// 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册 // 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册
measure->last_ecd = measure->ecd; measure->last_ecd = measure->ecd;
@ -223,7 +223,7 @@ void DJIMotorControl()
DJIMotorInstance *motor; DJIMotorInstance *motor;
Motor_Control_Setting_s *motor_setting; // 电机控制参数 Motor_Control_Setting_s *motor_setting; // 电机控制参数
Motor_Controller_s *motor_controller; // 电机控制器 Motor_Controller_s *motor_controller; // 电机控制器
DJI_Motor_Measure_s *motor_measure; // 电机测量值 DJI_Motor_Measure_s *measure; // 电机测量值
float pid_measure, pid_ref; // 电机PID测量值和设定值 float pid_measure, pid_ref; // 电机PID测量值和设定值
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值 // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
@ -232,10 +232,10 @@ void DJIMotorControl()
motor = dji_motor_instance[i]; motor = dji_motor_instance[i];
motor_setting = &motor->motor_settings; motor_setting = &motor->motor_settings;
motor_controller = &motor->motor_controller; motor_controller = &motor->motor_controller;
motor_measure = &motor->motor_measure; measure = &motor->measure;
pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改 pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改
if (motor_setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) if (motor_setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
pid_ref*= -1; // 设置反转 pid_ref *= -1; // 设置反转
// pid_ref会顺次通过被启用的闭环充当数据的载体 // pid_ref会顺次通过被启用的闭环充当数据的载体
// 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出 // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP) if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP)
@ -243,7 +243,7 @@ void DJIMotorControl()
if (motor_setting->angle_feedback_source == OTHER_FEED) if (motor_setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_angle_feedback_ptr; pid_measure = *motor_controller->other_angle_feedback_ptr;
else else
pid_measure = motor_measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃 pid_measure = measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
// 更新pid_ref进入下一个环 // 更新pid_ref进入下一个环
pid_ref = PIDCalculate(&motor_controller->angle_PID, pid_measure, pid_ref); pid_ref = PIDCalculate(&motor_controller->angle_PID, pid_measure, pid_ref);
} }
@ -254,7 +254,7 @@ void DJIMotorControl()
if (motor_setting->speed_feedback_source == OTHER_FEED) if (motor_setting->speed_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_speed_feedback_ptr; pid_measure = *motor_controller->other_speed_feedback_ptr;
else // MOTOR_FEED else // MOTOR_FEED
pid_measure = motor_measure->speed_aps; pid_measure = measure->speed_aps;
// 更新pid_ref进入下一个环 // 更新pid_ref进入下一个环
pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref); pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
} }
@ -262,13 +262,12 @@ void DJIMotorControl()
// 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈 // 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
if (motor_setting->close_loop_type & CURRENT_LOOP) if (motor_setting->close_loop_type & CURRENT_LOOP)
{ {
pid_ref = PIDCalculate(&motor_controller->current_PID, motor_measure->real_current, pid_ref); pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
} }
// 获取最终输出 // 获取最终输出
set = (int16_t)pid_ref; set = (int16_t)pid_ref;
// 分组填入发送数据 // 分组填入发送数据
group = motor->sender_group; group = motor->sender_group;
num = motor->message_num; num = motor->message_num;

2
modules/motor/DJImotor/dji_motor.h
View File

@ -49,7 +49,7 @@ typedef struct
typedef struct typedef struct
{ {
DJI_Motor_Measure_s motor_measure; // 电机测量值 DJI_Motor_Measure_s measure; // 电机测量值
Motor_Control_Setting_s motor_settings; // 电机设置 Motor_Control_Setting_s motor_settings; // 电机设置
Motor_Controller_s motor_controller; // 电机控制器 Motor_Controller_s motor_controller; // 电机控制器

16
modules/motor/HTmotor/HT04.c
View File

@ -42,16 +42,16 @@ static void HTMotorDecode(CANInstance *motor_can)
{ {
uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量 uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
uint8_t *rxbuff = motor_can->rx_buff; uint8_t *rxbuff = motor_can->rx_buff;
HTMotor_Measure_t *measure = &((HTMotorInstance *)motor_can->id)->motor_measure; // 将can实例中保存的id转换成电机实例的指针 HTMotor_Measure_t *measure = &((HTMotorInstance *)motor_can->id)->measure; // 将can实例中保存的id转换成电机实例的指针
measure->last_angle = measure->total_angle; measure->last_angle = measure->total_angle;
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]); tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
measure->total_angle = RAD_2_ANGLE * uint_to_float(tmp, P_MIN, P_MAX, 16); measure->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16);
tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4)); tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4));
measure->speed_aps = SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MIN, V_MAX, 12) + measure->speed_rads = SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MIN, V_MAX, 12) +
(1 - SPEED_SMOOTH_COEF) * measure->speed_aps; (1 - SPEED_SMOOTH_COEF) * measure->speed_rads;
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]); tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) + measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
@ -97,7 +97,7 @@ void HTMotorControl()
for (size_t i = 0; i < idx; i++) for (size_t i = 0; i < idx; i++)
{ // 先获取地址避免反复寻址 { // 先获取地址避免反复寻址
motor = ht_motor_instance[i]; motor = ht_motor_instance[i];
measure = &motor->motor_measure; measure = &motor->measure;
setting = &motor->motor_settings; setting = &motor->motor_settings;
motor_can = motor->motor_can_instace; motor_can = motor->motor_can_instace;
pid_ref = motor->pid_ref; pid_ref = motor->pid_ref;
@ -109,7 +109,7 @@ void HTMotorControl()
else else
pid_measure = measure->real_current; pid_measure = measure->real_current;
// measure单位是rad,ref是角度,统一到angle下计算,方便建模 // measure单位是rad,ref是角度,统一到angle下计算,方便建模
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure * RAD_2_ANGLE, pid_ref); pid_ref = PIDCalculate(&motor->angle_PID, pid_measure * RAD_2_DEGREE, pid_ref);
} }
if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)) if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
@ -120,9 +120,9 @@ void HTMotorControl()
if (setting->angle_feedback_source == OTHER_FEED) if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_speed_feedback_ptr; pid_measure = *motor->other_speed_feedback_ptr;
else else
pid_measure = measure->speed_aps; pid_measure = measure->speed_rads;
// measure单位是rad / s ,ref是angle per sec,统一到angle下计算 // measure单位是rad / s ,ref是angle per sec,统一到angle下计算
pid_ref = PIDCalculate(&motor->speed_PID, pid_measure * RAD_2_ANGLE, pid_ref); pid_ref = PIDCalculate(&motor->speed_PID, pid_measure * RAD_2_DEGREE, pid_ref);
} }
if (setting->close_loop_type & CURRENT_LOOP) if (setting->close_loop_type & CURRENT_LOOP)

4
modules/motor/HTmotor/HT04.h
View File

@ -21,14 +21,14 @@ typedef struct // HT04
{ {
float last_angle; float last_angle;
float total_angle; // 角度为多圈角度,范围是-95.5~95.5,单位为rad float total_angle; // 角度为多圈角度,范围是-95.5~95.5,单位为rad
float speed_aps; float speed_rads;
float real_current; float real_current;
} HTMotor_Measure_t; } HTMotor_Measure_t;
/* HT电机类型定义*/ /* HT电机类型定义*/
typedef struct typedef struct
{ {
HTMotor_Measure_t motor_measure; HTMotor_Measure_t measure;
Motor_Control_Setting_s motor_settings; Motor_Control_Setting_s motor_settings;

7
modules/motor/LKmotor/LK9025.c
View File

@ -1,5 +1,6 @@
#include "LK9025.h" #include "LK9025.h"
#include "stdlib.h" #include "stdlib.h"
#include "general_def.h"
static uint8_t idx; static uint8_t idx;
static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL}; static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
@ -23,8 +24,8 @@ static void LKMotorDecode(CANInstance *_instance)
measure->angle_single_round = ECD_ANGLE_COEF_LK * measure->ecd; measure->angle_single_round = ECD_ANGLE_COEF_LK * measure->ecd;
measure->speed_aps = (1 - SPEED_SMOOTH_COEF) * measure->speed_aps + measure->speed_rads = (1 - SPEED_SMOOTH_COEF) * measure->speed_rads +
SPEED_SMOOTH_COEF * (float)((int16_t)(rx_buff[5] << 8 | rx_buff[4])); DEGREE_2_RAD * SPEED_SMOOTH_COEF * (float)((int16_t)(rx_buff[5] << 8 | rx_buff[4]));
measure->real_current = (1 - CURRENT_SMOOTH_COEF) * measure->real_current + measure->real_current = (1 - CURRENT_SMOOTH_COEF) * measure->real_current +
CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2])); CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2]));
@ -101,7 +102,7 @@ void LKMotorControl()
if (setting->angle_feedback_source == OTHER_FEED) if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_speed_feedback_ptr; pid_measure = *motor->other_speed_feedback_ptr;
else else
pid_measure = measure->speed_aps; pid_measure = measure->speed_rads;
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref); pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
if (setting->feedforward_flag & CURRENT_FEEDFORWARD) if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor->current_feedforward_ptr; pid_ref += *motor->current_feedforward_ptr;

3
modules/motor/LKmotor/LK9025.h
View File

@ -14,13 +14,14 @@
#define SPEED_SMOOTH_COEF 0.85f #define SPEED_SMOOTH_COEF 0.85f
#define REDUCTION_RATIO_DRIVEN 1 #define REDUCTION_RATIO_DRIVEN 1
#define ECD_ANGLE_COEF_LK (360.0f / 65536.0f) #define ECD_ANGLE_COEF_LK (360.0f / 65536.0f)
#define CURRENT_TORQUE_COEF_LK 0.003645f // 电流设定值转换成扭矩的系数,算出来的设定值除以这个系数就是扭矩值
typedef struct // 9025 typedef struct // 9025
{ {
uint16_t last_ecd; // 上一次读取的编码器值 uint16_t last_ecd; // 上一次读取的编码器值
uint16_t ecd; // 当前编码器值 uint16_t ecd; // 当前编码器值
float angle_single_round; // 单圈角度 float angle_single_round; // 单圈角度
float speed_aps; // speed angle per sec(degree:°) float speed_rads; // speed rad/s
int16_t real_current; // 实际电流 int16_t real_current; // 实际电流
uint8_t temperate; // 温度,C° uint8_t temperate; // 温度,C°