diff --git a/application/chassis/balance.c b/application/chassis/balance.c index 4c49b83..a3f0074 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -21,6 +21,22 @@ #include "speed_estimation.h" #include "fly_detection.h" #include "buzzer.h" +#include "math.h" +#include "string.h" + +#define STOOL_SPEED_REF 1.5f +#define STOOL_YAW_RATE_REF 3.5f +#define STOOL_ACC_REF 4.0f +#define STOOL_BRAKE_ACC_REF 8.0f +#define STOOL_PITCH_K 55.0f +#define STOOL_PITCH_W_K 7.0f +#define STOOL_VEL_K 10.0f +#define STOOL_STOP_DIST_K 8.0f +#define STOOL_VEL_LPF_RC 0.03f +#define STOOL_WHEEL_TORQUE_LIMIT 25.0f +#define STOOL_RC_DEADBAND 40 +#define CHASSIS_UNDERVOLTAGE_LIMIT 15.0f +#define REFEREE_VOLTAGE_VALID_MIN 5.0f // 计时变量 static uint32_t balance_dwt_cnt; static float del_t; @@ -29,8 +45,6 @@ static float del_t; static INS_t *Chassis_IMU_data; static RC_ctrl_t *rc_data; // 底盘单独调试用 static Chassis_Ctrl_Cmd_s chassis_cmd_recv; -static Chassis_Upload_Data_s chassis_feedback_data; // 底盘反馈数据 -static Chassis_Can_Comm chassis_can_recv; // 四个关节电机和两个驱动轮电机 static DMMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试 static LKMotorInstance *l_driven, *r_driven, *driven[2]; @@ -51,10 +65,112 @@ static Robot_Status_e chassis_status; static referee_info_t *referee_data; // 用于获取裁判系统的数据 static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI -static CANCommInstance *cmd_can_comm; // 底盘CAN通信实例 - static SuperCapInstance *cap; // 超级电容 -static uint16_t DataSend2Cap[4] = {0, 0, 0, 0}; + +static uint8_t stool_drive_key_pressed; +static float stool_vel; +static float stool_dist; +static float stool_stop_dist; +static float stool_target_wz; +static chassis_mode_e last_chassis_mode = CHASSIS_ZERO_FORCE; + +static void ClearMotionTargets(void) +{ + chassis.target_v = 0.0f; + chassis.target_dist = chassis.dist; + stool_target_wz = 0.0f; +} + +static void ClearControlOutput(void) +{ + l_side.F_leg = 0.0f; + l_side.T_hip = 0.0f; + l_side.T_back = 0.0f; + l_side.T_front = 0.0f; + l_side.T_wheel = 0.0f; + + r_side.F_leg = 0.0f; + r_side.T_hip = 0.0f; + r_side.T_back = 0.0f; + r_side.T_front = 0.0f; + r_side.T_wheel = 0.0f; +} + +static void ClearJointOutput(void) +{ + l_side.F_leg = 0.0f; + l_side.T_hip = 0.0f; + l_side.T_back = 0.0f; + l_side.T_front = 0.0f; + + r_side.F_leg = 0.0f; + r_side.T_hip = 0.0f; + r_side.T_back = 0.0f; + r_side.T_front = 0.0f; +} + +static void ApproachFloat(float *value, float target, float step) +{ + if (*value < target) + { + *value += step; + if (*value > target) + *value = target; + } + else if (*value > target) + { + *value -= step; + if (*value < target) + *value = target; + } +} + +static float RCChannelToRatio(int16_t ch) +{ + if (ch > -STOOL_RC_DEADBAND && ch < STOOL_RC_DEADBAND) + return 0.0f; + return float_constrain((float)ch / 660.0f, -1.0f, 1.0f); +} + +static void ResetStoolRuntime(void) +{ + stool_drive_key_pressed = 0; + stool_vel = 0.0f; + stool_dist = chassis.dist; + stool_stop_dist = stool_dist; + ClearMotionTargets(); + ClearControlOutput(); +} + +static void DisableAllJointMotor(void) +{ + for (uint8_t i = 0; i < JOINT_CNT; i++) + { + DMMotorSetRef(joint[i], 0.0f); + DMMotorOuterLoop(joint[i], OPEN_LOOP); + DMMotorStop(joint[i]); + } +} + +static void RefreshJointMotorDisable(void) +{ + DisableAllJointMotor(); +} + +static void EnableDrivenMotor(void) +{ + for (uint8_t i = 0; i < DRIVEN_CNT; i++) + LKMotorEnable(driven[i]); +} + +static void StopDrivenMotor(void) +{ + for (uint8_t i = 0; i < DRIVEN_CNT; i++) + { + LKMotorSetRef(driven[i], 0.0f); + LKMotorStop(driven[i]); + } +} void BalanceInit() { @@ -68,18 +184,6 @@ void BalanceInit() .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的 }}; cap = SuperCapInit(&cap_conf); // ww超级电容初始化 - CANComm_Init_Config_s comm_conf = { - .can_config = { - .can_handle = &hcan2, - .tx_id = 0x311, - .rx_id = 0x312, - }, - .daemon_count = 100, - .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s), - .send_data_len = sizeof(Chassis_Upload_Data_s), - }; - cmd_can_comm = CANCommInit(&comm_conf); - // 关节电机 Motor_Init_Config_s joint_conf = { // 写一个,剩下的修改方向和id即可 @@ -200,28 +304,14 @@ void BalanceInit() // 状态初始化 l_side.target_len = r_side.target_len = 0.12; chassis.vel_cov = 100; // 速度协方差初始化 - chassis_status = ROBOT_READY; - DWT_GetDeltaT(&balance_dwt_cnt); -} - -static void EnableAllMotor() /* 打开所有电机 */ -{ - for (uint8_t i = 0; i < JOINT_CNT; i++) // 打开关节电机 - DMMotorEnable(joint[i]); - for (uint8_t i = 0; i < DRIVEN_CNT; i++) // 打开驱动电机 - LKMotorEnable(driven[i]); -} - -// 检查关节电机是否离线 -static uint8_t JointMotorIsLost() -{ + chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; + chassis_status = ROBOT_STOP; + ResetStoolRuntime(); + DisableAllJointMotor(); for (uint8_t i = 0; i < JOINT_CNT; i++) - { - if (joint[i]->motor_daemon->temp_count == 0) - return 1; - } - - return 0; + DMMotorSetMode(DM_CMD_RESET_MODE, joint[i]); + StopDrivenMotor(); + DWT_GetDeltaT(&balance_dwt_cnt); } // 检查驱动轮电机是否离线 @@ -236,147 +326,100 @@ static uint8_t DrivenMotorIsLost() return 0; } -/* 切换底盘遥控器控制和云台双板控制 */ +/* Chassis-board local RC control. Right switch down is hard emergency stop. */ static void ControlSwitch() { - float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001; + memset(&chassis_cmd_recv, 0, sizeof(chassis_cmd_recv)); + chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; + chassis_cmd_recv.direction = CHASSIS_ALIGN; + chassis_cmd_recv.friction_mode = FRICTION_OFF; + chassis_cmd_recv.loader_mode = LOAD_STOP; + chassis_cmd_recv.vision_mode = UNLOCK; + chassis_cmd_recv.ui_mode = UI_KEEP; - // 根据裁判系统底盘输出电压设定底盘状态 - if (chassis_vol < 15.0f || JointMotorIsLost() || DrivenMotorIsLost()) + if (!RemoteControlIsOnline()) { - chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停 return; } - // // 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令 - // if (switch_is_down(rc_data->rc.switch_right) && RemoteControlIsOnline()) - // { - // if (switch_is_up(rc_data->rc.switch_left)) - // { - // chassis_cmd_recv.chassis_mode = CHASSIS_RESET; - // chassis_cmd_recv.vx = 0.5 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s - // chassis_cmd_recv.rotate_w = 0.5 * (float)rc_data[TEMP].rc.rocker_r_; - // } - // else - // { - // chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后 - // chassis_cmd_recv.vx = 0.003 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s - // chassis_cmd_recv.delta_leglen = -0.0000005f * (float)rc_data[TEMP].rc.dial; - // chassis_cmd_recv.offset_angle -= 0.000005 * (float)rc_data[TEMP].rc.rocker_r_; - // } - // } - // else - // { - // chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm); - // } - chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm); - // if (abs(l_side.theta) > (30.0f * DEGREE_2_RAD) || abs(r_side.theta) > (30.0f * DEGREE_2_RAD)) - // { - // chassis_cmd_recv.chassis_mode = CHASSIS_RESET; - // } + if (switch_is_down(rc_data[TEMP].rc.switch_right)) + return; + + float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001f; + if (chassis_vol > REFEREE_VOLTAGE_VALID_MIN && + chassis_vol < CHASSIS_UNDERVOLTAGE_LIMIT) + return; + + if (DrivenMotorIsLost()) + return; + + if (!switch_is_up(rc_data[TEMP].rc.switch_right)) + return; + + chassis_cmd_recv.chassis_mode = CHASSIS_STOOL_MODE; + chassis_cmd_recv.vx = RCChannelToRatio(rc_data[TEMP].rc.rocker_r1) * STOOL_SPEED_REF; + chassis_cmd_recv.offset_angle = -RCChannelToRatio(rc_data[TEMP].rc.rocker_r_) * STOOL_YAW_RATE_REF; } -/* 腿缩回复位,只允许驱动轮电机移动 */ +/* Joint motors must remain disabled on this demo chassis. */ static void ResetChassis() { - EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身 - - // 目标速度置0 - chassis.target_v = 0; - // 复位时清空距离和腿长积累量,保证顺利站起 - chassis.dist = chassis.target_dist = 0; - l_side.target_len = r_side.target_len = 0.12; - // 角度输入为当前角度 - // chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw; - - // 撞墙时前后移动保证能重新站立,执行速度输入 - LKMotorSetRef(l_driven, chassis_cmd_recv.vx + (float)chassis_cmd_recv.rotate_w); - LKMotorSetRef(r_driven, -chassis_cmd_recv.vx + (float)chassis_cmd_recv.rotate_w); - - // 若关节完成复位,进入ready态 - 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_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++) - DMMotorOuterLoop(joint[i], OPEN_LOOP); // 改回直接开环扭矩输入,让电调对扭矩闭环 - - return; // 退出函数不再执行关节指令 - } - else - chassis_status = ROBOT_STOP; - - // 还在复位中,关节改为位置环,执行复位 - for (uint8_t i = 0; i < JOINT_CNT; i++) - { - DMMotorOuterLoop(joint[i], ANGLE_LOOP); - DMMotorSetRef(joint[i], 0); - } + ClearMotionTargets(); + ClearControlOutput(); + l_side.target_len = r_side.target_len = 0.12f; + chassis.dist = chassis.target_dist = 0.0f; + chassis_status = ROBOT_STOP; + RefreshJointMotorDisable(); + StopDrivenMotor(); } // 工作状态设定 static void WokingStateSet() { - if (chassis_cmd_recv.chassis_mode == CHASSIS_RESET) // 复位模式 + RefreshJointMotorDisable(); + + if (last_chassis_mode != chassis_cmd_recv.chassis_mode) + { + if (chassis_cmd_recv.chassis_mode == CHASSIS_STOOL_MODE) + ResetStoolRuntime(); + else + ClearControlOutput(); + last_chassis_mode = chassis_cmd_recv.chassis_mode; + } + + if (chassis_cmd_recv.chassis_mode == CHASSIS_RESET || + chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) { ResetChassis(); return; } - else if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) // 未收到遥控器和云台指令底盘进入急停 - { - // 目标速度置0 - chassis.target_v = 0; - // 清空腿长和距离 - l_side.target_len = r_side.target_len = 0.12; - chassis.dist = chassis.target_dist = 0; - // 角度输入为当前角度 - // chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw; - for (uint8_t i = 0; i < JOINT_CNT; i++) - DMMotorStop(joint[i]); - for (uint8_t i = 0; i < DRIVEN_CNT; i++) - LKMotorStop(driven[i]); - return; // 关闭所有电机,发送的指令为零 + if (chassis_cmd_recv.chassis_mode != CHASSIS_STOOL_MODE) + { + ResetChassis(); + return; } - // 运动模式 - EnableAllMotor(); - // 保证关节电机为开环扭矩控制 - for (uint8_t i = 0; i < JOINT_CNT; i++) - DMMotorOuterLoop(joint[i], OPEN_LOOP); + EnableDrivenMotor(); + chassis_status = ROBOT_READY; + l_side.target_len = r_side.target_len = 0.12f; + stool_target_wz = chassis_cmd_recv.offset_angle; - // 设置目标速度/腿长/距离 - l_side.target_len += 0.00001f*(float)chassis_cmd_recv.delta_leglen; - r_side.target_len += 0.00001f*(float)chassis_cmd_recv.delta_leglen; - // 腿长限幅 - VAL_LIMIT(l_side.target_len, 0.12, 0.30); - VAL_LIMIT(r_side.target_len, 0.12, 0.30); - - // 加速度限幅,防止键盘控制摔倒 - chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t; - // VAL_LIMIT(chassis.target_v, 0.002, 2.0); - // 角度输入 - if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG) + if (fabsf(chassis_cmd_recv.vx) > 0.001f) { - chassis.target_yaw = chassis_cmd_recv.offset_angle; + ApproachFloat(&chassis.target_v, chassis_cmd_recv.vx, STOOL_ACC_REF * del_t); + stool_stop_dist = stool_dist; + stool_drive_key_pressed = 1; } - chassis.target_yaw = chassis.yaw + chassis_cmd_recv.offset_angle * DEGREE_2_RAD; - - // TODO 转向速度限幅 - - // TODO 最大dist误差限幅 - - // TODO 最大速度误差限幅 + else + { + if (stool_drive_key_pressed) + stool_stop_dist = stool_dist; + ApproachFloat(&chassis.target_v, 0.0f, STOOL_BRAKE_ACC_REF * del_t); + stool_drive_key_pressed = 0; + } + VAL_LIMIT(chassis.target_v, -STOOL_SPEED_REF, STOOL_SPEED_REF); + chassis.target_dist = chassis.dist; } /** @@ -410,8 +453,40 @@ static void ParamAssemble() r_side.w_ecd = -r_driven->measure.speed_rads; } +static void StoolModeBalanceControl(void) +{ + float raw_vel = WHEEL_RADIUS * (l_side.w_ecd + r_side.w_ecd) * 0.5f; + float vel_lpf_alpha = del_t / (STOOL_VEL_LPF_RC + del_t); + VAL_LIMIT(vel_lpf_alpha, 0.0f, 1.0f); + + stool_vel += vel_lpf_alpha * (raw_vel - stool_vel); + stool_dist += stool_vel * del_t; + + if (fabsf(chassis.target_v) > 0.001f) + stool_stop_dist = stool_dist; + + float stop_dist_error = stool_dist - stool_stop_dist; + float wheel_torque = -STOOL_PITCH_K * chassis.pitch - + STOOL_PITCH_W_K * chassis.pitch_w + + STOOL_VEL_K * (stool_vel - chassis.target_v) + + STOOL_STOP_DIST_K * stop_dist_error; + + VAL_LIMIT(wheel_torque, -STOOL_WHEEL_TORQUE_LIMIT, STOOL_WHEEL_TORQUE_LIMIT); + + l_side.T_wheel = wheel_torque; + r_side.T_wheel = wheel_torque; +} + static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */ { + if (chassis_cmd_recv.chassis_mode == CHASSIS_STOOL_MODE) + { + PIDCalculate(&steer_v_pid, chassis.wz, stool_target_wz); + l_side.T_wheel -= steer_v_pid.Output; + r_side.T_wheel += steer_v_pid.Output; + return; + } + if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG || chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) // 底盘跟随 { @@ -452,10 +527,7 @@ static void LegControl() /* 腿长控制和Roll补偿 */ static void WattLimitSet() /* 设定运动模态的输出 */ { - DMMotorSetRef(lf, 1.0f * -l_side.T_front); // 根据扭矩常数计算得到的系数 todo 需修改 - DMMotorSetRef(lb, 1.0f * -l_side.T_back); - DMMotorSetRef(rf, 1.0f * r_side.T_front); - DMMotorSetRef(rb, 1.0f * r_side.T_back); + RefreshJointMotorDisable(); LKMotorSetRef(l_driven, 195.3125 * l_side.T_wheel); LKMotorSetRef(r_driven, 195.3125 * -r_side.T_wheel); @@ -464,16 +536,6 @@ static void WattLimitSet() /* 设定运动模态的输出 */ // 裁判系统,双板通信,电容功率控制等 static void CommNPower() { - //static uint8_t supercap_send_cnt = 0; - // CANCommSend(cmd_can_comm, (void *)&chassis_feedback_data); - // supercap_send_cnt++; - // if (supercap_send_cnt % 5 == 0) - // { - // DataSend2Cap[0] = referee_data->PowerHeatData.buffer_energy; // 200hz发送 - // DataSend2Cap[1] = referee_data->GameRobotState.chassis_power_limit; - // SuperCapSend(cap, (uint8_t *)&DataSend2Cap); - // supercap_send_cnt = 0; - // } /* 更新ui数据 */ ui_data.direction = chassis_cmd_recv.direction; ui_data.friction_mode = chassis_cmd_recv.friction_mode; @@ -497,6 +559,21 @@ void BalanceTask() CommNPower(); // 参数组装 ParamAssemble(); + + if (chassis_status == ROBOT_STOP || + chassis_cmd_recv.chassis_mode == CHASSIS_RESET || + chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) + return; + + if (chassis_cmd_recv.chassis_mode == CHASSIS_STOOL_MODE) + { + StoolModeBalanceControl(); + SynthesizeMotion(); + ClearJointOutput(); + WattLimitSet(); + return; + } + // 将五连杆映射成单杆 Link2Leg(&l_side, &chassis); Link2Leg(&r_side, &chassis); @@ -516,12 +593,6 @@ void BalanceTask() NormalForceSolve(&l_side, Chassis_IMU_data); NormalForceSolve(&r_side, Chassis_IMU_data); - // stop表示复位尚未完成,reset表明还未切换到其他模式,故都不执行运动模态的代码 - if (chassis_status == ROBOT_STOP || - chassis_cmd_recv.chassis_mode == CHASSIS_RESET || - chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) - return; // 复位模态或急停,直接退出 - // 运动模态,电机输出映射和限幅 WattLimitSet(); -} \ No newline at end of file +} diff --git a/application/chassis/lqr_calc.h b/application/chassis/lqr_calc.h index ce05c24..93efa28 100644 --- a/application/chassis/lqr_calc.h +++ b/application/chassis/lqr_calc.h @@ -9,18 +9,20 @@ */ static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis) { - static float k[12][3] = {91.443258,-113.149301,-8.780431, - 2.186397,-6.908826,-0.171192, - 47.943203,-39.423913,-13.297140, - 30.671413,-28.308799,-9.363969, - 231.778706,-224.742110,68.974495, - 18.910995,-20.079072,7.490403, - 50.114072,-58.670210,23.856863, - 3.273746,-3.321150,1.267603, - 140.969395,-137.481822,42.507159, - 93.859327,-90.754295,27.811839, - -283.581279,232.017305,87.865897, - -28.193619,23.802666,3.963794,}; + static float k[12][3] = { + {91.443258f, -113.149301f, -8.780431f}, + {2.186397f, -6.908826f, -0.171192f}, + {47.943203f, -39.423913f, -13.297140f}, + {30.671413f, -28.308799f, -9.363969f}, + {231.778706f, -224.742110f, 68.974495f}, + {18.910995f, -20.079072f, 7.490403f}, + {50.114072f, -58.670210f, 23.856863f}, + {3.273746f, -3.321150f, 1.267603f}, + {140.969395f, -137.481822f, 42.507159f}, + {93.859327f, -90.754295f, 27.811839f}, + {-283.581279f, 232.017305f, 87.865897f}, + {-28.193619f, 23.802666f, 3.963794f}, + }; float T[2] = {0}; // 0 T_wheel 1 T_hip float l = p->leg_len; float lsqr = l * l; @@ -48,4 +50,4 @@ static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis) p->T_wheel = T[0]; p->T_hip = T[1]; -} \ No newline at end of file +} diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 4b7e3a2..16d1351 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -16,6 +16,8 @@ #include "bsp_log.h" #include "string.h" +#if defined(ONE_BOARD) || defined(GIMBAL_BOARD) + static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者 static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者 static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关 @@ -415,3 +417,5 @@ void RobotCMDTask() PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send); PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send); } + +#endif diff --git a/application/robot_def.h b/application/robot_def.h index 171c62a..0fbcc81 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -17,8 +17,8 @@ #include "stdint.h" /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ -#define ONE_BOARD // 单板控制整车 -// #define CHASSIS_BOARD //底盘板 +// #define ONE_BOARD // 单板控制整车 +#define CHASSIS_BOARD //底盘板 // #define GIMBAL_BOARD //云台板 #define VISION_USE_VCP // 使用虚拟串口发送视觉数据 @@ -92,6 +92,7 @@ typedef enum CHASSIS_RESET, // 底盘重置,双腿缩回 CHASSIS_FREE_DEBUG, // 底盘单独调试模式 CHASSIS_ROTATE_REVERSE, + CHASSIS_STOOL_MODE, // 小板凳模式 } chassis_mode_e; @@ -254,4 +255,4 @@ typedef struct #pragma pack() // 开启字节对齐,结束前面的#pragma pack(1) -#endif // !ROBOT_DEF_H \ No newline at end of file +#endif // !ROBOT_DEF_H diff --git a/modules/algorithm/QuaternionEKF.c b/modules/algorithm/QuaternionEKF.c index 27c06e5..54f593e 100644 --- a/modules/algorithm/QuaternionEKF.c +++ b/modules/algorithm/QuaternionEKF.c @@ -199,8 +199,8 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, // 利用四元数反解欧拉角 QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f; - QEKF_INS.Pitch = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f; - QEKF_INS.Roll = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f; + QEKF_INS.Pitch = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f; + QEKF_INS.Roll = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f; // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺) if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast > 180.0f) diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c index 0873988..11d104f 100644 --- a/modules/motor/DMmotor/dmmotor.c +++ b/modules/motor/DMmotor/dmmotor.c @@ -65,8 +65,7 @@ static void DMMotorDecode(CANInstance *motor_can) static void DMMotorLostCallback(void *motor_ptr) { DMMotorInstance *motor = (DMMotorInstance *)motor_ptr; - DMMotorEnable(motor); - DMMotorSetMode(DM_CMD_MOTOR_MODE, motor); + DMMotorStop(motor); } void DMMotorCaliEncoder(DMMotorInstance *motor) @@ -131,17 +130,16 @@ void DMMotorTask(void const *argument) Motor_Control_Setting_s *setting; DMMotor_Send_s motor_send_mailbox; portTickType currentTime; - const portTickType xFrequency = pdMS_TO_TICKS(4); while (1) { currentTime = xTaskGetTickCount(); set = motor->pid_ref; + measure = &motor->measure; + setting = &motor->motor_settings; if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) set *= -1; - measure = &motor->measure; - setting = &motor->motor_settings; if ((setting->close_loop_type & ANGLE_LOOP) && (setting->outer_loop_type & ANGLE_LOOP)) { if (setting->angle_feedback_source == OTHER_FEED) diff --git a/modules/referee/referee_task.c b/modules/referee/referee_task.c index 5ee621a..8d786e4 100644 --- a/modules/referee/referee_task.c +++ b/modules/referee/referee_task.c @@ -38,7 +38,7 @@ static void DeterminRobotID() static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data); static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data); // 模式切换检测 -static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data); // 测试用函数，实现模式自动变化 +static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) __attribute__((unused)); // 测试用函数，实现模式自动变化 referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data) { @@ -225,6 +225,15 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i case CHASSIS_FOLLOW_GIMBAL_YAW: UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow "); break; + case CHASSIS_STOOL_MODE: + UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "stool "); + break; + case CHASSIS_FREE_DEBUG: + UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "debug "); + break; + case CHASSIS_ROTATE_REVERSE: + UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "revrot "); + break; } UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]); _Interactive_data->Referee_Interactive_Flag.chassis_flag = 0; @@ -263,6 +272,9 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i { switch (_Interactive_data->loader_mode) { + case LOAD_REVERSE: + UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Purplish_red, 15, 2, 270, 600, "rev "); + break; case LOAD_1_BULLET: UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Cyan, 15, 2, 270, 600, "1 bull"); break;