diff --git a/application/chassis/balance.c b/application/chassis/balance.c index 70018ec..b9b951b 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -4,7 +4,8 @@ #include "robot_def.h" #include "general_def.h" #include "ins_task.h" -#include "HT04.h" +// #include "HT04.h" +#include "dmmotor.h" #include "LK9025.h" #include "controller.h" #include "can_comm.h" @@ -31,7 +32,7 @@ static Chassis_Ctrl_Cmd_s chassis_cmd_recv; static Chassis_Upload_Data_s chassis_feedback_data; // 底盘反馈数据 static Chassis_Can_Comm chassis_can_recv; // 四个关节电机和两个驱动轮电机 -static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试 +static DMMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试 static LKMotorInstance *l_driven, *r_driven, *driven[2]; // 两个腿的参数,0为左腿,1为右腿 @@ -102,19 +103,19 @@ void BalanceInit() .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, }, - .motor_type = HT04}; + .motor_type = DM8009P}; joint_conf.can_init_config.tx_id = 1; joint_conf.can_init_config.rx_id = 11; - joint[LF] = lf = HTMotorInit(&joint_conf); + joint[LF] = lf = DMMotorInit(&joint_conf); joint_conf.can_init_config.tx_id = 2; joint_conf.can_init_config.rx_id = 12; - joint[LB] = lb = HTMotorInit(&joint_conf); + joint[LB] = lb = DMMotorInit(&joint_conf); joint_conf.can_init_config.tx_id = 3; joint_conf.can_init_config.rx_id = 13; - joint[RF] = rf = HTMotorInit(&joint_conf); + joint[RF] = rf = DMMotorInit(&joint_conf); joint_conf.can_init_config.tx_id = 4; joint_conf.can_init_config.rx_id = 14; - joint[RB] = rb = HTMotorInit(&joint_conf); + joint[RB] = rb = DMMotorInit(&joint_conf); // 驱动轮电机 Motor_Init_Config_s driven_conf = { @@ -206,7 +207,7 @@ void BalanceInit() static void EnableAllMotor() /* 打开所有电机 */ { for (uint8_t i = 0; i < JOINT_CNT; i++) // 打开关节电机 - HTMotorEnable(joint[i]); + DMMotorEnable(joint[i]); for (uint8_t i = 0; i < DRIVEN_CNT; i++) // 打开驱动电机 LKMotorEnable(driven[i]); } @@ -293,22 +294,22 @@ static void ResetChassis() LKMotorSetRef(r_driven, -chassis_cmd_recv.vx + (float)chassis_cmd_recv.rotate_w); // 若关节完成复位,进入ready态 - if (abs(lf->measure.total_angle) < 0.05 && - abs(lb->measure.total_angle) < 0.05 && - abs(rf->measure.total_angle) < 0.05 && - abs(rb->measure.total_angle) < 0.05) + if (abs(lf->measure.total_round) < 0.05 && + abs(lb->measure.total_round) < 0.05 && + abs(rf->measure.total_round) < 0.05 && + abs(rb->measure.total_round) < 0.05) { chassis_status = ROBOT_READY; // 底盘已经准备好重新站立 } - else if (abs(lf->measure.total_angle) <= 0.03 && - abs(lb->measure.total_angle) <= 0.03 && - abs(rf->measure.total_angle) <= 0.03 && - abs(rb->measure.total_angle) <= 0.03) + else if (abs(lf->measure.total_round) <= 0.03 && + abs(lb->measure.total_round) <= 0.03 && + abs(rf->measure.total_round) <= 0.03 && + abs(rb->measure.total_round) <= 0.03) { // 双阈值保证关节能够复位而不会进入死区 chassis_status = ROBOT_READY; // 底盘已经准备好重新站立 for (uint8_t i = 0; i < JOINT_CNT; i++) - HTMotorOuterLoop(joint[i], OPEN_LOOP); // 改回直接开环扭矩输入,让电调对扭矩闭环 + DMMotorOuterLoop(joint[i], OPEN_LOOP); // 改回直接开环扭矩输入,让电调对扭矩闭环 return; // 退出函数不再执行关节指令 } @@ -318,8 +319,8 @@ static void ResetChassis() // 还在复位中,关节改为位置环,执行复位 for (uint8_t i = 0; i < JOINT_CNT; i++) { - HTMotorOuterLoop(joint[i], ANGLE_LOOP); - HTMotorSetRef(joint[i], 0); + DMMotorOuterLoop(joint[i], ANGLE_LOOP); + DMMotorSetRef(joint[i], 0); } } @@ -342,7 +343,7 @@ static void WokingStateSet() // chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw; for (uint8_t i = 0; i < JOINT_CNT; i++) - HTMotorStop(joint[i]); + DMMotorStop(joint[i]); for (uint8_t i = 0; i < DRIVEN_CNT; i++) LKMotorStop(driven[i]); return; // 关闭所有电机,发送的指令为零 @@ -352,7 +353,7 @@ static void WokingStateSet() EnableAllMotor(); // 保证关节电机为开环扭矩控制 for (uint8_t i = 0; i < JOINT_CNT; i++) - HTMotorOuterLoop(joint[i], OPEN_LOOP); + DMMotorOuterLoop(joint[i], OPEN_LOOP); // 设置目标速度/腿长/距离 l_side.target_len += 0.00001f*(float)chassis_cmd_recv.delta_leglen; @@ -396,16 +397,16 @@ static void ParamAssemble() chassis.roll_w = Chassis_IMU_data->Gyro[1]; // HT04电机的角度是顺时针为正,LK9025电机的角度是逆时针为正 - l_side.phi1 = PI + LIMIT_LINK_RAD - lb->measure.total_angle; - l_side.phi1_w = -lb->measure.speed_rads; - l_side.phi4 = -lf->measure.total_angle - LIMIT_LINK_RAD; - l_side.phi4_w = -lf->measure.speed_rads; + l_side.phi1 = PI + LIMIT_LINK_RAD - lb->measure.total_round; + l_side.phi1_w = -lb->measure.velocity;// 注意速度的正负,HT04电机顺时针旋转为正,而模型中左腿前关节顺时针旋转为负 原本是speed_rads + l_side.phi4 = -lf->measure.total_round - LIMIT_LINK_RAD; + l_side.phi4_w = -lf->measure.velocity; l_side.w_ecd = l_driven->measure.speed_rads; - r_side.phi1 = PI + LIMIT_LINK_RAD + rb->measure.total_angle; - r_side.phi1_w = rb->measure.speed_rads; - r_side.phi4 = rf->measure.total_angle - LIMIT_LINK_RAD; - r_side.phi4_w = rf->measure.speed_rads; + r_side.phi1 = PI + LIMIT_LINK_RAD + rb->measure.total_round; + r_side.phi1_w = rb->measure.velocity; + r_side.phi4 = rf->measure.total_round - LIMIT_LINK_RAD; + r_side.phi4_w = rf->measure.velocity; r_side.w_ecd = -r_driven->measure.speed_rads; } @@ -451,10 +452,11 @@ static void LegControl() /* 腿长控制和Roll补偿 */ static void WattLimitSet() /* 设定运动模态的输出 */ { - HTMotorSetRef(lf, 0.2857f * -l_side.T_front); // 根据扭矩常数计算得到的系数 - HTMotorSetRef(lb, 0.2857f * -l_side.T_back); - HTMotorSetRef(rf, 0.2857f * r_side.T_front); - HTMotorSetRef(rb, 0.2857f * r_side.T_back); + DMMotorSetRef(lf, 0.2857f * -l_side.T_front); // 根据扭矩常数计算得到的系数 todo 需修改 + DMMotorSetRef(lb, 0.2857f * -l_side.T_back); + DMMotorSetRef(rf, 0.2857f * r_side.T_front); + DMMotorSetRef(rb, 0.2857f * r_side.T_back); + LKMotorSetRef(l_driven, 195.3125 * l_side.T_wheel); LKMotorSetRef(r_driven, 195.3125 * -r_side.T_wheel); } diff --git a/application/robot_def.h b/application/robot_def.h index 2f4c4db..776457a 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -18,8 +18,8 @@ /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ // #define ONE_BOARD // 单板控制整车 -// #define CHASSIS_BOARD //底盘板 -#define GIMBAL_BOARD //云台板 +#define CHASSIS_BOARD //底盘板 +// #define GIMBAL_BOARD //云台板 #define VISION_USE_VCP // 使用虚拟串口发送视觉数据 // #define VISION_USE_UART // 使用串口发送视觉数据 diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index d73476e..8bd3cd2 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -106,6 +106,7 @@ typedef enum M2006, LK9025, HT04, + DM8009P, } Motor_Type_e; /**