From da591dcfb49fe8a957029003f66a27b23070bcf1 Mon Sep 17 00:00:00 2001 From: chenfu <2412777093@qq.com> Date: Sun, 26 May 2024 11:40:33 +0800 Subject: [PATCH] =?UTF-8?q?=E8=A3=85=E8=B6=85=E7=94=B5=E5=90=8E=E4=B8=BA?= =?UTF-8?q?=E9=A2=84=E6=A3=80=E5=BD=95=E5=87=86=E5=A4=87=E7=9A=84=E7=89=88?= =?UTF-8?q?=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/launch.json | 2 +- .vscode/tasks.json | 2 +- application/chassis/balance.c | 47 +++++++++++++++----- application/chassis/balance.h | 2 +- application/chassis/speed_estimation.h | 2 +- application/cmd/robot_cmd.c | 61 ++++++++++++++++---------- application/robot_def.h | 5 ++- modules/super_cap/super_cap.c | 2 +- 8 files changed, 83 insertions(+), 40 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 49cd04b..036e9d4 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -43,7 +43,7 @@ "interface": "swd", "svdFile": "STM32F407.svd", "rtos": "FreeRTOS", - "preLaunchTask": "build task",//先运行Build任务,取消注释即可使用 + //"preLaunchTask": "build task",//先运行Build任务,取消注释即可使用 "liveWatch": { "enabled": true, "samplesPerSecond": 4 diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 3670422..e483241 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -24,7 +24,7 @@ { "label": "download jlink", // 要使用此任务,需添加jlink的环境变量 "type": "shell", - "command":"mingw32-make -j24 ; mingw32-make download_jlink", // "mingw32-make -j24 ; mingw32-make download_jlink" + "command":" download_jlink", // "mingw32-make -j24 ; mingw32-make download_jlink" "group": { "kind": "build", "isDefault": false, diff --git a/application/chassis/balance.c b/application/chassis/balance.c index b46c07a..01b95e4 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -53,12 +53,21 @@ static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据 static CANCommInstance *cmd_can_comm; // 底盘CAN通信实例 +static SuperCapInstance *cap; // 超级电容 +static uint16_t DataSend2Cap[4] = {0, 0, 0, 0}; + void BalanceInit() { rc_data = RemoteControlInit(&huart3); Chassis_IMU_data = INS_Init(); referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI - + SuperCap_Init_Config_s cap_conf = { + .can_config = { + .can_handle = &hcan2, + .tx_id = 0x302, // 超级电容默认接收id + .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的 + }}; + cap = SuperCapInit(&cap_conf); // ww超级电容初始化 CANComm_Init_Config_s comm_conf = { .can_config = { .can_handle = &hcan2, @@ -120,6 +129,7 @@ void BalanceInit() .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = LK9025, + }; driven_conf.can_init_config.tx_id = 1; driven[LD] = l_driven = LKMotorInit(&driven_conf); @@ -229,8 +239,9 @@ static uint8_t DrivenMotorIsLost() /* 切换底盘遥控器控制和云台双板控制 */ static void ControlSwitch() { - // 根据裁判系统底盘输出电压设定底盘状态 float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001; + + // 根据裁判系统底盘输出电压设定底盘状态 if (chassis_vol < 15.0f || JointMotorIsLost() || DrivenMotorIsLost()) { chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停 @@ -259,6 +270,10 @@ static void ControlSwitch() // 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; + // } } /* 腿缩回复位,只允许驱动轮电机移动 */ @@ -344,12 +359,12 @@ static void WokingStateSet() l_side.target_len += chassis_cmd_recv.delta_leglen; r_side.target_len += chassis_cmd_recv.delta_leglen; // 腿长限幅 - VAL_LIMIT(l_side.target_len, 0.12, 0.25); - VAL_LIMIT(r_side.target_len, 0.12, 0.25); + 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(r_side.target_len, 0.002, 2.0); // 角度输入 if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG) { @@ -397,15 +412,19 @@ static void ParamAssemble() static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */ { - if(chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG || + if (chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG || chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) // 底盘跟随 { float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw); PIDCalculate(&steer_v_pid, chassis.wz, p_ref); } - else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) // 小陀螺 + else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) // 小陀螺 { - PIDCalculate(&steer_v_pid, chassis.wz, 6); + PIDCalculate(&steer_v_pid, chassis.wz, chassis_cmd_recv.rotate_w); + } + else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE_REVERSE) + { + PIDCalculate(&steer_v_pid, chassis.wz, -chassis_cmd_recv.rotate_w); } l_side.T_wheel -= steer_v_pid.Output; r_side.T_wheel += steer_v_pid.Output; @@ -444,8 +463,16 @@ 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; @@ -459,7 +486,7 @@ static void CommNPower() void BalanceTask() { del_t = DWT_GetDeltaT(&balance_dwt_cnt); - + BuzzerOn(); // 切换遥控器控制or云台板控制 ControlSwitch(); diff --git a/application/chassis/balance.h b/application/chassis/balance.h index c965678..698c675 100644 --- a/application/chassis/balance.h +++ b/application/chassis/balance.h @@ -10,7 +10,7 @@ #define LIMIT_LINK_RAD 0.220039368 // 初始限位角度,见ParamAssemble #define BALANCE_GRAVITY_BIAS 0 #define ROLL_GRAVITY_BIAS 0 -#define MAX_ACC_REF 0.9f +#define MAX_ACC_REF 1.2f // 驱动轮质量 #define WHEEL_MASS 0.58f diff --git a/application/chassis/speed_estimation.h b/application/chassis/speed_estimation.h index 44f679c..a950cd8 100644 --- a/application/chassis/speed_estimation.h +++ b/application/chassis/speed_estimation.h @@ -61,7 +61,7 @@ void SpeedEstimation(LinkNPodParam *lp, LinkNPodParam *rp, ChassisParam *cp, INS VAL_LIMIT(cp->vel_cov, 0.01, 100); // 协方差限幅 // 速度和位置分离，有速度输入时不进行位置闭环 - if(abs(cp->target_v) < 0.001) + if(abs(cp->target_v) < 0.005) { cp->target_dist = 0; cp->dist += cp->vel * delta_t; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 71b6005..4294bff 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -83,11 +83,11 @@ static void CalcOffsetAngle() // @todo:相差一整圈时会出问题，待修复 // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数 uint16_t yaw_chassis_align_ecd; - if(chassis_direction == CHASSIS_ALIGN) + if (chassis_direction == CHASSIS_ALIGN) { yaw_chassis_align_ecd = 2716; } - else if(chassis_direction == CHASSIS_SIDLE) + else if (chassis_direction == CHASSIS_SIDLE) { yaw_chassis_align_ecd = 765; } @@ -96,7 +96,7 @@ static void CalcOffsetAngle() yaw_align_angle = yaw_chassis_align_ecd * ECD_ANGLE_COEF_DJI; // 从底盘获取的yaw电机对齐角度 angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度 - if (yaw_chassis_align_ecd > 4096) // 如果大于180度 + if (yaw_chassis_align_ecd > 4096) // 如果大于180度 { if (angle > yaw_align_angle) chassis_cmd_send.offset_angle = angle - yaw_align_angle; @@ -106,7 +106,7 @@ static void CalcOffsetAngle() chassis_cmd_send.offset_angle = angle - yaw_align_angle + 360.0f; } else - { // 小于180度 + { // 小于180度 if (angle > yaw_align_angle && angle <= 180.0f + yaw_align_angle) chassis_cmd_send.offset_angle = angle - yaw_align_angle; else if (angle > 180.0f + yaw_align_angle) @@ -122,19 +122,21 @@ static void CalcOffsetAngle() */ static void RemoteControlSet() { + memcpy(&rc_data->key_count, 0, sizeof(rc_data->key_count)); gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; // 云台参数,确定云台控制数据 gimbal_cmd_send.yaw -= 0.001f * (float)rc_data[TEMP].rc.rocker_l_; gimbal_cmd_send.pitch -= 0.0006f * (float)rc_data[TEMP].rc.rocker_l1; + chassis_cmd_send.rotate_w = 0.0f; // 摇杆控制的软件限位 gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE); if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[下],视觉模式 { - gimbal_cmd_send.yaw = ( vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw ); - gimbal_cmd_send.pitch = ( vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch ); + gimbal_cmd_send.yaw = (vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw); + gimbal_cmd_send.pitch = (vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch); } // 底盘参数 @@ -150,7 +152,17 @@ static void RemoteControlSet() // 右侧开关状态为[下],底盘旋转 if (switch_is_down(rc_data[TEMP].rc.switch_right)) { - chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; + chassis_cmd_send.rotate_w = 4.0f; + chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; + + if (abs(rc_data[TEMP].rc.rocker_r_) > 300) + { + chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; + } + if (abs(rc_data[TEMP].rc.rocker_r1) > 300) + { + chassis_cmd_send.chassis_mode = CHASSIS_ROTATE_REVERSE; + } chassis_cmd_send.vx = 0; } else @@ -182,7 +194,7 @@ static void RemoteControlSet() // 发射参数 shoot_cmd_send.bullet_speed = 30; - shoot_cmd_send.shoot_rate = 15; + shoot_cmd_send.shoot_rate = 10; } /** @@ -193,15 +205,14 @@ static void MouseKeySet() { gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - gimbal_cmd_send.yaw -= (float)(rc_data[TEMP].mouse.x + rc_data[LAST].mouse.x) / 660.0f * 4.0f ; // 系数待测 + gimbal_cmd_send.yaw -= (float)(rc_data[TEMP].mouse.x + rc_data[LAST].mouse.x) / 660.0f * 4.0f; // 系数待测 gimbal_cmd_send.pitch += (float)(rc_data[TEMP].mouse.y + rc_data[TEMP].mouse.y) / 660.0f * 4.0f; gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE); - if(chassis_cmd_send.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) + if (chassis_cmd_send.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) { - chassis_cmd_send.vx = BALANCE_MAX_SPEED * (float)(rc_data[TEMP].key[KEY_PRESS].w - rc_data[TEMP].key[KEY_PRESS].s - - rc_data[TEMP].key[KEY_PRESS].a + rc_data[TEMP].key[KEY_PRESS].d); + chassis_cmd_send.vx = BALANCE_MAX_SPEED * (float)(rc_data[TEMP].key[KEY_PRESS].w - rc_data[TEMP].key[KEY_PRESS].s - rc_data[TEMP].key[KEY_PRESS].a + rc_data[TEMP].key[KEY_PRESS].d); } else if (chassis_cmd_send.chassis_mode == CHASSIS_ROTATE) { @@ -218,17 +229,14 @@ static void MouseKeySet() { chassis_direction = CHASSIS_SIDLE; } - - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q 小陀螺 + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_B] % 2) { case 0: + break; + case 1: chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; break; - default: - chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; - break; } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F 摩擦轮 { case 0: @@ -239,7 +247,7 @@ static void MouseKeySet() break; } - if(shoot_cmd_send.friction_mode == FRICTION_ON) + if (shoot_cmd_send.friction_mode == FRICTION_ON) { if (rc_data[TEMP].mouse.press_l) shoot_cmd_send.load_mode = LOAD_BURSTFIRE; @@ -249,7 +257,7 @@ static void MouseKeySet() else shoot_cmd_send.load_mode = LOAD_STOP; - switch (rc_data[TEMP].key[KEY_PRESS].z) // Z键刷新UI + switch (rc_data[TEMP].key[KEY_PRESS].z) // Z键刷新UI { case 0: chassis_cmd_send.ui_mode = UI_KEEP; @@ -264,6 +272,15 @@ static void MouseKeySet() gimbal_cmd_send.yaw = (vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw); gimbal_cmd_send.pitch = (vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch); } + if (rc_data[TEMP].key[KEY_PRESS].q) + { + chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; + chassis_cmd_send.rotate_w = 6.0f; + } + if (rc_data[TEMP].key[KEY_PRESS].r) + { + chassis_cmd_send.chassis_mode = CHASSIS_RESET; + } } /** @@ -302,9 +319,7 @@ static void EmergencyHandler() switch (rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_C] % 2) // ctrl+c 进入急停 { case 0: - robot_state = ROBOT_READY; - shoot_cmd_send.shoot_mode = SHOOT_ON; - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + break; default: diff --git a/application/robot_def.h b/application/robot_def.h index ad8f7e5..3250ef8 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 // 使用串口发送视觉数据 @@ -91,6 +91,7 @@ typedef enum CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式，底盘叠加角度环控制 CHASSIS_RESET, // 底盘重置,双腿缩回 CHASSIS_FREE_DEBUG, // 底盘单独调试模式 + CHASSIS_ROTATE_REVERSE, } chassis_mode_e; diff --git a/modules/super_cap/super_cap.c b/modules/super_cap/super_cap.c index eb86749..864b4df 100644 --- a/modules/super_cap/super_cap.c +++ b/modules/super_cap/super_cap.c @@ -35,7 +35,7 @@ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config) void SuperCapSend(SuperCapInstance *instance, uint8_t *data) { memcpy(instance->can_ins->tx_buff, data, 8); - CANTransmit(instance->can_ins,1); + CANTransmit(instance->can_ins,0.5); } SuperCap_Msg_s SuperCapGet(SuperCapInstance *instance)