From 58db6e5705b5a16b8921e4e4e251731f1edb9ec6 Mon Sep 17 00:00:00 2001 From: chenfu <2412777093@qq.com> Date: Wed, 22 Mar 2023 16:37:12 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E7=94=B5=E6=9C=BA=E5=8F=8D?= =?UTF-8?q?=E9=A6=88=E7=9A=84bug,=E9=80=9A=E4=BF=A1bug,ps2=E6=89=8B?= =?UTF-8?q?=E6=9F=84=E6=94=AF=E6=8C=81,=E8=88=B5=E6=9C=BA=E7=9A=84?= =?UTF-8?q?=E4=BF=AE=E5=A4=8D,cmdbug=E7=9A=84=E4=BF=AE=E5=A4=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/launch.json | 10 ++-- .vscode/tasks.json | 4 +- HAL_N_Middlewares/Src/freertos.c | 4 +- HAL_N_Middlewares/Src/main.c | 2 +- HAL_N_Middlewares/Src/usart.c | 2 +- Makefile | 2 + VSCode+Ozone使用方法.md | 2 +- application/chassis/chassis.c | 40 +++++++------- application/cmd/robot_cmd.c | 44 ++++++++++++---- application/gimbal/gimbal.c | 66 +++++++++++++----------- application/robot.c | 2 +- application/robot_def.h | 14 +++-- application/shoot/shoot.c | 56 +++++++++++--------- bsp/usart/bsp_usart.c | 39 ++++++++------ bsp/usart/bsp_usart.h | 15 ++---- modules/algorithm/controller.h | 2 + modules/imu/ins_task.c | 11 +++- modules/master_machine/master_process.c | 10 +++- modules/master_machine/master_process.h | 2 +- modules/master_machine/seasky_protocol.c | 17 ++++-- modules/motor/DJImotor/dji_motor.c | 6 +-- modules/motor/DJImotor/dji_motor.md | 10 ++-- modules/motor/HTmotor/HT04.c | 2 +- modules/motor/LKmotor/LK9025.c | 2 +- modules/motor/motor_def.h | 12 +++-- 25 files changed, 231 insertions(+), 145 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 440a688..58fc8ea 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -6,7 +6,7 @@ { "name": "Debug-DAP", "cwd": "${workspaceRoot}", - "executable": "${workspaceRoot}\\build\\${workspaceFolderBasename}.elf", // 要下载到调试器的文件,花括号中的是vscode两个预定义的参数 + "executable": "${workspaceRoot}\\build\\basic_framework.elf", // 要下载到调试器的文件,花括号中的是vscode两个预定义的参数 "request": "launch", "type": "cortex-debug", //使用J-link GDB Server时必须;其他GBD Server时可选（有可能帮助自动选择SVD文件） @@ -20,13 +20,14 @@ "openocd_dap.cfg", // 配置文件已经在根目录提供,若要修改以此类推,openocd的路径下的share/scripts中有各种写好的配置文件 ], "runToEntryPoint": "main" // 调试时在main函数入口停下 - //"preLaunchTask": "build task",//先运行Build任务编译项目,取消注释即可使用 + //"preLaunchTask": "log", // 调试时同时开启RTT viewer窗口 + // 若想要在调试前编译并且打开log,可只使用log的prelaunch task并为log任务添加depends on依赖 }, // 使用j-link进行调试时的参考配置 { "name": "Debug-Jlink", "cwd": "${workspaceFolder}", - "executable": "${workspaceRoot}\\build\\${workspaceFolderBasename}.elf", + "executable": "${workspaceRoot}\\build\\basic_framework.elf", "request": "launch", "type": "cortex-debug", "device": "STM32F407IG", @@ -35,7 +36,8 @@ "servertype": "jlink", "interface": "swd", "svdFile": "STM32F407.svd", - // "preLaunchTask": "build task",//先运行Build任务,取消注释即可使用 + //"preLaunchTask": "log", // 调试时同时开启RTT viewer窗口 + // 若想要在调试前编译并且打开log,可只使用log的prelaunch task并为log任务添加depends on依赖 }, ], } \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 7ef9fdf..9ff5211 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -15,7 +15,7 @@ { "label": "download dap", "type": "shell", // 如果希望在下载前编译,可以把command换成下面的命令 - "command":"mingw32-make download_dap", // "mingw32-make -j24 && mingw32-make download_dap", + "command":"mingw32-make -j24 ; mingw32-make download_dap", // "mingw32-make -j24 && mingw32-make download_dap", "group": { // 如果没有修改代码,编译任务不会消耗时间,因此推荐使用上面的替换. "kind": "build", "isDefault": false, @@ -24,7 +24,7 @@ { "label": "download jlink", "type": "shell", - "command":"mingw32-make download_jlink", // "mingw32-make -j24 && mingw32-make download_dap" + "command":"mingw32-make -j24 ; mingw32-make download_jlink", // "mingw32-make -j24 && mingw32-make download_dap" "group": { "kind": "build", "isDefault": false, diff --git a/HAL_N_Middlewares/Src/freertos.c b/HAL_N_Middlewares/Src/freertos.c index 99e9a51..a7799d2 100644 --- a/HAL_N_Middlewares/Src/freertos.c +++ b/HAL_N_Middlewares/Src/freertos.c @@ -30,7 +30,7 @@ #include "led_task.h" #include "daemon.h" #include "robot.h" - +#include "ps_handle.h" /* USER CODE END Includes */ @@ -133,7 +133,7 @@ void MX_FREERTOS_Init(void) { /* USER CODE BEGIN RTOS_THREADS */ /* add threads, ... */ /* USER CODE END RTOS_THREADS */ - + } /* USER CODE BEGIN Header_StartDefaultTask */ diff --git a/HAL_N_Middlewares/Src/main.c b/HAL_N_Middlewares/Src/main.c index 535e8c4..df310d0 100644 --- a/HAL_N_Middlewares/Src/main.c +++ b/HAL_N_Middlewares/Src/main.c @@ -83,7 +83,7 @@ int main(void) /* MCU Configuration--------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ - HAL_Init(); + HAL_Init(); /* USER CODE BEGIN Init */ diff --git a/HAL_N_Middlewares/Src/usart.c b/HAL_N_Middlewares/Src/usart.c index 8595e32..5d513a2 100644 --- a/HAL_N_Middlewares/Src/usart.c +++ b/HAL_N_Middlewares/Src/usart.c @@ -46,7 +46,7 @@ void MX_USART1_UART_Init(void) /* USER CODE END USART1_Init 1 */ huart1.Instance = USART1; - huart1.Init.BaudRate = 115200; + huart1.Init.BaudRate = 921600; huart1.Init.WordLength = UART_WORDLENGTH_8B; huart1.Init.StopBits = UART_STOPBITS_1; huart1.Init.Parity = UART_PARITY_NONE; diff --git a/Makefile b/Makefile index dc73895..1139182 100644 --- a/Makefile +++ b/Makefile @@ -141,6 +141,7 @@ modules/can_comm/can_comm.c \ modules/message_center/message_center.c \ modules/daemon/daemon.c \ modules/vofa/vofa.c \ +modules/ps_handle/ps_handle.c \ application/gimbal/gimbal.c \ application/chassis/chassis.c \ application/shoot/shoot.c \ @@ -260,6 +261,7 @@ C_INCLUDES = \ -Imodules/message_center \ -Imodules/daemon \ -Imodules/vofa \ +-Imodules/ps_handle \ -Imodules # compile gcc flags diff --git a/VSCode+Ozone使用方法.md b/VSCode+Ozone使用方法.md index a9f241c..1f90a70 100644 --- a/VSCode+Ozone使用方法.md +++ b/VSCode+Ozone使用方法.md @@ -595,7 +595,7 @@ Project.SetOSPlugin(“plugin_name”) 2. 变量watch窗口，这里的变量不会实时更新，只有在暂停或遇到断点的时候才会更新。若希望实时查看，在这里右键选择需要动态查看的变量，选择Graph，他就会出现在**窗口8**的位置。 - 如果不需要可视化查看变量变化的趋势，但是想不赞同查看变量的值，请右键点击变量，选择一个合适的refresh rate： + 如果不需要可视化查看变量变化的趋势，但是想不暂停查看变量的值，请右键点击变量，选择一个合适的refresh rate： ![image-20221119173731119](assets/image-20221119173731119.png) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 62d2810..93e4a6e 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -62,16 +62,20 @@ void ChassisInit() .can_init_config.can_handle = &hcan1, .controller_param_init_config = { .speed_PID = { - .Kp = 10, - .Ki = 0, - .Kd = 0, - .MaxOut = 4000, + .Kp = 4.5,//9 + .Ki = 0,//0.02 + .Kd = 0.01,//0.01 + .IntegralLimit = 3000, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, + .MaxOut = 12000, }, .current_PID = { - .Kp = 1, - .Ki = 0, + .Kp = 0.4,//0.7 + .Ki = 0,//0.1 .Kd = 0, - .MaxOut = 4000, + .IntegralLimit = 3000, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, + .MaxOut = 15000, }, }, .controller_setting_init_config = { @@ -83,20 +87,20 @@ void ChassisInit() .motor_type = M3508, }; // @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改. - chassis_motor_config.can_init_config.tx_id = 4; - chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; + chassis_motor_config.can_init_config.tx_id = 1; + chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_lf = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id = 3, - chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; + chassis_motor_config.can_init_config.tx_id = 2; + chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_rf = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id = 1, - chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; + chassis_motor_config.can_init_config.tx_id = 4; + chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_lb = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id = 2, - chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; + chassis_motor_config.can_init_config.tx_id = 3; + chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_rb = DJIMotorInit(&chassis_motor_config); referee_data = RefereeInit(&huart6); // 裁判系统初始化 @@ -143,7 +147,7 @@ static void MecanumCalculate() { 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_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; } @@ -210,10 +214,10 @@ void ChassisTask() chassis_cmd_recv.wz = 0; break; case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 - chassis_cmd_recv.wz = 0.05f * powf(chassis_cmd_recv.wz, 2.0f); + chassis_cmd_recv.wz = -1.5*chassis_cmd_recv.offset_angle*abs(chassis_cmd_recv.offset_angle); break; case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 - // chassis_cmd_recv.wz=sin(t) + chassis_cmd_recv.wz=4000; break; default: break; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index ebfac6b..da3cd06 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -66,6 +66,7 @@ void RobotCMDInit() }; cmd_can_comm = CANCommInit(&comm_conf); #endif // GIMBAL_BOARD +gimbal_cmd_send.pitch = 0; robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 } @@ -97,6 +98,17 @@ static void CalcOffsetAngle() #endif } +static void OffsetAnglePidCalc() +{ + // float pid_measure,pid_ref; + // static PIDInstance AngleCal = { + // .Kp = -1, + // .Ki = 0, + // .Kd = 0, + // .MaxOut = 10000, + // }; + // chassis_cmd_send.offset_angle = PIDCalculate(&AngleCal,chassis_cmd_send.offset_angle,0); +} /** * @brief 控制输入为遥控器(调试时)的模式和控制量设置 * @@ -105,9 +117,11 @@ static void RemoteControlSet() { // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据? if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台 - chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; + {chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;} else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 - chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; + {chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; + gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;} // 云台参数,确定云台控制数据 if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式 @@ -118,14 +132,23 @@ static void RemoteControlSet() // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制 if (switch_is_down(rc_data[TEMP].rc.switch_left) || vision_recv_data->target_state == NO_TARGET) { // 按照摇杆的输出大小进行角度增量,增益系数需调整 - gimbal_cmd_send.yaw += 0.0015f * (float)rc_data[TEMP].rc.rocker_l_; - gimbal_cmd_send.pitch += 0.002f * (float)rc_data[TEMP].rc.rocker_l1; - gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; + gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_; + gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1; + // if (gimbal_cmd_send.pitch <= 122) + // { + // gimbal_cmd_send.pitch =125; + // } + // else if (gimbal_cmd_send.pitch >= 175) + // { + // gimbal_cmd_send.pitch = 174; + // } + } // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 chassis_cmd_send.vx = 10.0f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向 chassis_cmd_send.vy = 10.0f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向 + //chassis_cmd_send.wz = 300; // 发射参数 if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 @@ -144,7 +167,7 @@ static void RemoteControlSet() else shoot_cmd_send.load_mode = LOAD_STOP; // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频, - shoot_cmd_send.shoot_rate = 1; + shoot_cmd_send.shoot_rate = 8; } /** @@ -173,6 +196,8 @@ static void EmergencyHandler() gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; shoot_cmd_send.shoot_mode = SHOOT_OFF; + shoot_cmd_send.friction_mode = FRICTION_OFF; + shoot_cmd_send.load_mode = LOAD_STOP; } // 遥控器右侧开关为[上],恢复正常运行 if (switch_is_up(rc_data[TEMP].rc.switch_right)) @@ -197,7 +222,6 @@ void RobotCMDTask() // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成 CalcOffsetAngle(); - // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠 if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制 RemoteControlSet(); @@ -207,11 +231,11 @@ void RobotCMDTask() EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 // 设置视觉发送数据,还需增加加速度和角速度数据 - vision_send_data.bullet_speed = chassis_fetch_data.bullet_speed; - vision_send_data.enemy_color = chassis_fetch_data.enemy_color; + vision_send_data.bullet_speed = 15; + vision_send_data.enemy_color = 0; vision_send_data.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; vision_send_data.yaw = gimbal_fetch_data.gimbal_imu_data.Yaw; - vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll; + vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll;; // 推送消息,双板通信,视觉通信等 // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置 diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 5798ce5..99769be 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -54,7 +54,7 @@ void GimbalInit() .cali_mode=BMI088_CALIBRATE_ONLINE_MODE, .work_mode=BMI088_BLOCK_PERIODIC_MODE, }; - // imu=BMI088Register(&imu_config); + // imu=BMI088Register(&imu_config); gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // YAW Motor_Init_Config_s yaw_config = { @@ -64,60 +64,68 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp = 20, + .Kp = 8, //8 .Ki = 0, .Kd = 0, - .MaxOut = 2000, - .DeadBand = 0.3, + .DeadBand = 0.1, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, + .IntegralLimit = 100, + + .MaxOut = 500, }, .speed_PID = { - .Kp = 10, - .Ki = 0, + .Kp = 50,//40 + .Ki = 200, .Kd = 0, - .MaxOut = 4000, + .Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement, + .IntegralLimit = 3000, + .MaxOut = 20000, }, .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 - // .other_speed_feedback_ptr=&gimba_IMU_data.wz; + .other_speed_feedback_ptr=&gimba_IMU_data->Gyro[2], }, .controller_setting_init_config = { - .angle_feedback_source = MOTOR_FEED, - .speed_feedback_source = MOTOR_FEED, + .angle_feedback_source = OTHER_FEED, + .speed_feedback_source = OTHER_FEED, .outer_loop_type = ANGLE_LOOP, .close_loop_type = ANGLE_LOOP | SPEED_LOOP, - .reverse_flag = MOTOR_DIRECTION_NORMAL, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = GM6020}; // PITCH Motor_Init_Config_s pitch_config = { .can_init_config = { - .can_handle = &hcan1, + .can_handle = &hcan2, .tx_id = 2, }, .controller_param_init_config = { .angle_PID = { - .Kp = 30, + .Kp =10,//10 .Ki = 0, .Kd = 0, - .MaxOut = 4000, - .DeadBand = 0.3, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, + .IntegralLimit =100, + .MaxOut = 500, }, .speed_PID = { - .Kp = 10, - .Ki = 0, - .Kd = 0, - .MaxOut = 4000, + .Kp=50,//50 + .Ki =350,//350 + .Kd =0,//0.1 + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement, + .IntegralLimit =2500, + .MaxOut = 20000, }, .other_angle_feedback_ptr = &gimba_IMU_data->Pitch, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 - // .other_speed_feedback_ptr=&gimba_IMU_data.wy, + .other_speed_feedback_ptr=(&gimba_IMU_data->Gyro[0]), }, .controller_setting_init_config = { - .angle_feedback_source = MOTOR_FEED, - .speed_feedback_source = MOTOR_FEED, + .angle_feedback_source = OTHER_FEED, + .speed_feedback_source = OTHER_FEED, .outer_loop_type = ANGLE_LOOP, - .close_loop_type = ANGLE_LOOP | SPEED_LOOP, - .reverse_flag = MOTOR_DIRECTION_NORMAL, + .close_loop_type = SPEED_LOOP | ANGLE_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = GM6020, }; @@ -145,7 +153,7 @@ void GimbalTask() DJIMotorStop(pitch_motor); break; // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 - case GIMBAL_GYRO_MODE: // 后续只保留此模式 + case GIMBAL_GYRO_MODE: // 后续只保留此模式 DJIMotorEnable(yaw_motor); DJIMotorEnable(pitch_motor); DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); @@ -159,10 +167,10 @@ void GimbalTask() case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) DJIMotorEnable(yaw_motor); DJIMotorEnable(pitch_motor); - DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, MOTOR_FEED); - DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, MOTOR_FEED); - DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, MOTOR_FEED); - DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, MOTOR_FEED); + DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); + DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); + DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); + DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); break; diff --git a/application/robot.c b/application/robot.c index 72b014c..fb24f9f 100644 --- a/application/robot.c +++ b/application/robot.c @@ -34,7 +34,7 @@ void RobotInit() #endif #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - ChassisInit(); + ChassisInit(); #endif // 初始化完成,开启中断 __enable_irq(); diff --git a/application/robot_def.h b/application/robot_def.h index 2758616..f1532f7 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -34,21 +34,25 @@ /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ // 云台参数 -#define YAW_CHASSIS_ALIGN_ECD 4000 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 +#define YAW_CHASSIS_ALIGN_ECD 2711 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 -#define PITCH_HORIZON_ECD 0 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 +#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 // 发射参数 -#define ONE_BULLET_DELTA_ANGLE 0 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 +#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 #define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f -#define NUM_PER_CIRCLE 1 // 拨盘一圈的装载量 +#define NUM_PER_CIRCLE 10 // 拨盘一圈的装载量 // 机器人底盘修改的参数,单位为mm(毫米) -#define WHEEL_BASE 300 // 纵向轴距(前进后退方向) +#define WHEEL_BASE 350 // 纵向轴距(前进后退方向) #define TRACK_WIDTH 300 // 横向轮距(左右平移方向) #define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0 #define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0 #define RADIUS_WHEEL 60 // 轮子半径 #define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换 +#define GYRO2GIMBAL_DIR_YAW 1 //陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反 +#define GYRO2GIMBAL_DIR_PITCH 1 //陀螺仪数据相较于云台的pitch的方向,1为相同,-1为相反 +#define GYRO2GIMBAL_DIR_ROLL 1 //陀螺仪数据相较于云台的roll的方向,1为相同,-1为相反 + // 检查是否出现主控板定义冲突,只允许一个开发板定义存在,否则编译会自动报错 #if (defined(ONE_BOARD) && defined(CHASSIS_BOARD)) || \ (defined(ONE_BOARD) && defined(GIMBAL_BOARD)) || \ diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index f2ff607..c9cb2e3 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -25,20 +25,23 @@ void ShootInit() Motor_Init_Config_s friction_config = { .can_init_config = { .can_handle = &hcan2, - .tx_id = 6, }, .controller_param_init_config = { .speed_PID = { - .Kp = 10, - .Ki = 0, + .Kp = 0,//20 + .Ki = 0,//1 .Kd = 0, - .MaxOut = 2000, + .Improve = PID_Integral_Limit, + .IntegralLimit = 10000, + .MaxOut = 15000, }, .current_PID = { - .Kp = 1, - .Ki = 0, + .Kp = 0,//0.7 + .Ki = 0,//0.1 .Kd = 0, - .MaxOut = 2000, + .Improve = PID_Integral_Limit, + .IntegralLimit = 10000, + .MaxOut = 15000, }, }, .controller_setting_init_config = { @@ -47,47 +50,52 @@ void ShootInit() .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, - .reverse_flag = MOTOR_DIRECTION_NORMAL, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = M3508}; + friction_config.can_init_config.tx_id = 1, friction_l = DJIMotorInit(&friction_config); - friction_config.can_init_config.tx_id = 5; // 右摩擦轮,改txid和方向就行 - friction_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; + friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行 + friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; friction_r = DJIMotorInit(&friction_config); // 拨盘电机 Motor_Init_Config_s loader_config = { .can_init_config = { .can_handle = &hcan2, - .tx_id = 7, + .tx_id = 3, }, .controller_param_init_config = { .angle_PID = { // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降 - .Kp = 10, + .Kp = 0,//10 .Ki = 0, .Kd = 0, .MaxOut = 200, }, .speed_PID = { - .Kp = 1, - .Ki = 0, + .Kp = 0,//10 + .Ki = 0,//1 .Kd = 0, - .MaxOut = 2000, + .Improve = PID_Integral_Limit, + .IntegralLimit = 5000, + .MaxOut = 5000, }, .current_PID = { - .Kp = 1, - .Ki = 0, + .Kp = 0,//0.7 + .Ki = 0,//0.1 .Kd = 0, - .MaxOut = 3000, + .Improve = PID_Integral_Limit, + .IntegralLimit = 5000, + .MaxOut = 5000, }, }, .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转 - .close_loop_type = ANGLE_LOOP | SPEED_LOOP, - .reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向 + .close_loop_type = CURRENT_LOOP | SPEED_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向 }, .motor_type = M2006 // 英雄使用m3508 }; @@ -119,8 +127,8 @@ void ShootTask() // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可 // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发) - if (hibernate_time + dead_time > DWT_GetTimeline_ms()) - return; + // if (hibernate_time + dead_time > DWT_GetTimeline_ms()) + // return; // 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换 switch (shoot_cmd_recv.load_mode) @@ -181,8 +189,8 @@ void ShootTask() DJIMotorSetRef(friction_r, 0); break; default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速. - DJIMotorSetRef(friction_l, 1000); - DJIMotorSetRef(friction_r, 1000); + DJIMotorSetRef(friction_l, 30000); + DJIMotorSetRef(friction_r, 30000); break; } } diff --git a/bsp/usart/bsp_usart.c b/bsp/usart/bsp_usart.c index 0e26191..c9edaee 100644 --- a/bsp/usart/bsp_usart.c +++ b/bsp/usart/bsp_usart.c @@ -19,7 +19,7 @@ static USARTInstance *usart_instance[DEVICE_USART_CNT] = {NULL}; /** * @brief 启动串口服务,会在每个实例注册之后自动启用接收,当前实现为DMA接收,后续可能添加IT和BLOCKING接收 - * + * * @todo 串口服务会在每个实例注册之后自动启用接收,当前实现为DMA接收,后续可能添加IT和BLOCKING接收 * 可能还要将此函数修改为extern,使得module可以控制串口的启停 * @@ -52,28 +52,37 @@ USARTInstance *USARTRegister(USART_Init_Config_s *init_config) } /* @todo 当前仅进行了形式上的封装,后续要进一步考虑是否将module的行为与bsp完全分离 */ -void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size) -{ - HAL_UART_Transmit_DMA(_instance->usart_handle, send_buf, send_size); -} - -void USARTAbort(USARTInstance *_instance, USART_TRANSFER_MODE mode) +void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size, USART_TRANSFER_MODE mode) { switch (mode) { - case USART_TRANSFER_TX: - // if(_instance.work_mode == USART_TX_DMA) - HAL_UART_AbortTransmit_IT(_instance->usart_handle); + case USART_TRANSFER_BLOCKING: + HAL_UART_Transmit(_instance->usart_handle, send_buf, send_size, 100); break; - case USART_TRANSFER_RX: - // if(_instance.work_mode == USART_RX_DMA) - HAL_UART_AbortReceive_IT(_instance->usart_handle); + case USART_TRANSFER_IT: + HAL_UART_Transmit_IT(_instance->usart_handle, send_buf, send_size); break; - case USART_TRANSFER_NONE: + case USART_TRANSFER_DMA: + HAL_UART_Transmit_DMA(_instance->usart_handle, send_buf, send_size); + break; + default: + while (1) + ; // illegal mode! check your code context! break; } } - +/* 串口发送时,gstate会被设为BUSY_TX */ +uint8_t USARTIsReady(USARTInstance *_instance) +{ + if(_instance->usart_handle->gState | HAL_UART_STATE_BUSY_TX) + { + return 0; + } + else + { + return 1; + } +} /** * @brief 每次dma/idle中断发生时，都会调用此函数.对于每个uart实例会调用对应的回调进行进一步的处理 * 例如:视觉协议解析/遥控器解析/裁判系统解析 diff --git a/bsp/usart/bsp_usart.h b/bsp/usart/bsp_usart.h index 34e74db..e7e9dcf 100644 --- a/bsp/usart/bsp_usart.h +++ b/bsp/usart/bsp_usart.h @@ -13,8 +13,9 @@ typedef void (*usart_module_callback)(); typedef enum { USART_TRANSFER_NONE=0, - USART_TRANSFER_TX, - USART_TRANSFER_RX, + USART_TRANSFER_BLOCKING, + USART_TRANSFER_IT, + USART_TRANSFER_DMA, } USART_TRANSFER_MODE; // 串口实例结构体,每个module都要包含一个实例. @@ -52,14 +53,6 @@ USARTInstance *USARTRegister(USART_Init_Config_s *init_config); * @param send_buf 待发送数据的buffer * @param send_size how many bytes to send */ -void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size); - -/** - * @brief 通过调用该函数终止串口的发送或者接收,通过传入mode参数来选择终止发送还是接收 - * - * @param _instance 串口实例 - * @param mode 选择终止发送还是接收的模式 - */ -void USARTAbort(USARTInstance *_instance,USART_TRANSFER_MODE mode); +void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size,USART_TRANSFER_MODE mode); #endif diff --git a/modules/algorithm/controller.h b/modules/algorithm/controller.h index 38b7fb1..d9c860f 100644 --- a/modules/algorithm/controller.h +++ b/modules/algorithm/controller.h @@ -113,6 +113,8 @@ typedef struct // config parameter float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|recv_buff, &flag_register, (uint8_t *)&recv_data.pitch); + // TODO: code to resolve flag_register; } @@ -58,8 +59,13 @@ void VisionSend(Vision_Send_s *send) static uint8_t send_buff[VISION_SEND_SIZE]; static uint16_t tx_len; // TODO: code to set flag_register - + flag_register = 0x0001; // 将数据转化为seasky协议的数据包 get_protocol_send_data(0x02, flag_register, &send->yaw, 3, send_buff, &tx_len); - USARTSend(vision_usart_instance, send_buff, tx_len); + USARTSend(vision_usart_instance, send_buff, tx_len,USART_TRANSFER_IT); + // if(vision_usart_instance->usart_handle->ReceptionType == HAL_UART_RECEPTION_TOIDLE) + // {HAL_UARTEx_ReceiveToIdle_DMA(vision_usart_instance->usart_handle, vision_usart_instance->recv_buff, vision_usart_instance->recv_buff_size); + // __HAL_DMA_DISABLE_IT(vision_usart_instance->usart_handle->hdmarx, DMA_IT_HT); + + // } } \ No newline at end of file diff --git a/modules/master_machine/master_process.h b/modules/master_machine/master_process.h index 224faa3..e23cf7a 100644 --- a/modules/master_machine/master_process.h +++ b/modules/master_machine/master_process.h @@ -4,7 +4,7 @@ #include "bsp_usart.h" #include "seasky_protocol.h" -#define VISION_RECV_SIZE 36u // 当前为固定值,36字节 +#define VISION_RECV_SIZE 18u // 当前为固定值,36字节 #define VISION_SEND_SIZE 36u #pragma pack(1) diff --git a/modules/master_machine/seasky_protocol.c b/modules/master_machine/seasky_protocol.c index 105a6b8..18502f7 100644 --- a/modules/master_machine/seasky_protocol.c +++ b/modules/master_machine/seasky_protocol.c @@ -15,6 +15,11 @@ #include "crc16.h" #include "memory.h" +/*获取CRC8校验码*/ +uint8_t Get_CRC8_Check(uint8_t *pchMessage,uint16_t dwLength) +{ + return crc_8(pchMessage,dwLength); +} /*检验CRC8数据段*/ static uint8_t CRC8_Check_Sum(uint8_t *pchMessage, uint16_t dwLength) { @@ -25,6 +30,12 @@ static uint8_t CRC8_Check_Sum(uint8_t *pchMessage, uint16_t dwLength) return (ucExpected == pchMessage[dwLength - 1]); } +/*获取CRC16校验码*/ +uint16_t Get_CRC16_Check(uint8_t *pchMessage,uint32_t dwLength) +{ + return crc_16(pchMessage,dwLength); +} + /*检验CRC16数据段*/ static uint16_t CRC16_Check_Sum(uint8_t *pchMessage, uint32_t dwLength) { @@ -45,7 +56,7 @@ static uint8_t protocol_heade_Check(protocol_rm_struct *pro, uint8_t *rx_buf) pro->header.sof = rx_buf[0]; if (CRC8_Check_Sum(&rx_buf[0], 4)) { - pro->header.data_length = (rx_buf[2] << 8) | rx_buf[1]; + pro->header.data_length = ((rx_buf[2] << 8) | rx_buf[1]); pro->header.crc_check = rx_buf[3]; pro->cmd_id = (rx_buf[5] << 8) | rx_buf[4]; return 1; @@ -114,9 +125,9 @@ uint16_t get_protocol_info(uint8_t *rx_buf, // 接收到的原始数据 if (CRC16_Check_Sum(&rx_buf[0], date_length)) { *flags_register = (rx_buf[7] << 8) | rx_buf[6]; - memcpy(rx_data, rx_buf + 8, 4 * sizeof(pro.header.data_length - 2)); + memcpy(rx_data, rx_buf + 8, (pro.header.data_length - 2)); return pro.cmd_id; - } + } } return 0; } diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index 36b6641..06952d8 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -238,7 +238,8 @@ void DJIMotorControl() motor_controller = &motor->motor_controller; motor_measure = &motor->motor_measure; pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改 - + if (motor_setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) + pid_ref*= -1; // 设置反转 // pid_ref会顺次通过被启用的闭环充当数据的载体 // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出 if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP) @@ -270,8 +271,7 @@ void DJIMotorControl() // 获取最终输出 set = (int16_t)pid_ref; - if (motor_setting->reverse_flag == MOTOR_DIRECTION_REVERSE) - set *= -1; // 设置反转 + // 分组填入发送数据 group = motor->sender_group; diff --git a/modules/motor/DJImotor/dji_motor.md b/modules/motor/DJImotor/dji_motor.md index bed1d82..7c936e7 100644 --- a/modules/motor/DJImotor/dji_motor.md +++ b/modules/motor/DJImotor/dji_motor.md @@ -138,7 +138,7 @@ Motor_Init_Config_s config = { .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, .speed_feedback_source = MOTOR_FEED, - .reverse_flag = MOTOR_DIRECTION_NORMAL}, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL}, // 电流环和速度环PID参数的设置,不采用计算优化则不需要传入Improve参数 // 不使用其他数据来源(如IMU),不需要传入反馈数据变量指针 .controller_param_init_config = {.current_PID = {.Improve = 0, @@ -241,7 +241,7 @@ typedef struct { Closeloop_Type_e outer_loop_type; Closeloop_Type_e close_loop_type; - Reverse_Flag_e reverse_flag; + Motor_Reverse_Flag_e motor_reverse_flag; Feedback_Source_e angle_feedback_source; Feedback_Source_e speed_feedback_source; } Motor_Control_Setting_s; @@ -268,14 +268,14 @@ typedef struct > 注意，务必分清串级控制（多环）和外层闭环的区别。前者是为了提高内环的性能，使得其能更好地跟随外环参考值；而后者描述的是系统真实的控制目标（闭环目标）。如3508，没有电流环仍然可以对速度完成闭环，对于高层的应用来说，它们本质上不关心电机内部是否还有电流环，它们只把外层闭环为速度的电机当作一个**速度伺服执行器**，**外层闭环**描述的就是真正的闭环目标。 - - 为了避开恼人的正负号，提高代码的可维护性，在初始化电机时设定`reverse_flag`使得所有电机都按照你想要的方向旋转，其定义如下： + - 为了避开恼人的正负号，提高代码的可维护性，在初始化电机时设定`motor_reverse_flag`使得所有电机都按照你想要的方向旋转，其定义如下： ```c typedef enum { MOTOR_DIRECTION_NORMAL = 0, MOTOR_DIRECTION_REVERSE = 1 - } Reverse_Flag_e; + } Motor_Reverse_Flag_e; ``` - `speed_feedback_source`以及`angle_feedback_source`是指示电机反馈来源的标志位。一般情况下电机使用自身的编码器作为控制反馈量。但在某些时候，如小陀螺模式，云台电机会使用IMU的姿态数据作为反馈数据来源。其定义如下： @@ -433,7 +433,7 @@ Motor_Init_Config_s config = { .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | ANGLE_LOOP, .speed_feedback_source = MOTOR_FEED, - .reverse_flag = MOTOR_DIRECTION_NORMAL + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL }, .controller_param_init_config = { .angle_PID = { diff --git a/modules/motor/HTmotor/HT04.c b/modules/motor/HTmotor/HT04.c index 13ec46a..38a00ea 100644 --- a/modules/motor/HTmotor/HT04.c +++ b/modules/motor/HTmotor/HT04.c @@ -137,7 +137,7 @@ void HTMotorControl() } set = pid_ref; - if (setting->reverse_flag == MOTOR_DIRECTION_REVERSE) + if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) set *= -1; LIMIT_MIN_MAX(set, T_MIN, T_MAX); // 限幅,实际上这似乎和pid输出限幅重复了 diff --git a/modules/motor/LKmotor/LK9025.c b/modules/motor/LKmotor/LK9025.c index 01af445..24e1795 100644 --- a/modules/motor/LKmotor/LK9025.c +++ b/modules/motor/LKmotor/LK9025.c @@ -110,7 +110,7 @@ void LKMotorControl() } set = pid_ref; - if (setting->reverse_flag == MOTOR_DIRECTION_REVERSE) + if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) set *= -1; // 这里随便写的,为了兼容多电机命令.后续应该将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/motor_def.h b/modules/motor/motor_def.h index ed16fec..e3cfa28 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -54,8 +54,13 @@ typedef enum { MOTOR_DIRECTION_NORMAL = 0, MOTOR_DIRECTION_REVERSE = 1 -} Reverse_Flag_e; - +} Motor_Reverse_Flag_e; +/* 反馈量正反标志 */ +typedef enum +{ + FEEDBACK_DIRECTION_NORMAL = 0, + FEEDBACK_DIRECTION_REVERSE = 1 +} Feedback_Reverse_Flag_e; typedef enum { MOTOR_STOP = 0, @@ -67,7 +72,8 @@ typedef struct { Closeloop_Type_e outer_loop_type; // 最外层的闭环,未设置时默认为最高级的闭环 Closeloop_Type_e close_loop_type; // 使用几个闭环(串级) - Reverse_Flag_e reverse_flag; // 是否反转 + Motor_Reverse_Flag_e motor_reverse_flag; // 是否反转 + Feedback_Reverse_Flag_e feedback_reverse_flag; // 反馈是否反向 Feedback_Source_e angle_feedback_source; // 角度反馈类型 Feedback_Source_e speed_feedback_source; // 速度反馈类型 Feedfoward_Type_e feedforward_flag; // 前馈标志