HFUT_Firmament_EC
/
sentry_gimbal_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

修复电机反馈的bug,通信bug,ps2手柄支持,舵机的修复,cmdbug的修复

This commit is contained in:
chenfu 2023-03-22 16:37:12 +08:00
parent 3cf1831237
commit 58db6e5705
25 changed files with 231 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
.vscode/launch.json vendored
View File

@ -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依赖
},
],
}

4
.vscode/tasks.json vendored
View File

@ -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,

2
HAL_N_Middlewares/Src/freertos.c
View File

@ -30,7 +30,7 @@
#include "led_task.h"
#include "daemon.h"
#include "robot.h"
#include "ps_handle.h"
/* USER CODE END Includes */

2
HAL_N_Middlewares/Src/main.c
View File

@ -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 */

2
HAL_N_Middlewares/Src/usart.c
View File

@ -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;

2
Makefile
View File

@ -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

2
VSCode+Ozone使用方法.md
View File

@ -595,7 +595,7 @@ Project.SetOSPlugin(“plugin_name”)
2. 变量watch窗口，这里的变量不会实时更新，只有在暂停或遇到断点的时候才会更新。若希望实时查看，在这里右键选择需要动态查看的变量，选择Graph，他就会出现在**窗口8**的位置。
如果不需要可视化查看变量变化的趋势，但是想不赞同查看变量的值，请右键点击变量，选择一个合适的refresh rate：
如果不需要可视化查看变量变化的趋势，但是想不暂停查看变量的值，请右键点击变量，选择一个合适的refresh rate：
![image-20221119173731119](assets/image-20221119173731119.png)

40
application/chassis/chassis.c
View File

@ -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;

44
application/cmd/robot_cmd.c
View File

@ -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中完成设置

64
application/gimbal/gimbal.c
View File

@ -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,
};
@ -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;

2
application/robot.c
View File

@ -34,7 +34,7 @@ void RobotInit()
#endif
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
ChassisInit();
ChassisInit();
#endif
// 初始化完成,开启中断
__enable_irq();

14
application/robot_def.h
View File

@ -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)) || \

56
application/shoot/shoot.c
View File

@ -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;
}
}

37
bsp/usart/bsp_usart.c
View File

@ -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实例会调用对应的回调进行进一步的处理
* 例如:视觉协议解析/遥控器解析/裁判系统解析

15
bsp/usart/bsp_usart.h
View File

@ -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

2
modules/algorithm/controller.h
View File

@ -113,6 +113,8 @@ typedef struct // config parameter
float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // RC = 1/omegac
float Derivative_LPF_RC;
} PID_Init_Config_s;
/**

11
modules/imu/ins_task.c
View File

@ -17,7 +17,7 @@
#include "bsp_temperature.h"
#include "spi.h"
#include "user_lib.h"
#include "general_def.h"
static INS_t INS;
static IMU_Param_t IMU_Param;
static PIDInstance TempCtrl = {0};
@ -118,11 +118,18 @@ void INS_Task(void)
}
BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
// 获取最终数据
//获取最终数据
INS.Accel[X] = INS.Accel[X]*RAD_2_ANGLE;
INS.Accel[Y] = INS.Accel[Y]*RAD_2_ANGLE;
INS.Accel[Z] = INS.Accel[Z]*RAD_2_ANGLE;
INS.Gyro[X] = INS.Gyro[X]*RAD_2_ANGLE;
INS.Gyro[Y] = INS.Gyro[Y]*RAD_2_ANGLE;
INS.Gyro[Z] = INS.Gyro[Z]*RAD_2_ANGLE;
INS.Yaw = QEKF_INS.Yaw;
INS.Pitch = QEKF_INS.Pitch;
INS.Roll = QEKF_INS.Roll;
INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
}
// temperature control

10
modules/master_machine/master_process.c
View File

@ -28,6 +28,7 @@ static void DecodeVision()
{
static uint16_t flag_register;
get_protocol_info(vision_usart_instance->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);
// }
}

2
modules/master_machine/master_process.h
View File

@ -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)

17
modules/master_machine/seasky_protocol.c
View File

@ -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;
}

6
modules/motor/DJImotor/dji_motor.c
View File

@ -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;

10
modules/motor/DJImotor/dji_motor.md
View File

@ -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 = {

2
modules/motor/HTmotor/HT04.c
View File

@ -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输出限幅重复了

2
modules/motor/LKmotor/LK9025.c
View File

@ -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));

12
modules/motor/motor_def.h
View File

@ -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; // 前馈标志