From 2ecea006f0a40655eeeb834ad66f4d7de4f2e75f Mon Sep 17 00:00:00 2001 From: kai <1797003616@qq.com> Date: Wed, 17 Jan 2024 18:57:26 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E7=94=B5=E6=9C=BA=E5=8F=8D?= =?UTF-8?q?=E8=BD=ACbug=EF=BC=8C=E6=B7=BB=E5=8A=A0=E9=83=A8=E5=88=86?= =?UTF-8?q?=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/launch.json | 4 ++-- .vscode/settings.json | 5 ++++- .vscode/tasks.json | 6 +++--- application/chassis/balance.c | 38 +++++++++++++++++++++++----------- modules/motor/HTmotor/HT04.c | 7 ++++--- modules/motor/LKmotor/LK9025.c | 7 ++++--- modules/motor/LKmotor/LK9025.h | 2 +- 7 files changed, 44 insertions(+), 25 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 704f635..49cd04b 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -47,8 +47,8 @@ "liveWatch": { "enabled": true, "samplesPerSecond": 4 - } - //"preLaunchTask": "log", // 调试时同时开启RTT viewer窗口,若daplink使用jlinkGDBserver启动,需要先开始调试再打开log + }, + // "preLaunchTask": "log", // 调试时同时开启RTT viewer窗口,若daplink使用jlinkGDBserver启动,需要先开始调试再打开log // 若想要在调试前编译并且打开log,可只使用log的prelaunch task并为log任务添加depends on依赖 }, { diff --git a/.vscode/settings.json b/.vscode/settings.json index 7590390..cc63151 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,5 +1,8 @@ { "files.associations": { - "stm32f4xx_hal_def.h": "c" + "stm32f4xx_hal_def.h": "c", + "general_def.h": "c", + "stdlib.h": "c", + "bsp_can.h": "c" } } \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 3c32888..46afe8e 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -36,9 +36,9 @@ "command":"JlinkRTTClient", "args": [], "problemMatcher": [], - // "dependsOn":[ - // "build task", // 可以添加多个. - // ] + "dependsOn":[ + "build task", // 可以添加多个. + ] // 若使用daplink,则将log任务设置为依赖于jlink launch任务,保证jlink launch任务先于log任务执行 } ] diff --git a/application/chassis/balance.c b/application/chassis/balance.c index 2e98c8e..eb902c0 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -22,49 +22,54 @@ static float del_t; /* 底盘拥有的模块实例 */ static attitude_t *imu_data; -static RC_ctrl_t *rc_data; // 调试用 +static RC_ctrl_t *rc_data; // 底盘单独调试用 static Referee_Interactive_info_t my_ui; static referee_info_t *referee_data; -static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // syh +static Chassis_Ctrl_Cmd_s chassis_cmd_recv; static Chassis_Upload_Data_s chassis_feed; static CANCommInstance *ci; -static SuperCapInstance *cap; // syh +static SuperCapInstance *cap; + // 四个关节电机和两个驱动轮电机 static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试 static LKMotorInstance *l_driven, *r_driven, *driven[2]; + // 两个腿的参数,0为左腿,1为右腿 -static LinkNPodParam l_side, r_side; // syh phi5 +static LinkNPodParam l_side, r_side; static ChassisParam chassis; + // 综合运动补偿的PID控制器 static PIDInstance steer_p_pid, steer_v_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量 static PIDInstance anti_crash_pid, phi5_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上 static PIDInstance leglen_pid_l, leglen_pid_r; // 用PD模拟弹簧,不要积分(弹簧是无积分二阶系统),增益不可过大否则抗外界冲击响应时太"硬" -static PIDInstance legdot_pid_l, legdot_pid_r; static PIDInstance roll_compensate_pid, rolldot_pid; // roll轴补偿,用于保持机体水平 static Robot_Status_e chassis_status; void BalanceInit() { - referee_data = UITaskInit(&huart6, &my_ui); rc_data = RemoteControlInit(&huart3); + imu_data = INS_Init(); + + // 双板通信 CANComm_Init_Config_s commconf = { .can_config = { - .can_handle = &hcan1, + .can_handle = &hcan2, .tx_id = 0x40, .rx_id = 0x41}, .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s), .send_data_len = sizeof(Chassis_Upload_Data_s)}; ci = CANCommInit(&commconf); - imu_data = INS_Init(); + // 超级电容 SuperCap_Init_Config_s cap_conf = { .can_config = { - .can_handle = &hcan1, + .can_handle = &hcan2, .tx_id = 0x302, // todo 电容id .rx_id = 0x301}}; cap = SuperCapInit(&cap_conf); + // 关节电机 Motor_Init_Config_s joint_conf = { // 写一个,剩下的修改方向和id即可 .can_init_config = { @@ -101,6 +106,7 @@ void BalanceInit() joint_conf.can_init_config.rx_id = 11; joint[RB] = rb = HTMotorInit(&joint_conf); + // 驱动轮电机 Motor_Init_Config_s driven_conf = { // 写一个,剩下的修改方向和id即可 .can_init_config.can_handle = &hcan2, @@ -118,6 +124,7 @@ void BalanceInit() driven_conf.can_init_config.tx_id = 2; driven[RD] = r_driven = LKMotorInit(&driven_conf); + // 转向PID PID_Init_Config_s steer_p_pid_conf = { .Kp = 2, .Kd = 1, @@ -140,6 +147,7 @@ void BalanceInit() }; PIDInit(&steer_v_pid, &steer_v_pid_conf); + // 抗劈叉 PID_Init_Config_s anti_crash_pid_conf = { .Kp = 8, .Kd = 2.5, @@ -154,6 +162,7 @@ void BalanceInit() }; PIDInit(&anti_crash_pid, &anti_crash_pid_conf); + // 腿长控制 PID_Init_Config_s leg_length_pid_conf = { .Kp = 450, .Kd = 150, @@ -168,6 +177,7 @@ void BalanceInit() PIDInit(&leglen_pid_l, &leg_length_pid_conf); PIDInit(&leglen_pid_r, &leg_length_pid_conf); + // 横滚角补偿 PID_Init_Config_s roll_compensate_pid_conf = { .Kp = 0.0008f, .Kd = 0.00065f, @@ -179,7 +189,7 @@ void BalanceInit() }; PIDInit(&roll_compensate_pid, &roll_compensate_pid_conf); - l_side.target_len = r_side.target_len = 0.23; + l_side.target_len = r_side.target_len = 0.23; // 初始腿长 chassis.vel_cov = 1000; // 初始化速度协方差 chassis_status = ROBOT_READY; DWT_GetDeltaT(&balance_dwt_cnt); @@ -195,7 +205,8 @@ static void EnableAllMotor() /* 打开所有电机 */ /* 切换底盘遥控器控制和云台双板控制 */ static void ControlSwitch() -{ // 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令 +{ + // 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令 if (switch_is_down(rc_data->rc.switch_right) && RemoteControlIsOnline()) { if (rc_data->rc.rocker_l1 < -600) @@ -220,6 +231,9 @@ static void ControlSwitch() void BalanceTask() { - + del_t = DWT_GetDeltaT(&balance_dwt_cnt); + + // 切换遥控器控制or云台板控制 + ControlSwitch(); } \ No newline at end of file diff --git a/modules/motor/HTmotor/HT04.c b/modules/motor/HTmotor/HT04.c index fdfad15..eec1167 100644 --- a/modules/motor/HTmotor/HT04.c +++ b/modules/motor/HTmotor/HT04.c @@ -121,7 +121,7 @@ HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config) Daemon_Init_Config_s conf = { .callback = HTMotorLostCallback, .owner_id = motor, - .reload_count = 5, // 20ms + .reload_count = 5, // 50ms }; motor->motor_daemon = DaemonRegister(&conf); @@ -155,6 +155,9 @@ __attribute__((noreturn)) void HTMotorTask(void const *argument) while (1) { pid_ref = motor->pid_ref; + if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) + pid_ref *= -1; + if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP) { if (setting->angle_feedback_source == OTHER_FEED) @@ -186,8 +189,6 @@ __attribute__((noreturn)) void HTMotorTask(void const *argument) } set = pid_ref; - if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) - set *= -1; LIMIT_MIN_MAX(set, T_MIN, T_MAX); tmp = float_to_uint(set, T_MIN, T_MAX, 12); diff --git a/modules/motor/LKmotor/LK9025.c b/modules/motor/LKmotor/LK9025.c index c9fadbc..b98c018 100644 --- a/modules/motor/LKmotor/LK9025.c +++ b/modules/motor/LKmotor/LK9025.c @@ -104,6 +104,8 @@ void LKMotorControl() measure = &motor->measure; setting = &motor->motor_settings; pid_ref = motor->pid_ref; + if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) + pid_ref *= -1; if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP) { @@ -132,9 +134,8 @@ void LKMotorControl() pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref); } - set = pid_ref; - if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) - set *= -1; + set = (int16_t)pid_ref; + // 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t)); diff --git a/modules/motor/LKmotor/LK9025.h b/modules/motor/LKmotor/LK9025.h index a5cef5a..0bf077c 100644 --- a/modules/motor/LKmotor/LK9025.h +++ b/modules/motor/LKmotor/LK9025.h @@ -15,7 +15,7 @@ #define SPEED_SMOOTH_COEF 0.85f #define REDUCTION_RATIO_DRIVEN 1 #define ECD_ANGLE_COEF_LK (360.0f / 65536.0f) -#define CURRENT_TORQUE_COEF_LK 0.003645f // 电流设定值转换成扭矩的系数,算出来的设定值除以这个系数就是扭矩值 +#define CURRENT_TORQUE_COEF_LK 0.00512f // 电流设定值转换成扭矩的系数,算出来的设定值除以这个系数就是扭矩值 typedef struct // 9025 {