parent
3981b5e0b9
commit
4ba86983c0
|
|
@ -51,11 +51,11 @@ endif ()
|
|||
|
||||
include_directories(Inc Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_Driver/Inc/Legacy Middlewares/Third_Party/FreeRTOS/Source/include Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F Middlewares/ST/STM32_USB_Device_Library/Core/Inc Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc Drivers/CMSIS/Device/ST/STM32F4xx/Include Drivers/CMSIS/Include Middlewares/ST/ARM/DSP/Inc
|
||||
bsp bsp/adc bsp/can bsp/dwt bsp/flash bsp/gpio bsp/iic bsp/log bsp/pwm bsp/spi bsp/usart bsp/usb
|
||||
modules modules/alarm modules/algorithm modules/auto_aim modules/BMI088 modules/can_comm modules/daemon modules/encoder modules/imu modules/ist8310
|
||||
modules modules/alarm modules/algorithm modules/BMI088 modules/can_comm modules/daemon modules/encoder modules/imu modules/ist8310
|
||||
modules/led modules/master_machine modules/message_center modules/motor modules/oled modules/referee modules/remote modules/standard_cmd
|
||||
modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa
|
||||
modules/motor/DJImotor modules/motor/Dmmotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor
|
||||
application application/chassis application/cmd application/gimbal application/to_stretch
|
||||
modules/motor/DJImotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor
|
||||
application application/chassis application/cmd application/gimbal application/shoot
|
||||
Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config)
|
||||
|
||||
include_directories(Middlewares/ST/ARM/DSP/Inc)
|
||||
|
|
|
|||
|
|
@ -22,7 +22,6 @@
|
|||
#include "bsp_dwt.h"
|
||||
#include "referee_UI.h"
|
||||
#include "arm_math.h"
|
||||
#include "user_lib.h"
|
||||
|
||||
/* 根据robot_def.h中的macro自动计算的参数 */
|
||||
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距
|
||||
|
|
@ -43,14 +42,11 @@ static Subscriber_t *chassis_sub; // 用于订阅底盘的控
|
|||
static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令
|
||||
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据
|
||||
|
||||
static referee_info_t* referee_data; // 用于获取裁判系统的数据
|
||||
static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
|
||||
|
||||
static SuperCapInstance *cap; // 超级电容
|
||||
static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
|
||||
float chassis_power = 0;
|
||||
static first_order_filter_type_t vx_filter;
|
||||
static first_order_filter_type_t vy_filter;
|
||||
const static float32_t chassis_x_order_filter=0.05f;
|
||||
const static float32_t chassis_y_order_filter=0.05f;
|
||||
|
||||
/* 用于自旋变速策略的时间变量 */
|
||||
// static float t;
|
||||
|
|
@ -58,46 +54,44 @@ const static float32_t chassis_y_order_filter=0.05f;
|
|||
/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
|
||||
static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘
|
||||
static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
|
||||
float P_cmdall = 0;
|
||||
|
||||
void ChassisInit()
|
||||
{
|
||||
// 四个轮子的参数一样,改tx_id和反转标志位即可
|
||||
Motor_Init_Config_s chassis_motor_config = {
|
||||
.can_init_config.can_handle = &hcan1,
|
||||
.controller_param_init_config = {
|
||||
.speed_PID = {
|
||||
.Kp = 4, // 4
|
||||
.Ki = 1.2f, // 1.2
|
||||
.Kd = 0.01f, // 0.01
|
||||
.IntegralLimit = 3000,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.MaxOut = 12000,
|
||||
},
|
||||
.current_PID = {
|
||||
.Kp = 0.4f, // 0.4
|
||||
.Ki = 0.1f, // 0
|
||||
.Kd = 0,
|
||||
.IntegralLimit = 3000,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.MaxOut = 15000,
|
||||
},
|
||||
.can_init_config.can_handle = &hcan1,
|
||||
.controller_param_init_config = {
|
||||
.speed_PID = {
|
||||
.Kp = 10, // 4.5
|
||||
.Ki = 0, // 0
|
||||
.Kd = 0, // 0
|
||||
.IntegralLimit = 3000,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.MaxOut = 12000,
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.outer_loop_type = SPEED_LOOP,
|
||||
.close_loop_type = SPEED_LOOP,
|
||||
|
||||
.current_PID = {
|
||||
.Kp = 0.5, // 0.4
|
||||
.Ki = 0, // 0
|
||||
.Kd = 0,
|
||||
.IntegralLimit = 3000,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.MaxOut = 15000,
|
||||
},
|
||||
.motor_type = M3508,
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.outer_loop_type = SPEED_LOOP,
|
||||
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
|
||||
},
|
||||
.motor_type = M3508,
|
||||
};
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 3;
|
||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
// @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
|
||||
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 = 1;
|
||||
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);
|
||||
|
||||
|
|
@ -105,21 +99,19 @@ void ChassisInit()
|
|||
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.motor_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);
|
||||
|
||||
// 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); // 超级电容初始化
|
||||
|
||||
first_order_filter_init(&vy_filter,0.002f,&chassis_y_order_filter);
|
||||
first_order_filter_init(&vx_filter,0.002f,&chassis_x_order_filter);
|
||||
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); // 超级电容初始化
|
||||
|
||||
// 发布订阅初始化,如果为双板,则需要can comm来传递消息
|
||||
#ifdef CHASSIS_BOARD
|
||||
|
|
@ -142,7 +134,7 @@ void ChassisInit()
|
|||
chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
|
||||
#endif // ONE_BOARD
|
||||
}
|
||||
//225+0+215-0
|
||||
|
||||
#define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
|
||||
#define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
|
||||
#define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
|
||||
|
|
@ -153,10 +145,27 @@ void ChassisInit()
|
|||
*/
|
||||
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_rb = -chassis_vx - chassis_vy + chassis_cmd_recv.wz * (-RB_CENTER);
|
||||
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_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
|
||||
*
|
||||
*/
|
||||
static void LimitChassisOutput()
|
||||
{
|
||||
// 功率限制待添加
|
||||
// referee_data->PowerHeatData.chassis_power;
|
||||
// referee_data->PowerHeatData.chassis_power_buffer;
|
||||
|
||||
// 完成功率限制后进行电机参考输入设定
|
||||
DJIMotorSetRef(motor_lf, vt_lf);
|
||||
DJIMotorSetRef(motor_rf, vt_rf);
|
||||
DJIMotorSetRef(motor_lb, vt_lb);
|
||||
DJIMotorSetRef(motor_rb, vt_rb);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -182,6 +191,7 @@ void ChassisTask()
|
|||
#ifdef CHASSIS_BOARD
|
||||
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
|
||||
#endif // CHASSIS_BOARD
|
||||
|
||||
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
|
||||
{ // 如果出现重要模块离线或遥控器设置为急停,让电机停止
|
||||
DJIMotorStop(motor_lf);
|
||||
|
|
@ -200,21 +210,17 @@ void ChassisTask()
|
|||
// 根据控制模式设定旋转速度
|
||||
switch (chassis_cmd_recv.chassis_mode)
|
||||
{
|
||||
case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
|
||||
chassis_cmd_recv.wz = 0;
|
||||
break;
|
||||
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
|
||||
chassis_cmd_recv.wz = 2.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
|
||||
if(chassis_cmd_recv.wz > 4500)
|
||||
chassis_cmd_recv.wz = 4500;
|
||||
if(chassis_cmd_recv.wz < -4500)
|
||||
chassis_cmd_recv.wz = -4500;
|
||||
break;
|
||||
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
|
||||
chassis_cmd_recv.wz = 4500;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
|
||||
chassis_cmd_recv.wz = 0;
|
||||
break;
|
||||
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
|
||||
chassis_cmd_recv.wz = -1.5f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
|
||||
break;
|
||||
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
|
||||
chassis_cmd_recv.wz = 4000;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
// 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
|
||||
|
|
@ -222,31 +228,21 @@ void ChassisTask()
|
|||
static float sin_theta, cos_theta;
|
||||
cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
|
||||
sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
|
||||
|
||||
first_order_filter_cali(&vx_filter,chassis_cmd_recv.vx);
|
||||
first_order_filter_cali(&vy_filter,chassis_cmd_recv.vy);
|
||||
|
||||
chassis_cmd_recv.vy = vy_filter.out;
|
||||
chassis_cmd_recv.vx = vx_filter.out;
|
||||
|
||||
chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
|
||||
chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
|
||||
|
||||
|
||||
// 根据控制模式进行正运动学解算,计算底盘输出
|
||||
MecanumCalculate();
|
||||
//读取底盘功率，方便功率控制
|
||||
//chassis_power = PowerMeterGet(power_meter);
|
||||
|
||||
// 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
|
||||
//LimitChassisOutput();
|
||||
LimitChassisOutput();
|
||||
|
||||
// 根据电机的反馈速度和IMU(如果有)计算真实速度
|
||||
EstimateSpeed();
|
||||
|
||||
//获取底盘功率
|
||||
// // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
|
||||
// // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
|
||||
//chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color;
|
||||
|
||||
// chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0;
|
||||
// // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
|
||||
// chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
|
||||
// chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;
|
||||
|
|
|
|||
|
|
@ -8,12 +8,9 @@
|
|||
#include "message_center.h"
|
||||
#include "general_def.h"
|
||||
#include "dji_motor.h"
|
||||
#include "auto_aim.h"
|
||||
#include "super_cap.h"
|
||||
// bsp
|
||||
#include "bsp_dwt.h"
|
||||
#include "bsp_log.h"
|
||||
#include "referee_task.h"
|
||||
|
||||
// 私有宏,自动将编码器转换成角度值
|
||||
#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360
|
||||
|
|
@ -31,43 +28,32 @@ static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者
|
|||
|
||||
static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关
|
||||
static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
|
||||
extern SuperCapInstance *cap; // 超级电容
|
||||
static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回
|
||||
//static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回
|
||||
//static Vision_Send_s vision_send_data; // 视觉发送数据
|
||||
static RecievePacket_t *vision_recv_data; // 视觉接收数据指针,初始化时返回
|
||||
static SendPacket_t vision_send_data; // 视觉发送数据
|
||||
|
||||
//自瞄相关信息
|
||||
static Trajectory_Type_t trajectory_cal;
|
||||
static Aim_Select_Type_t aim_select;
|
||||
static uint32_t no_find_cnt; // 未发现目标计数
|
||||
static uint8_t auto_aim_flag = 0; //辅助瞄准标志位 视野内有目标开启 目标丢失关闭
|
||||
static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回
|
||||
static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回
|
||||
static Vision_Send_s vision_send_data; // 视觉发送数据
|
||||
|
||||
static Publisher_t *gimbal_cmd_pub; // 云台控制消息发布者
|
||||
static Subscriber_t *gimbal_feed_sub; // 云台反馈信息订阅者
|
||||
static Gimbal_Ctrl_Cmd_s gimbal_cmd_send; // 传递给云台的控制信息
|
||||
static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息
|
||||
|
||||
static Publisher_t *to_stretch_cmd_pub; // 伸缩控制消息发布者
|
||||
static Subscriber_t *to_stretch_feed_sub; // 伸缩反馈信息订阅者
|
||||
static To_stretch_Ctrl_Cmd_s to_stretch_cmd_send; // 传递给伸缩的控制信息
|
||||
static To_stretch_Upload_Data_s to_stretch_fetch_data; // 从发射伸缩的反馈信息
|
||||
static Publisher_t *shoot_cmd_pub; // 发射控制消息发布者
|
||||
static Subscriber_t *shoot_feed_sub; // 发射反馈信息订阅者
|
||||
static Shoot_Ctrl_Cmd_s shoot_cmd_send; // 传递给发射的控制信息
|
||||
static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
|
||||
|
||||
static Robot_Status_e robot_state; // 机器人整体工作状态
|
||||
|
||||
static referee_info_t *referee_data; // 用于获取裁判系统的数据
|
||||
|
||||
void RobotCMDInit() {
|
||||
void RobotCMDInit()
|
||||
{
|
||||
rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
|
||||
vision_recv_data = VisionInit(&huart1); // 视觉通信串口
|
||||
|
||||
referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI
|
||||
|
||||
gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
|
||||
gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
|
||||
to_stretch_cmd_pub = PubRegister("shoot_cmd", sizeof(To_stretch_Ctrl_Cmd_s));
|
||||
to_stretch_feed_sub = SubRegister("shoot_feed", sizeof(To_stretch_Upload_Data_s));
|
||||
shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
|
||||
shoot_feed_sub = SubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
|
||||
|
||||
#ifdef ONE_BOARD // 双板兼容
|
||||
chassis_cmd_pub = PubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
|
||||
|
|
@ -95,7 +81,8 @@ void RobotCMDInit() {
|
|||
* 单圈绝对角度的范围是0~360,说明文档中有图示
|
||||
*
|
||||
*/
|
||||
static void CalcOffsetAngle() {
|
||||
static void CalcOffsetAngle()
|
||||
{
|
||||
// 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
|
||||
static float angle;
|
||||
angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
|
||||
|
|
@ -120,98 +107,139 @@ static void CalcOffsetAngle() {
|
|||
* @brief 控制输入为遥控器(调试时)的模式和控制量设置
|
||||
*
|
||||
*/
|
||||
static void update_ui_data() {
|
||||
|
||||
}
|
||||
|
||||
|
||||
static void RemoteControlSet() {
|
||||
static void RemoteControlSet()
|
||||
{
|
||||
// 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据?
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],小陀螺
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台
|
||||
{
|
||||
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_FOLLOW_GIMBAL_YAW;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
}
|
||||
// else if (switch_is_up(rc_data[TEMP].rc.switch_right))// 右侧开关状态[上],小陀螺
|
||||
// {
|
||||
// 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;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
|
||||
}
|
||||
|
||||
// 云台参数,确定云台控制数据
|
||||
if (switch_is_mid(rc_data[TEMP].rc.switch_left) ||
|
||||
(vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
|
||||
&& vision_recv_data->vx == 0 && vision_recv_data->vy == 0 &&
|
||||
vision_recv_data->vz == 0)) // 左侧开关状态为[中],或视觉未识别到目标,纯遥控器拨杆控制
|
||||
if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式
|
||||
{
|
||||
// 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制
|
||||
// ...
|
||||
aim_select.suggest_fire = 0;
|
||||
gimbal_cmd_send.yaw -= 0.0025f * (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 >= PITCH_MAX_ANGLE) gimbal_cmd_send.pitch = PITCH_MAX_ANGLE;
|
||||
if (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
|
||||
|
||||
|
||||
}
|
||||
// 左侧开关状态为[下],视觉模式
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_left)) {
|
||||
//auto_aim_mode();
|
||||
// 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_left) || vision_recv_data->target_state == NO_TARGET)
|
||||
{ // 按照摇杆的输出大小进行角度增量,增益系数需调整
|
||||
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;
|
||||
}
|
||||
// 云台软件限位
|
||||
|
||||
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整，遥控器输入灵敏度
|
||||
chassis_cmd_send.vx = 8.0f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向
|
||||
chassis_cmd_send.vy = 8.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向
|
||||
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
|
||||
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数值方向
|
||||
|
||||
// 发射参数
|
||||
if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
|
||||
; // 弹舱舵机控制,待添加servo_motor模块,开启
|
||||
else
|
||||
; // 弹舱舵机控制,待添加servo_motor模块,关闭
|
||||
|
||||
// 摩擦轮控制,拨轮向上打为负,向下为正
|
||||
if (rc_data[TEMP].rc.dial < -100) // 向上超过100,打开摩擦轮
|
||||
shoot_cmd_send.friction_mode = FRICTION_ON;
|
||||
else
|
||||
shoot_cmd_send.friction_mode = FRICTION_OFF;
|
||||
// 拨弹控制,遥控器固定为一种拨弹模式,可自行选择
|
||||
if (rc_data[TEMP].rc.dial < -500)
|
||||
shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
|
||||
else
|
||||
shoot_cmd_send.load_mode = LOAD_STOP;
|
||||
// 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
|
||||
shoot_cmd_send.shoot_rate = 8;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 输入为键鼠时模式和控制量设置
|
||||
*
|
||||
*/
|
||||
static void MouseKeySet() {
|
||||
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 3000 - rc_data[TEMP].key[KEY_PRESS].s * 3000; // 系数待测
|
||||
chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 3000 - rc_data[TEMP].key[KEY_PRESS].d * 3000;
|
||||
static void MouseKeySet()
|
||||
{
|
||||
chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].w * 300 - rc_data[TEMP].key[KEY_PRESS].s * 300; // 系数待测
|
||||
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].s * 300 - rc_data[TEMP].key[KEY_PRESS].d * 300;
|
||||
|
||||
gimbal_cmd_send.yaw -= (float) rc_data[TEMP].mouse.x / 660 * 4; // 系数待测
|
||||
gimbal_cmd_send.pitch += (float) rc_data[TEMP].mouse.y / 660 * 6;
|
||||
gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测
|
||||
gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10;
|
||||
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键手动刷新UI
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速
|
||||
{
|
||||
case 1:
|
||||
MyUIInit();
|
||||
rc_data[TEMP].key_count[KEY_PRESS][Key_R]++;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
case 0:
|
||||
shoot_cmd_send.bullet_speed = 15;
|
||||
break;
|
||||
case 1:
|
||||
shoot_cmd_send.bullet_speed = 18;
|
||||
break;
|
||||
default:
|
||||
shoot_cmd_send.bullet_speed = 30;
|
||||
break;
|
||||
}
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_V] % 2) // V键开关红点激光
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式
|
||||
{
|
||||
case 0:
|
||||
HAL_GPIO_WritePin(GPIOC,GPIO_PIN_8,GPIO_PIN_SET);
|
||||
break;
|
||||
default:
|
||||
HAL_GPIO_WritePin(GPIOC,GPIO_PIN_8,GPIO_PIN_RESET);
|
||||
break;
|
||||
case 0:
|
||||
shoot_cmd_send.load_mode = LOAD_STOP;
|
||||
break;
|
||||
case 1:
|
||||
shoot_cmd_send.load_mode = LOAD_1_BULLET;
|
||||
break;
|
||||
case 2:
|
||||
shoot_cmd_send.load_mode = LOAD_3_BULLET;
|
||||
break;
|
||||
default:
|
||||
shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
|
||||
break;
|
||||
}
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱
|
||||
{
|
||||
case 0:
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
|
||||
break;
|
||||
default:
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
break;
|
||||
case 0:
|
||||
shoot_cmd_send.lid_mode = LID_OPEN;
|
||||
break;
|
||||
default:
|
||||
shoot_cmd_send.lid_mode = LID_CLOSE;
|
||||
break;
|
||||
}
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮
|
||||
{
|
||||
case 0:
|
||||
shoot_cmd_send.friction_mode = FRICTION_OFF;
|
||||
break;
|
||||
default:
|
||||
shoot_cmd_send.friction_mode = FRICTION_ON;
|
||||
break;
|
||||
}
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度
|
||||
{
|
||||
case 0:
|
||||
chassis_cmd_send.chassis_speed_buff = 40;
|
||||
break;
|
||||
case 1:
|
||||
chassis_cmd_send.chassis_speed_buff = 60;
|
||||
break;
|
||||
case 2:
|
||||
chassis_cmd_send.chassis_speed_buff = 80;
|
||||
break;
|
||||
default:
|
||||
chassis_cmd_send.chassis_speed_buff = 100;
|
||||
break;
|
||||
}
|
||||
switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量
|
||||
{
|
||||
case 1:
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -222,56 +250,63 @@ static void MouseKeySet() {
|
|||
* @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现
|
||||
*
|
||||
*/
|
||||
static void EmergencyHandler() {
|
||||
static void EmergencyHandler()
|
||||
{
|
||||
// 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正
|
||||
if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
|
||||
{
|
||||
robot_state = ROBOT_STOP;
|
||||
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;
|
||||
LOGERROR("[CMD] emergency stop!");
|
||||
}
|
||||
// 遥控器右侧开关为[上],恢复正常运行
|
||||
if (switch_is_up(rc_data[TEMP].rc.switch_right)) {
|
||||
if (switch_is_up(rc_data[TEMP].rc.switch_right))
|
||||
{
|
||||
robot_state = ROBOT_READY;
|
||||
shoot_cmd_send.shoot_mode = SHOOT_ON;
|
||||
LOGINFO("[CMD] reinstate, robot ready");
|
||||
}
|
||||
}
|
||||
|
||||
/* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */
|
||||
void RobotCMDTask() {
|
||||
void RobotCMDTask()
|
||||
{
|
||||
// 从其他应用获取回传数据
|
||||
#ifdef ONE_BOARD
|
||||
SubGetMessage(chassis_feed_sub, (void *) &chassis_fetch_data);
|
||||
SubGetMessage(chassis_feed_sub, (void *)&chassis_fetch_data);
|
||||
#endif // ONE_BOARD
|
||||
#ifdef GIMBAL_BOARD
|
||||
chassis_fetch_data = *(Chassis_Upload_Data_s *)CANCommGet(cmd_can_comm);
|
||||
#endif // GIMBAL_BOARD
|
||||
SubGetMessage(to_stretch_feed_sub, &to_stretch_fetch_data);
|
||||
SubGetMessage(shoot_feed_sub, &shoot_fetch_data);
|
||||
SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
|
||||
update_ui_data();
|
||||
|
||||
// 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
|
||||
CalcOffsetAngle();
|
||||
// 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_left) ||
|
||||
switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],[中],遥控器控制
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
|
||||
RemoteControlSet();
|
||||
else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
|
||||
MouseKeySet();
|
||||
|
||||
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
|
||||
|
||||
// 设置视觉发送数据,还需增加加速度和角速度数据
|
||||
// VisionSetFlag(chassis_fetch_data.enemy_color,,chassis_fetch_data.bullet_speed)
|
||||
|
||||
// 推送消息,双板通信,视觉通信等
|
||||
// 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置
|
||||
#ifdef ONE_BOARD
|
||||
PubPushMessage(chassis_cmd_pub, (void *) &chassis_cmd_send);
|
||||
PubPushMessage(chassis_cmd_pub, (void *)&chassis_cmd_send);
|
||||
#endif // ONE_BOARD
|
||||
#ifdef GIMBAL_BOARD
|
||||
CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send);
|
||||
#endif // GIMBAL_BOARD
|
||||
PubPushMessage(to_stretch_cmd_pub, (void *) &to_stretch_cmd_send);
|
||||
PubPushMessage(gimbal_cmd_pub, (void *) &gimbal_cmd_send);
|
||||
PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
|
||||
PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
|
||||
VisionSend(&vision_send_data);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -14,5 +14,4 @@ void RobotCMDInit();
|
|||
*/
|
||||
void RobotCMDTask();
|
||||
|
||||
|
||||
#endif // !ROBOT_CMD_H
|
||||
|
|
@ -15,7 +15,7 @@
|
|||
|
||||
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
|
||||
#include "gimbal.h"
|
||||
#include "to_stretch.h"
|
||||
#include "shoot.h"
|
||||
#include "robot_cmd.h"
|
||||
#endif
|
||||
|
||||
|
|
@ -32,7 +32,7 @@ void RobotInit()
|
|||
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
|
||||
RobotCMDInit();
|
||||
GimbalInit();
|
||||
To_stretchInit();
|
||||
ShootInit();
|
||||
#endif
|
||||
|
||||
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
|
||||
|
|
@ -50,7 +50,7 @@ void RobotTask()
|
|||
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
|
||||
RobotCMDTask();
|
||||
GimbalTask();
|
||||
To_stretchTask();
|
||||
ShootTask();
|
||||
#endif
|
||||
|
||||
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
|
||||
|
|
|
|||
|
|
@ -26,21 +26,21 @@
|
|||
|
||||
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
|
||||
// 云台参数
|
||||
#define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机position值,若对云台有机械改动需要修改
|
||||
#define YAW_ECD_GREATER_THAN_4096 1 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
|
||||
#define YAW_CHASSIS_ALIGN_ECD 2711 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
|
||||
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
|
||||
#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
|
||||
#define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
|
||||
#define PITCH_MIN_ANGLE 0 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
|
||||
// 发射参数
|
||||
#define ONE_BULLET_DELTA_ANGLE 1933.272 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
|
||||
#define REDUCTION_RATIO_LOADER 27.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f//初始是49，增加为27
|
||||
#define NUM_PER_CIRCLE 5 // 拨盘一圈的装载量
|
||||
#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
|
||||
#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
|
||||
#define NUM_PER_CIRCLE 10 // 拨盘一圈的装载量
|
||||
// 机器人底盘修改的参数,单位为mm(毫米)
|
||||
#define WHEEL_BASE 433.86 // 纵向轴距(前进后退方向)
|
||||
#define TRACK_WIDTH 463.92 // 横向轮距(左右平移方向)
|
||||
#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 75 // 轮子半径
|
||||
#define RADIUS_WHEEL 60 // 轮子半径
|
||||
#define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
|
||||
|
||||
#define GYRO2GIMBAL_DIR_YAW 1 // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
|
||||
|
|
@ -94,15 +94,40 @@ typedef enum
|
|||
GIMBAL_ZERO_FORCE = 0, // 电流零输入
|
||||
GIMBAL_FREE_MODE, // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle;似乎可以改为全部用IMU数据?
|
||||
GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
|
||||
ADJUST_PITCH_MODE, //找pitch的限位
|
||||
} gimbal_mode_e;
|
||||
|
||||
// 伸缩模式设置
|
||||
// 发射模式设置
|
||||
typedef enum
|
||||
{
|
||||
TO_STRETCH_ZERO_FORCE = 0, // 电流零输入
|
||||
SHOOT_OFF = 0,
|
||||
SHOOT_ON,
|
||||
} shoot_mode_e;
|
||||
typedef enum
|
||||
{
|
||||
FRICTION_OFF = 0, // 摩擦轮关闭
|
||||
FRICTION_ON, // 摩擦轮开启
|
||||
} friction_mode_e;
|
||||
|
||||
} to_stretch_mode_e;
|
||||
typedef enum
|
||||
{
|
||||
LID_OPEN = 0, // 弹舱盖打开
|
||||
LID_CLOSE, // 弹舱盖关闭
|
||||
} lid_mode_e;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
LOAD_STOP = 0, // 停止发射
|
||||
LOAD_REVERSE, // 反转
|
||||
LOAD_1_BULLET, // 单发
|
||||
LOAD_3_BULLET, // 三发
|
||||
LOAD_BURSTFIRE, // 连发
|
||||
} loader_mode_e;
|
||||
|
||||
// 功率限制,从裁判系统获取,是否有必要保留?
|
||||
typedef struct
|
||||
{ // 功率控制
|
||||
float chassis_power_mx;
|
||||
} Chassis_Power_Data_s;
|
||||
|
||||
/* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */
|
||||
/**
|
||||
|
|
@ -118,8 +143,7 @@ typedef struct
|
|||
float wz; // 旋转速度
|
||||
float offset_angle; // 底盘和归中位置的夹角
|
||||
chassis_mode_e chassis_mode;
|
||||
int chassis_power_buffer;
|
||||
uint16_t chassis_power_limit;
|
||||
int chassis_speed_buff;
|
||||
// UI部分
|
||||
// ...
|
||||
|
||||
|
|
@ -135,12 +159,17 @@ typedef struct
|
|||
gimbal_mode_e gimbal_mode;
|
||||
} Gimbal_Ctrl_Cmd_s;
|
||||
|
||||
// cmd发布的伸缩控制数据,由to_stretch订阅
|
||||
// cmd发布的发射控制数据,由shoot订阅
|
||||
typedef struct
|
||||
{
|
||||
to_stretch_mode_e to_stretch_mode;
|
||||
|
||||
} To_stretch_Ctrl_Cmd_s;
|
||||
shoot_mode_e shoot_mode;
|
||||
loader_mode_e load_mode;
|
||||
lid_mode_e lid_mode;
|
||||
friction_mode_e friction_mode;
|
||||
Bullet_Speed_e bullet_speed; // 弹速枚举
|
||||
uint8_t rest_heat;
|
||||
float shoot_rate; // 连续发射的射频,unit per s,发/秒
|
||||
} Shoot_Ctrl_Cmd_s;
|
||||
|
||||
/* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/
|
||||
/**
|
||||
|
|
@ -157,23 +186,25 @@ typedef struct
|
|||
// float real_vx;
|
||||
// float real_vy;
|
||||
// float real_wz;
|
||||
float cap_power;
|
||||
|
||||
uint8_t rest_heat; // 剩余枪口热量
|
||||
Bullet_Speed_e bullet_speed; // 弹速限制
|
||||
Enemy_Color_e enemy_color; // 0 for blue, 1 for red
|
||||
|
||||
} Chassis_Upload_Data_s;
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
attitude_t gimbal_imu_data;
|
||||
float yaw_motor_single_round_angle;
|
||||
float pitch_motor_total_angle;
|
||||
uint16_t yaw_motor_single_round_angle;
|
||||
} Gimbal_Upload_Data_s;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
|
||||
} To_stretch_Upload_Data_s;
|
||||
// code to go here
|
||||
// ...
|
||||
} Shoot_Upload_Data_s;
|
||||
|
||||
#pragma pack() // 开启字节对齐,结束前面的#pragma pack(1)
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,213 @@
|
|||
#include "shoot.h"
|
||||
#include "robot_def.h"
|
||||
|
||||
#include "dji_motor.h"
|
||||
#include "message_center.h"
|
||||
#include "bsp_dwt.h"
|
||||
#include "general_def.h"
|
||||
|
||||
/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
|
||||
static DJIMotorInstance *friction_l, *friction_r, *loader; // 拨盘电机
|
||||
// static servo_instance *lid; 需要增加弹舱盖
|
||||
|
||||
static Publisher_t *shoot_pub;
|
||||
static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自cmd的发射控制信息
|
||||
static Subscriber_t *shoot_sub;
|
||||
static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息
|
||||
|
||||
// dwt定时,计算冷却用
|
||||
static float hibernate_time = 0, dead_time = 0;
|
||||
|
||||
void ShootInit()
|
||||
{
|
||||
// 左摩擦轮
|
||||
Motor_Init_Config_s friction_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan2,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.speed_PID = {
|
||||
.Kp = 0, // 20
|
||||
.Ki = 0, // 1
|
||||
.Kd = 0,
|
||||
.Improve = PID_Integral_Limit,
|
||||
.IntegralLimit = 10000,
|
||||
.MaxOut = 15000,
|
||||
},
|
||||
.current_PID = {
|
||||
.Kp = 0, // 0.7
|
||||
.Ki = 0, // 0.1
|
||||
.Kd = 0,
|
||||
.Improve = PID_Integral_Limit,
|
||||
.IntegralLimit = 10000,
|
||||
.MaxOut = 15000,
|
||||
},
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
|
||||
.outer_loop_type = SPEED_LOOP,
|
||||
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
|
||||
.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 = 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 = 3,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
// 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
|
||||
.Kp = 0, // 10
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 200,
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = 0, // 10
|
||||
.Ki = 0, // 1
|
||||
.Kd = 0,
|
||||
.Improve = PID_Integral_Limit,
|
||||
.IntegralLimit = 5000,
|
||||
.MaxOut = 5000,
|
||||
},
|
||||
.current_PID = {
|
||||
.Kp = 0, // 0.7
|
||||
.Ki = 0, // 0.1
|
||||
.Kd = 0,
|
||||
.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 = CURRENT_LOOP | SPEED_LOOP,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
|
||||
},
|
||||
.motor_type = M2006 // 英雄使用m3508
|
||||
};
|
||||
loader = DJIMotorInit(&loader_config);
|
||||
|
||||
shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
|
||||
shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
|
||||
}
|
||||
|
||||
/* 机器人发射机构控制核心任务 */
|
||||
void ShootTask()
|
||||
{
|
||||
// 从cmd获取控制数据
|
||||
SubGetMessage(shoot_sub, &shoot_cmd_recv);
|
||||
|
||||
// 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止)
|
||||
if (shoot_cmd_recv.shoot_mode == SHOOT_OFF)
|
||||
{
|
||||
DJIMotorStop(friction_l);
|
||||
DJIMotorStop(friction_r);
|
||||
DJIMotorStop(loader);
|
||||
}
|
||||
else // 恢复运行
|
||||
{
|
||||
DJIMotorEnable(friction_l);
|
||||
DJIMotorEnable(friction_r);
|
||||
DJIMotorEnable(loader);
|
||||
}
|
||||
|
||||
// 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
|
||||
// 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
|
||||
// if (hibernate_time + dead_time > DWT_GetTimeline_ms())
|
||||
// return;
|
||||
|
||||
// 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换
|
||||
switch (shoot_cmd_recv.load_mode)
|
||||
{
|
||||
// 停止拨盘
|
||||
case LOAD_STOP:
|
||||
DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
|
||||
DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快
|
||||
break;
|
||||
// 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
|
||||
case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
|
||||
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
|
||||
DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
|
||||
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
|
||||
dead_time = 150; // 完成1发弹丸发射的时间
|
||||
break;
|
||||
// 三连发,如果不需要后续可能删除
|
||||
case LOAD_3_BULLET:
|
||||
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
|
||||
DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
|
||||
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
|
||||
dead_time = 300; // 完成3发弹丸发射的时间
|
||||
break;
|
||||
// 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
|
||||
case LOAD_BURSTFIRE:
|
||||
DJIMotorOuterLoop(loader, SPEED_LOOP);
|
||||
DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8);
|
||||
// x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
|
||||
break;
|
||||
// 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
|
||||
// 也有可能需要从switch-case中独立出来
|
||||
case LOAD_REVERSE:
|
||||
DJIMotorOuterLoop(loader, SPEED_LOOP);
|
||||
// ...
|
||||
break;
|
||||
default:
|
||||
while (1)
|
||||
; // 未知模式,停止运行,检查指针越界,内存溢出等问题
|
||||
}
|
||||
|
||||
// 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
|
||||
if (shoot_cmd_recv.friction_mode == FRICTION_ON)
|
||||
{
|
||||
// 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
|
||||
switch (shoot_cmd_recv.bullet_speed)
|
||||
{
|
||||
case SMALL_AMU_15:
|
||||
DJIMotorSetRef(friction_l, 0);
|
||||
DJIMotorSetRef(friction_r, 0);
|
||||
break;
|
||||
case SMALL_AMU_18:
|
||||
DJIMotorSetRef(friction_l, 0);
|
||||
DJIMotorSetRef(friction_r, 0);
|
||||
break;
|
||||
case SMALL_AMU_30:
|
||||
DJIMotorSetRef(friction_l, 0);
|
||||
DJIMotorSetRef(friction_r, 0);
|
||||
break;
|
||||
default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
|
||||
DJIMotorSetRef(friction_l, 30000);
|
||||
DJIMotorSetRef(friction_r, 30000);
|
||||
break;
|
||||
}
|
||||
}
|
||||
else // 关闭摩擦轮
|
||||
{
|
||||
DJIMotorSetRef(friction_l, 0);
|
||||
DJIMotorSetRef(friction_r, 0);
|
||||
}
|
||||
|
||||
// 开关弹舱盖
|
||||
if (shoot_cmd_recv.lid_mode == LID_CLOSE)
|
||||
{
|
||||
//...
|
||||
}
|
||||
else if (shoot_cmd_recv.lid_mode == LID_OPEN)
|
||||
{
|
||||
//...
|
||||
}
|
||||
|
||||
// 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
|
||||
PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);
|
||||
}
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
#ifndef SHOOT_H
|
||||
#define SHOOT_H
|
||||
|
||||
/**
|
||||
* @brief 发射初始化,会被RobotInit()调用
|
||||
*
|
||||
*/
|
||||
void ShootInit();
|
||||
|
||||
/**
|
||||
* @brief 发射任务
|
||||
*
|
||||
*/
|
||||
void ShootTask();
|
||||
|
||||
#endif // SHOOT_H
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
# shoot
|
||||
|
||||
|
||||
|
||||
## 工作流程
|
||||
|
||||
初始化3个电机和一个舵机，包括发射的2个m3508，拨盘的m2006和弹仓盖上的舵机（双开门舵机可能要换成2个）。m2006初始化时设置为速度闭环，防止上电乱转。订阅shoot_cmd话题（robot_cmd发布的）并发布shoot_feed话题（robot_cmd会订阅）。
|
||||
|
||||
1. 从shoot_cmd获取消息
|
||||
2. 根据工作模式确定是否急停
|
||||
3. 如果之前是单发模式或3发模式并且冷却时间没到，直接结束本次任务，等待下一次进入
|
||||
4. 如果已经冷却完成，根据发来的拨盘模式，设定m2006的闭环类型和参考值
|
||||
5. 根据发来的弹速数据，设定摩擦轮的参考值
|
||||
6. 根据发来的弹舱数据进行开合
|
||||
7. 设定反馈数据，推送到shoot_feed话题
|
||||
|
|
@ -1,99 +0,0 @@
|
|||
#include "to_stretch.h"
|
||||
#include "robot_def.h"
|
||||
#include "dji_motor.h"
|
||||
#include "message_center.h"
|
||||
#include "referee_task.h"
|
||||
#include "general_def.h"
|
||||
#include "bsp_dwt.h"
|
||||
#include "referee_UI.h"
|
||||
#include "arm_math.h"
|
||||
#include "user_lib.h"
|
||||
|
||||
//to_stretch
|
||||
|
||||
static Publisher_t *to_stretch_pub; // 用于发布伸缩的数据
|
||||
static Subscriber_t *to_stretch_sub; // 用于订阅伸缩的控制命令
|
||||
static To_stretch_Ctrl_Cmd_s to_stretch_cmd_recv; // 伸缩接收到的控制命令
|
||||
static To_stretch_Upload_Data_s to_stretch_feedback_data; // 伸缩回传的反馈数据
|
||||
|
||||
static DJIMotorInstance *motor_lu, *motor_ru, *motor_ld, *motor_rd;
|
||||
|
||||
void To_stretchInit() {
|
||||
Motor_Init_Config_s chassis_motor_config = {
|
||||
.can_init_config.can_handle = &hcan1,
|
||||
.controller_param_init_config = {
|
||||
.speed_PID = {
|
||||
.Kp = 4,
|
||||
.Ki = 1.2f,
|
||||
.Kd = 0.01f,
|
||||
.IntegralLimit = 3000,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.MaxOut = 12000,
|
||||
},
|
||||
.current_PID = {
|
||||
.Kp = 0.4f,
|
||||
.Ki = 0.1f,
|
||||
.Kd = 0,
|
||||
.IntegralLimit = 3000,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.MaxOut = 15000,
|
||||
},
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.outer_loop_type = SPEED_LOOP,
|
||||
.close_loop_type = SPEED_LOOP,
|
||||
|
||||
},
|
||||
.motor_type = M3508,
|
||||
};
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 5;
|
||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
motor_lu = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 6;
|
||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
motor_ru = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 7;
|
||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
motor_ld = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 8;
|
||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
motor_rd = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
}
|
||||
|
||||
/* 机器人伸缩控制核心任务 */
|
||||
void To_stretchTask()
|
||||
{
|
||||
|
||||
SubGetMessage(to_stretch_sub, &to_stretch_cmd_recv);
|
||||
|
||||
if (to_stretch_cmd_recv.to_stretch_mode == TO_STRETCH_ZERO_FORCE)
|
||||
{ // 如果出现重要模块离线或遥控器设置为急停,让电机停止
|
||||
DJIMotorStop(motor_lu);
|
||||
DJIMotorStop(motor_ru);
|
||||
DJIMotorStop(motor_ld);
|
||||
DJIMotorStop(motor_rd);
|
||||
}
|
||||
else
|
||||
{ // 正常工作
|
||||
DJIMotorEnable(motor_lu);
|
||||
DJIMotorEnable(motor_ru);
|
||||
DJIMotorEnable(motor_ld);
|
||||
DJIMotorEnable(motor_rd);
|
||||
}
|
||||
|
||||
// 推送反馈消息
|
||||
#ifdef ONE_BOARD
|
||||
PubPushMessage(to_stretch_pub, (void *)&to_stretch_feedback_data);
|
||||
#endif
|
||||
#ifdef CHASSIS_BOARD
|
||||
CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data);
|
||||
#endif // CHASSIS_BOARD
|
||||
}
|
||||
|
||||
|
|
@ -1,17 +0,0 @@
|
|||
#ifndef TO_STRETCH_H
|
||||
#define TO_STRETCH_H
|
||||
|
||||
|
||||
/**
|
||||
* @brief 伸缩初始化,会被RobotInit()调用
|
||||
*
|
||||
*/
|
||||
void To_stretchInit();
|
||||
|
||||
/**
|
||||
* @brief 伸缩任务
|
||||
*
|
||||
*/
|
||||
void To_stretchTask();
|
||||
|
||||
#endif //TO_STRETCH_H
|
||||
|
|
@ -1,29 +1,7 @@
|
|||
#include "crc16.h"
|
||||
|
||||
static uint8_t crc_tab16_init = 0;
|
||||
static uint16_t crc_tab16[256] = {
|
||||
0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, 0x9dc1, 0xaf5a, 0xbed3,
|
||||
0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
|
||||
0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399,
|
||||
0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
|
||||
0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50,
|
||||
0xfbef, 0xea66, 0xd8fd, 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
|
||||
0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, 0x430c, 0x7197, 0x601e,
|
||||
0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
|
||||
0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5,
|
||||
0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
|
||||
0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693,
|
||||
0xc22c, 0xd3a5, 0xe13e, 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
|
||||
0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, 0x0948, 0x3bd3, 0x2a5a,
|
||||
0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
|
||||
0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710,
|
||||
0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
|
||||
0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df,
|
||||
0x0c60, 0x1de9, 0x2f72, 0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
|
||||
0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e, 0xf687, 0xc41c, 0xd595,
|
||||
0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
|
||||
0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c,
|
||||
0x3de3, 0x2c6a, 0x1ef1, 0x0f78};
|
||||
static uint16_t crc_tab16[256];
|
||||
|
||||
/*
|
||||
* uint16_t crc_16( const unsigned char *input_str, size_t num_bytes );
|
||||
|
|
@ -33,29 +11,21 @@ static uint16_t crc_tab16[256] = {
|
|||
*要检查的字节也是一个参数。字符串中的字节数为
|
||||
*受恒定大小最大值的限制。
|
||||
*/
|
||||
uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes) {
|
||||
// uint16_t crc;
|
||||
// const uint8_t *ptr;
|
||||
// uint16_t a;
|
||||
//// if (!crc_tab16_init)
|
||||
//// init_crc16_tab();
|
||||
// crc = CRC_START_16;
|
||||
// ptr = input_str;
|
||||
// if (ptr != NULL)
|
||||
// for (a = 0; a < num_bytes; a++) {
|
||||
// crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t) *ptr++) & 0x00FF];
|
||||
// }
|
||||
// return crc;
|
||||
uint8_t ch_data;
|
||||
uint16_t wCRC = 0xFFFF;
|
||||
|
||||
if (input_str == NULL) return 0xFFFF;
|
||||
while (num_bytes--) {
|
||||
ch_data = *input_str++;
|
||||
(wCRC) =
|
||||
((uint16_t)(wCRC) >> 8) ^ crc_tab16[((uint16_t)(wCRC) ^ (uint16_t)(ch_data)) & 0x00ff];
|
||||
}
|
||||
return wCRC;
|
||||
uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes)
|
||||
{
|
||||
uint16_t crc;
|
||||
const uint8_t *ptr;
|
||||
uint16_t a;
|
||||
if (!crc_tab16_init)
|
||||
init_crc16_tab();
|
||||
crc = CRC_START_16;
|
||||
ptr = input_str;
|
||||
if (ptr != NULL)
|
||||
for (a = 0; a < num_bytes; a++)
|
||||
{
|
||||
crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)*ptr++) & 0x00FF];
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -66,7 +36,8 @@ uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes) {
|
|||
*要检查的字节数也是一个参数。
|
||||
*/
|
||||
|
||||
uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes) {
|
||||
uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes)
|
||||
{
|
||||
uint16_t crc;
|
||||
const uint8_t *ptr;
|
||||
uint16_t a;
|
||||
|
|
@ -77,9 +48,10 @@ uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes) {
|
|||
crc = CRC_START_MODBUS;
|
||||
ptr = input_str;
|
||||
if (ptr != NULL)
|
||||
for (a = 0; a < num_bytes; a++) {
|
||||
for (a = 0; a < num_bytes; a++)
|
||||
{
|
||||
|
||||
crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t) *ptr++) & 0x00FF];
|
||||
crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)*ptr++) & 0x00FF];
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
|
@ -90,10 +62,11 @@ uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes) {
|
|||
*函数update_crc_16()根据
|
||||
*前一个循环冗余校验值和下一个要检查的数据字节。
|
||||
*/
|
||||
uint16_t update_crc_16(uint16_t crc, uint8_t c) {
|
||||
uint16_t update_crc_16(uint16_t crc, uint8_t c)
|
||||
{
|
||||
if (!crc_tab16_init)
|
||||
init_crc16_tab();
|
||||
return (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t) c) & 0x00FF];
|
||||
return (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)c) & 0x00FF];
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -104,15 +77,18 @@ uint16_t update_crc_16(uint16_t crc, uint8_t c) {
|
|||
*查找表首次由init_crc16_tab()例程计算
|
||||
*调用循环冗余校验函数。
|
||||
*/
|
||||
void init_crc16_tab(void) {
|
||||
void init_crc16_tab(void)
|
||||
{
|
||||
uint16_t i;
|
||||
uint16_t j;
|
||||
uint16_t crc;
|
||||
uint16_t c;
|
||||
for (i = 0; i < 256; i++) {
|
||||
for (i = 0; i < 256; i++)
|
||||
{
|
||||
crc = 0;
|
||||
c = i;
|
||||
for (j = 0; j < 8; j++) {
|
||||
for (j = 0; j < 8; j++)
|
||||
{
|
||||
if ((crc ^ c) & 0x0001)
|
||||
crc = (crc >> 1) ^ CRC_POLY_16;
|
||||
else
|
||||
|
|
@ -123,12 +99,3 @@ void init_crc16_tab(void) {
|
|||
}
|
||||
crc_tab16_init = 1;
|
||||
}
|
||||
|
||||
uint32_t VerifyCRC16CheckSum(uint8_t *pchMessage, uint32_t dwLength) {
|
||||
uint16_t wExpected = 0;
|
||||
if ((pchMessage == NULL) || (dwLength <= 2)) {
|
||||
return 0;
|
||||
}
|
||||
wExpected = crc_16(pchMessage, dwLength - 2);
|
||||
return ((wExpected & 0xff) == pchMessage[dwLength - 2] && ((wExpected >> 8) & 0xff) == pchMessage[dwLength - 1]);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -10,5 +10,5 @@ uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes);
|
|||
uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes);
|
||||
uint16_t update_crc_16(uint16_t crc, uint8_t c);
|
||||
void init_crc16_tab(void);
|
||||
uint32_t VerifyCRC16CheckSum(uint8_t *pchMessage, uint32_t dwLength);
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -212,33 +212,3 @@ void MatInit(mat *m, uint8_t row, uint8_t col)
|
|||
m->numRows = row;
|
||||
m->pData = (float *)zmalloc(row * col * sizeof(float));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 一阶低通滤波初始化
|
||||
* @author RM
|
||||
* @param[in] 一阶低通滤波结构体
|
||||
* @param[in] 间隔的时间，单位 s
|
||||
* @param[in] 滤波参数
|
||||
* @retval 返回空
|
||||
*/
|
||||
void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, float frame_period, const float num[1])
|
||||
{
|
||||
first_order_filter_type->frame_period = frame_period;
|
||||
first_order_filter_type->num[0] = num[0];
|
||||
first_order_filter_type->input = 0.0f;
|
||||
first_order_filter_type->out = 0.0f;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 一阶低通滤波计算
|
||||
* @author RM
|
||||
* @param[in] 一阶低通滤波结构体
|
||||
* @param[in] 间隔的时间，单位 s
|
||||
* @retval 返回空
|
||||
*/
|
||||
void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, float input)
|
||||
{
|
||||
first_order_filter_type->input = input;
|
||||
first_order_filter_type->out =
|
||||
first_order_filter_type->num[0] / (first_order_filter_type->num[0] + first_order_filter_type->frame_period) * first_order_filter_type->out + first_order_filter_type->frame_period / (first_order_filter_type->num[0] + first_order_filter_type->frame_period) * first_order_filter_type->input;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -53,13 +53,6 @@ void MatInit(mat *m, uint8_t row, uint8_t col);
|
|||
#ifndef PI
|
||||
#define PI 3.14159265354f
|
||||
#endif
|
||||
typedef __packed struct
|
||||
{
|
||||
float input; //输入数据
|
||||
float out; //滤波输出的数据
|
||||
float num[1]; //滤波参数
|
||||
float frame_period; //滤波的时间间隔 单位 s
|
||||
} first_order_filter_type_t;
|
||||
|
||||
#define VAL_LIMIT(val, min, max) \
|
||||
do \
|
||||
|
|
@ -127,10 +120,6 @@ void Cross3d(float *v1, float *v2, float *res);
|
|||
float Dot3d(float *v1, float *v2);
|
||||
|
||||
float AverageFilter(float new_data, float *buf, uint8_t len);
|
||||
//一阶低通滤波初始化
|
||||
void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, float frame_period, const float num[1]);
|
||||
//一阶低通滤波计算
|
||||
void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, float input);
|
||||
|
||||
#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
|
||||
|
||||
|
|
|
|||
|
|
@ -1,303 +0,0 @@
|
|||
//
|
||||
// Created by sph on 2024/1/21.
|
||||
//
|
||||
#include "auto_aim.h"
|
||||
#include "arm_math.h"
|
||||
|
||||
/**
|
||||
* @brief 选择目标装甲板
|
||||
* @author SJQ
|
||||
* @param[in] trajectory_cal:弹道解算结构体
|
||||
* @retval 返回空
|
||||
*/
|
||||
int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) {
|
||||
aim_sel->delay_time = trajectory_cal->fly_time + trajectory_cal->extra_delay_time;
|
||||
aim_sel->target_state.yaw += aim_sel->target_state.v_yaw * aim_sel->delay_time;
|
||||
|
||||
//计算四块装甲板的位置
|
||||
int use_1 = 1;
|
||||
int i = 0;
|
||||
int idx = 0; // 选择的装甲板
|
||||
// 为平衡步兵
|
||||
if (aim_sel->target_state.armor_num == 2) {
|
||||
for (i = 0; i < 2; i++) {
|
||||
float tmp_yaw = aim_sel->target_state.yaw + i * PI;
|
||||
float r = aim_sel->target_state.r1;
|
||||
|
||||
aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
|
||||
aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw);
|
||||
aim_sel->armor_pose[i].z = aim_sel->target_state.z;
|
||||
aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * PI;
|
||||
}
|
||||
|
||||
// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
|
||||
//
|
||||
// //因为是平衡步兵 只需判断两块装甲板即可
|
||||
// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[1].yaw);
|
||||
// if (temp_yaw_diff < yaw_diff_min) {
|
||||
// yaw_diff_min = temp_yaw_diff;
|
||||
// idx = 1;
|
||||
// }
|
||||
// 平衡步兵选择两块装甲板中较近的装甲板
|
||||
float distance_min = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2);
|
||||
float distance_temp = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2);
|
||||
if (distance_temp < distance_min) {
|
||||
distance_min = distance_temp;
|
||||
idx = 1;
|
||||
}
|
||||
|
||||
} else if (aim_sel->target_state.armor_num == 3)//前哨站
|
||||
{
|
||||
for (i = 0; i < 3; i++) {
|
||||
float tmp_yaw;
|
||||
if (aim_sel->target_state.v_yaw <= 0)
|
||||
tmp_yaw = aim_sel->target_state.yaw + i * (2.0 * PI / 3.0);
|
||||
else
|
||||
tmp_yaw = aim_sel->target_state.yaw - i * (2.0 * PI / 3.0);
|
||||
float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2;
|
||||
aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * cos(tmp_yaw);
|
||||
aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * sin(tmp_yaw);
|
||||
aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z +
|
||||
aim_sel->target_state.dz;
|
||||
aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * (2.0 * PI / 3.0);
|
||||
use_1 = !use_1;
|
||||
}
|
||||
|
||||
// //计算枪管到目标装甲板yaw最小的那个装甲板
|
||||
// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
|
||||
// for (i = 1; i < 3; i++) {
|
||||
// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
|
||||
// if (temp_yaw_diff < yaw_diff_min) {
|
||||
// yaw_diff_min = temp_yaw_diff;
|
||||
// idx = i;
|
||||
// }
|
||||
// }
|
||||
// 选择两块较近的装甲板，再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
|
||||
// float distance_temp0 = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2);
|
||||
// float distance_temp1 = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2);
|
||||
// float distance_temp2 = powf(aim_sel->armor_pose[2].x, 2) + powf(aim_sel->armor_pose[2].y, 2);
|
||||
|
||||
// int label_first = 0;
|
||||
// int label_second = 1;
|
||||
// if (distance_temp0 > distance_temp1) {
|
||||
// label_first = 1;
|
||||
// if (distance_temp0 > distance_temp2) {
|
||||
// label_second = 2;
|
||||
// } else label_second = 0;
|
||||
// } else {
|
||||
// label_first = 0;
|
||||
// if (distance_temp1 > distance_temp2) {
|
||||
// label_second = 2;
|
||||
// } else label_second = 1;
|
||||
// }
|
||||
|
||||
// 选择两块较近的装甲板
|
||||
float distance[3];
|
||||
for (i = 0; i < 3; i++) {
|
||||
distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
|
||||
}
|
||||
|
||||
int label_first = 0;
|
||||
int label_second = 0;
|
||||
|
||||
for (i = 1; i < 3; i++) {
|
||||
if (distance[i] < distance[label_first]) {
|
||||
label_second = label_first;
|
||||
label_first = i;
|
||||
} else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) {
|
||||
label_second = i;
|
||||
}else if (distance[i] < distance[label_second]) {
|
||||
label_second = i;
|
||||
}
|
||||
}
|
||||
|
||||
//再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
|
||||
float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 2));
|
||||
|
||||
float cos_theta_first = (aim_sel->armor_pose[label_first].x * aim_sel->target_state.x +
|
||||
aim_sel->armor_pose[label_first].y * aim_sel->target_state.y) /
|
||||
(sqrtf(powf(aim_sel->armor_pose[label_first].x, 2) +
|
||||
powf(aim_sel->armor_pose[label_first].y, 2)) * center_length);
|
||||
float cos_theta_second = (aim_sel->armor_pose[label_second].x * aim_sel->target_state.x +
|
||||
aim_sel->armor_pose[label_second].y * aim_sel->target_state.y) /
|
||||
(sqrtf(powf(aim_sel->armor_pose[label_second].x, 2) +
|
||||
powf(aim_sel->armor_pose[label_second].y, 2)) * center_length);
|
||||
|
||||
if (cos_theta_first > cos_theta_second)
|
||||
idx = label_first;
|
||||
else
|
||||
idx = label_second;
|
||||
|
||||
} else {
|
||||
for (i = 0; i < 4; i++) {
|
||||
float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0;
|
||||
float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2;
|
||||
|
||||
aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
|
||||
aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw);
|
||||
aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z +
|
||||
aim_sel->target_state.dz;
|
||||
aim_sel->armor_pose[i].yaw = tmp_yaw;
|
||||
use_1 = !use_1;
|
||||
}
|
||||
|
||||
// //计算枪管到目标装甲板yaw最小的那个装甲板
|
||||
// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
|
||||
// for (i = 1; i < 4; i++) {
|
||||
// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
|
||||
// if (temp_yaw_diff < yaw_diff_min) {
|
||||
// yaw_diff_min = temp_yaw_diff;
|
||||
// idx = i;
|
||||
// }
|
||||
// }
|
||||
|
||||
// 选择两块较近的装甲板
|
||||
float distance[4];
|
||||
for (i = 0; i < 4; i++) {
|
||||
distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
|
||||
}
|
||||
|
||||
int label_first = 0;
|
||||
int label_second = 1;
|
||||
|
||||
if(distance[label_first] > distance[label_second])
|
||||
{
|
||||
label_first = 1;
|
||||
label_second = 0;
|
||||
}
|
||||
|
||||
if(distance[2]<distance[label_first]){
|
||||
label_second = label_first;
|
||||
label_first = 2;
|
||||
}
|
||||
else if(distance[2]<distance[label_second])
|
||||
label_second = 2;
|
||||
|
||||
|
||||
if(distance[3]<distance[label_first]){
|
||||
label_second = label_first;
|
||||
label_first = 3;
|
||||
}
|
||||
else if(distance[3]<distance[label_second])
|
||||
label_second = 3;
|
||||
|
||||
//再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
|
||||
float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 2));
|
||||
|
||||
float cos_theta_first = (aim_sel->armor_pose[label_first].x * aim_sel->target_state.x +
|
||||
aim_sel->armor_pose[label_first].y * aim_sel->target_state.y) /
|
||||
(sqrtf(powf(aim_sel->armor_pose[label_first].x, 2) +
|
||||
powf(aim_sel->armor_pose[label_first].y, 2)) * center_length);
|
||||
float cos_theta_second = (aim_sel->armor_pose[label_second].x * aim_sel->target_state.x +
|
||||
aim_sel->armor_pose[label_second].y * aim_sel->target_state.y) /
|
||||
(sqrtf(powf(aim_sel->armor_pose[label_second].x, 2) +
|
||||
powf(aim_sel->armor_pose[label_second].y, 2)) * center_length);
|
||||
|
||||
if (cos_theta_first > cos_theta_second)
|
||||
idx = label_first;
|
||||
else
|
||||
idx = label_second;
|
||||
}
|
||||
|
||||
idx = 0;
|
||||
|
||||
aim_sel->aim_point[0] = aim_sel->armor_pose[idx].x + aim_sel->target_state.vx * aim_sel->delay_time;
|
||||
aim_sel->aim_point[1] = aim_sel->armor_pose[idx].y + aim_sel->target_state.vy * aim_sel->delay_time;
|
||||
aim_sel->aim_point[2] = aim_sel->armor_pose[idx].z + aim_sel->target_state.vz * aim_sel->delay_time;
|
||||
return idx;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 子弹飞行时间解算
|
||||
* @author SJQ
|
||||
* @param[in] x:瞄准时shooter_link下目标x坐标
|
||||
* @param[in] vx:瞄准时shooter_link下目标x速度
|
||||
* @param[in] v_x0:弹速水平分量
|
||||
* @retval 返回空
|
||||
*/
|
||||
const float k1 = 0.0761; //标准大气压25度下空气阻力系数
|
||||
float get_fly_time(float x, float vx, float v_x0) {
|
||||
float t = 0;
|
||||
float f_ti = 0, df_ti = 0;
|
||||
for (int i = 0; i < 5; i++) {
|
||||
f_ti = log(k1 * v_x0 * t + 1) / k1 - x - vx * t;
|
||||
df_ti = v_x0 / (k1 * v_x0 * t + 1) - vx;
|
||||
t = t - f_ti / df_ti;
|
||||
}
|
||||
return t;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 解算期望云台角度（考虑子弹飞行时间）
|
||||
* @author SJQ
|
||||
* @param[in] trajectory_cal:弹道解算结构体
|
||||
* @retval 返回空
|
||||
*/
|
||||
void get_cmd_angle(Trajectory_Type_t *trajectory_cal) {
|
||||
|
||||
arm_power_f32(trajectory_cal->position_xy, 2, &trajectory_cal->dis2);
|
||||
arm_sqrt_f32(trajectory_cal->dis2, &trajectory_cal->dis);
|
||||
|
||||
trajectory_cal->dis = trajectory_cal->dis - 0.20;
|
||||
|
||||
trajectory_cal->theta_0 = atan2f(trajectory_cal->z, trajectory_cal->dis);
|
||||
trajectory_cal->alpha = atan2f(trajectory_cal->position_xy[1], trajectory_cal->position_xy[0]);
|
||||
//将目标的xyz速度转化为平行枪管与垂直枪管的速度
|
||||
trajectory_cal->vx = trajectory_cal->velocity[0] * arm_cos_f32(trajectory_cal->alpha)
|
||||
+ trajectory_cal->velocity[1] * arm_sin_f32(trajectory_cal->alpha);
|
||||
trajectory_cal->vy = -trajectory_cal->velocity[0] * arm_sin_f32(trajectory_cal->alpha)
|
||||
+ trajectory_cal->velocity[1] * arm_cos_f32(trajectory_cal->alpha);
|
||||
|
||||
float v_x0 = trajectory_cal->v0 * arm_cos_f32(trajectory_cal->theta_0);//水平方向弹速
|
||||
float v_y0 = trajectory_cal->v0 * arm_sin_f32(trajectory_cal->theta_0);//竖直方向弹速
|
||||
|
||||
trajectory_cal->fly_time = get_fly_time(trajectory_cal->dis, trajectory_cal->vx, v_x0);
|
||||
arm_power_f32(&trajectory_cal->fly_time, 1, &trajectory_cal->fly_time2);
|
||||
trajectory_cal->h_r = trajectory_cal->z + trajectory_cal->velocity[2] * trajectory_cal->fly_time;
|
||||
//开始迭代
|
||||
trajectory_cal->theta_k = trajectory_cal->theta_0;
|
||||
trajectory_cal->k = 0;
|
||||
while (trajectory_cal->k < 10) {
|
||||
v_y0 = trajectory_cal->v0 * arm_sin_f32(trajectory_cal->theta_k);//竖直方向弹速
|
||||
trajectory_cal->h_k = v_y0 * trajectory_cal->fly_time - 0.5 * 9.8 * trajectory_cal->fly_time2;
|
||||
trajectory_cal->err_k = trajectory_cal->h_r - trajectory_cal->h_k;
|
||||
trajectory_cal->theta_k += 0.1 * trajectory_cal->err_k;
|
||||
trajectory_cal->k++;
|
||||
if (trajectory_cal->err_k < 0.005) break;
|
||||
}
|
||||
|
||||
trajectory_cal->cmd_yaw = atan2f(trajectory_cal->position_xy[1],
|
||||
trajectory_cal->position_xy[0]);
|
||||
|
||||
trajectory_cal->cmd_pitch = trajectory_cal->theta_k;
|
||||
}
|
||||
|
||||
int auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, RecievePacket_t *receive_packet) {
|
||||
trajectory_cal->extra_delay_time = 0.035;//0.025
|
||||
|
||||
aim_sel->target_state.armor_type = receive_packet->id;
|
||||
aim_sel->target_state.armor_num = receive_packet->armors_num;
|
||||
aim_sel->target_state.x = receive_packet->x;
|
||||
aim_sel->target_state.y = receive_packet->y;
|
||||
aim_sel->target_state.z = receive_packet->z;
|
||||
aim_sel->target_state.vx = receive_packet->vx;
|
||||
aim_sel->target_state.vy = receive_packet->vy;
|
||||
aim_sel->target_state.vz = receive_packet->vz;
|
||||
aim_sel->target_state.yaw = receive_packet->yaw;
|
||||
aim_sel->target_state.v_yaw = receive_packet->v_yaw;
|
||||
aim_sel->target_state.r1 = receive_packet->r1;
|
||||
aim_sel->target_state.r2 = receive_packet->r2;
|
||||
aim_sel->target_state.dz = receive_packet->dz;
|
||||
|
||||
int idx = aim_armor_select(aim_sel, trajectory_cal);
|
||||
|
||||
trajectory_cal->position_xy[0] = aim_sel->aim_point[0];
|
||||
trajectory_cal->position_xy[1] = aim_sel->aim_point[1];
|
||||
trajectory_cal->z = aim_sel->aim_point[2];
|
||||
trajectory_cal->velocity[0] = aim_sel->target_state.vx;
|
||||
trajectory_cal->velocity[1] = aim_sel->target_state.vy;
|
||||
trajectory_cal->velocity[2] = aim_sel->target_state.vz;
|
||||
|
||||
get_cmd_angle(trajectory_cal);
|
||||
return idx;
|
||||
}
|
||||
|
|
@ -1,77 +0,0 @@
|
|||
//
|
||||
// Created by sph on 2024/1/21.
|
||||
//
|
||||
|
||||
#ifndef BASIC_FRAMEWORK_AUTO_AIM_H
|
||||
#define BASIC_FRAMEWORK_AUTO_AIM_H
|
||||
|
||||
#include "master_process.h"
|
||||
//弹道解算
|
||||
typedef struct
|
||||
{
|
||||
float v0; //子弹射速
|
||||
float velocity[3];//目标xyz速度
|
||||
float vx; //目标相对枪口方向的速度
|
||||
float vy;
|
||||
float alpha; //目标初始航向角
|
||||
float position_xy[2];//目标xy坐标
|
||||
float z; //目标z坐标
|
||||
float fly_time; //子弹飞行时间
|
||||
float fly_time2; //子弹飞行时间平方
|
||||
float extra_delay_time ;
|
||||
float theta_0; //初始目标角度
|
||||
float theta_k; //迭代目标角度
|
||||
float dis; //目标距离
|
||||
float dis2; //目标距离平方
|
||||
float err_k; //迭代误差
|
||||
uint8_t k; //迭代次数
|
||||
float h_k; //迭代高度
|
||||
float h_r; //目标真实高度
|
||||
|
||||
float cmd_yaw;
|
||||
float cmd_pitch;
|
||||
} Trajectory_Type_t;
|
||||
|
||||
//整车状态
|
||||
typedef struct
|
||||
{
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
float yaw;
|
||||
float vx;
|
||||
float vy;
|
||||
float vz;
|
||||
float v_yaw;
|
||||
float r1;
|
||||
float r2;
|
||||
float dz;
|
||||
uint8_t armor_type;
|
||||
uint8_t armor_num;
|
||||
}Target_State_Type_t;
|
||||
|
||||
//预瞄点
|
||||
typedef struct
|
||||
{
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
float yaw;
|
||||
}Armor_Pose_Type_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
Target_State_Type_t target_state; //整车状态
|
||||
Armor_Pose_Type_t armor_pose[4]; //四个装甲板状态
|
||||
float aim_point[3]; //预瞄点
|
||||
float delay_time; //预瞄时间差
|
||||
uint8_t suggest_fire;
|
||||
}Aim_Select_Type_t;
|
||||
|
||||
|
||||
int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal);
|
||||
float get_fly_time(float x, float vx, float v_x0);
|
||||
void get_cmd_angle(Trajectory_Type_t *trajectory_cal);
|
||||
int auto_aim(Aim_Select_Type_t *aim_sel,Trajectory_Type_t *trajectory_cal,RecievePacket_t *receive_packet);
|
||||
|
||||
#endif //BASIC_FRAMEWORK_AUTO_AIM_H
|
||||
|
|
@ -19,7 +19,6 @@
|
|||
#include "user_lib.h"
|
||||
#include "general_def.h"
|
||||
#include "master_process.h"
|
||||
#include "crc16.h"
|
||||
|
||||
static INS_t INS;
|
||||
static IMU_Param_t IMU_Param;
|
||||
|
|
@ -36,26 +35,30 @@ static float RefTemp = 40; // 恒温设定温度
|
|||
|
||||
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
|
||||
|
||||
static void IMUPWMSet(uint16_t pwm) {
|
||||
__HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm);
|
||||
static void IMUPWMSet(uint16_t pwm)
|
||||
{
|
||||
__HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 温度控制
|
||||
*
|
||||
*/
|
||||
static void IMU_Temperature_Ctrl(void) {
|
||||
static void IMU_Temperature_Ctrl(void)
|
||||
{
|
||||
PIDCalculate(&TempCtrl, BMI088.Temperature, RefTemp);
|
||||
IMUPWMSet(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX));
|
||||
}
|
||||
|
||||
// 使用加速度计的数据初始化Roll和Pitch,而Yaw置0,这样可以避免在初始时候的姿态估计误差
|
||||
static void InitQuaternion(float *init_q4) {
|
||||
static void InitQuaternion(float *init_q4)
|
||||
{
|
||||
float acc_init[3] = {0};
|
||||
float gravity_norm[3] = {0, 0, 1}; // 导航系重力加速度矢量,归一化后为(0,0,1)
|
||||
float axis_rot[3] = {0}; // 旋转轴
|
||||
// 读取100次加速度计数据,取平均值作为初始值
|
||||
for (uint8_t i = 0; i < 100; ++i) {
|
||||
for (uint8_t i = 0; i < 100; ++i)
|
||||
{
|
||||
BMI088_Read(&BMI088);
|
||||
acc_init[X] += BMI088.Accel[X];
|
||||
acc_init[Y] += BMI088.Accel[Y];
|
||||
|
|
@ -74,15 +77,17 @@ static void InitQuaternion(float *init_q4) {
|
|||
init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
|
||||
}
|
||||
|
||||
attitude_t *INS_Init(void) {
|
||||
attitude_t *INS_Init(void)
|
||||
{
|
||||
if (!INS.init)
|
||||
INS.init = 1;
|
||||
else
|
||||
return (attitude_t *) &INS.Gyro;
|
||||
return (attitude_t *)&INS.Gyro;
|
||||
|
||||
HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1);
|
||||
|
||||
while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR);
|
||||
while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR)
|
||||
;
|
||||
IMU_Param.scale[X] = 1;
|
||||
IMU_Param.scale[Y] = 1;
|
||||
IMU_Param.scale[Z] = 1;
|
||||
|
|
@ -96,22 +101,23 @@ attitude_t *INS_Init(void) {
|
|||
IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0);
|
||||
// imu heat init
|
||||
PID_Init_Config_s config = {.MaxOut = 2000,
|
||||
.IntegralLimit = 300,
|
||||
.DeadBand = 0,
|
||||
.Kp = 1000,
|
||||
.Ki = 20,
|
||||
.Kd = 0,
|
||||
.Improve = 0x01}; // enable integratiaon limit
|
||||
.IntegralLimit = 300,
|
||||
.DeadBand = 0,
|
||||
.Kp = 1000,
|
||||
.Ki = 20,
|
||||
.Kd = 0,
|
||||
.Improve = 0x01}; // enable integratiaon limit
|
||||
PIDInit(&TempCtrl, &config);
|
||||
|
||||
// noise of accel is relatively big and of high freq,thus lpf is used
|
||||
INS.AccelLPF = 0.0085;
|
||||
DWT_GetDeltaT(&INS_DWT_Count);
|
||||
return (attitude_t *) &INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
|
||||
return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
|
||||
}
|
||||
|
||||
/* 注意以1kHz的频率运行此任务 */
|
||||
void INS_Task(void) {
|
||||
void INS_Task(void)
|
||||
{
|
||||
static uint32_t count = 0;
|
||||
const float gravity[3] = {0, 0, 9.81f};
|
||||
|
||||
|
|
@ -119,7 +125,8 @@ void INS_Task(void) {
|
|||
t += dt;
|
||||
|
||||
// ins update
|
||||
if ((count % 1) == 0) {
|
||||
if ((count % 1) == 0)
|
||||
{
|
||||
BMI088_Read(&BMI088);
|
||||
|
||||
INS.Accel[X] = BMI088.Accel[X];
|
||||
|
|
@ -151,8 +158,7 @@ void INS_Task(void) {
|
|||
EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
|
||||
for (uint8_t i = 0; i < 3; ++i) // 同样过一个低通滤波
|
||||
{
|
||||
INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) +
|
||||
INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
|
||||
INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
|
||||
}
|
||||
BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
|
||||
|
||||
|
|
@ -161,17 +167,18 @@ void INS_Task(void) {
|
|||
INS.Roll = QEKF_INS.Roll;
|
||||
INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
|
||||
|
||||
//VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
|
||||
VisionSetAltitude(INS.Yaw * PI / 180, -INS.Roll * PI / 180);
|
||||
VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
|
||||
}
|
||||
|
||||
// temperature control
|
||||
if ((count % 2) == 0) {
|
||||
if ((count % 2) == 0)
|
||||
{
|
||||
// 500hz
|
||||
IMU_Temperature_Ctrl();
|
||||
}
|
||||
|
||||
if ((count++ % 1000) == 0) {
|
||||
if ((count++ % 1000) == 0)
|
||||
{
|
||||
// 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
|
||||
}
|
||||
}
|
||||
|
|
@ -182,7 +189,8 @@ void INS_Task(void) {
|
|||
* @param[2] vector in EarthFrame
|
||||
* @param[3] quaternion
|
||||
*/
|
||||
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q) {
|
||||
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q)
|
||||
{
|
||||
vecEF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecBF[0] +
|
||||
(q[1] * q[2] - q[0] * q[3]) * vecBF[1] +
|
||||
(q[1] * q[3] + q[0] * q[2]) * vecBF[2]);
|
||||
|
|
@ -202,7 +210,8 @@ void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q) {
|
|||
* @param[2] vector in BodyFrame
|
||||
* @param[3] quaternion
|
||||
*/
|
||||
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q) {
|
||||
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q)
|
||||
{
|
||||
vecBF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecEF[0] +
|
||||
(q[1] * q[2] + q[0] * q[3]) * vecEF[1] +
|
||||
(q[1] * q[3] - q[0] * q[2]) * vecEF[2]);
|
||||
|
|
@ -224,14 +233,16 @@ void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q) {
|
|||
* @param gyro 角速度
|
||||
* @param accel 加速度
|
||||
*/
|
||||
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]) {
|
||||
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3])
|
||||
{
|
||||
static float lastYawOffset, lastPitchOffset, lastRollOffset;
|
||||
static float c_11, c_12, c_13, c_21, c_22, c_23, c_31, c_32, c_33;
|
||||
float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
|
||||
|
||||
if (fabsf(param->Yaw - lastYawOffset) > 0.001f ||
|
||||
fabsf(param->Pitch - lastPitchOffset) > 0.001f ||
|
||||
fabsf(param->Roll - lastRollOffset) > 0.001f || param->flag) {
|
||||
fabsf(param->Roll - lastRollOffset) > 0.001f || param->flag)
|
||||
{
|
||||
cosYaw = arm_cos_f32(param->Yaw / 57.295779513f);
|
||||
cosPitch = arm_cos_f32(param->Pitch / 57.295779513f);
|
||||
cosRoll = arm_cos_f32(param->Roll / 57.295779513f);
|
||||
|
|
@ -291,7 +302,8 @@ static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[
|
|||
/**
|
||||
* @brief Update quaternion
|
||||
*/
|
||||
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt) {
|
||||
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt)
|
||||
{
|
||||
float qa, qb, qc;
|
||||
|
||||
gx *= 0.5f * dt;
|
||||
|
|
@ -309,7 +321,8 @@ void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt) {
|
|||
/**
|
||||
* @brief Convert quaternion to eular angle
|
||||
*/
|
||||
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll) {
|
||||
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll)
|
||||
{
|
||||
*Yaw = atan2f(2.0f * (q[0] * q[3] + q[1] * q[2]), 2.0f * (q[0] * q[0] + q[1] * q[1]) - 1.0f) * 57.295779513f;
|
||||
*Pitch = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), 2.0f * (q[0] * q[0] + q[3] * q[3]) - 1.0f) * 57.295779513f;
|
||||
*Roll = asinf(2.0f * (q[0] * q[2] - q[1] * q[3])) * 57.295779513f;
|
||||
|
|
@ -318,7 +331,8 @@ void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll) {
|
|||
/**
|
||||
* @brief Convert eular angle to quaternion
|
||||
*/
|
||||
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q) {
|
||||
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q)
|
||||
{
|
||||
float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
|
||||
Yaw /= 57.295779513f;
|
||||
Pitch /= 57.295779513f;
|
||||
|
|
|
|||
|
|
@ -13,42 +13,25 @@
|
|||
#include "daemon.h"
|
||||
#include "bsp_log.h"
|
||||
#include "robot_def.h"
|
||||
#include "crc16.h"
|
||||
|
||||
static RecievePacket_t recv_data;
|
||||
static SendPacket_t send_data;
|
||||
static Vision_Recv_s recv_data;
|
||||
static Vision_Send_s send_data;
|
||||
static DaemonInstance *vision_daemon_instance;
|
||||
|
||||
//void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed)
|
||||
//{
|
||||
// send_data.enemy_color = enemy_color;
|
||||
// send_data.work_mode = work_mode;
|
||||
// send_data.bullet_speed = bullet_speed;
|
||||
//}
|
||||
void VisionSetFlag(Enemy_Color_e enemy_color)
|
||||
void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed)
|
||||
{
|
||||
send_data.detect_color = enemy_color;
|
||||
send_data.reserved = 0;
|
||||
send_data.enemy_color = enemy_color;
|
||||
send_data.work_mode = work_mode;
|
||||
send_data.bullet_speed = bullet_speed;
|
||||
}
|
||||
|
||||
//void VisionSetAltitude(float yaw, float pitch)
|
||||
//{
|
||||
// send_data.yaw = yaw;
|
||||
// send_data.pitch = pitch;
|
||||
//}
|
||||
void VisionSetAltitude(float yaw, float pitch)
|
||||
void VisionSetAltitude(float yaw, float pitch, float roll)
|
||||
{
|
||||
send_data.yaw = yaw;
|
||||
send_data.pitch = pitch;
|
||||
send_data.roll = roll;
|
||||
}
|
||||
|
||||
void VisionSetAim(float aim_x, float aim_y, float aim_z) {
|
||||
send_data.aim_x = aim_x;
|
||||
send_data.aim_y = aim_y;
|
||||
send_data.aim_z = aim_z;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 离线回调函数,将在daemon.c中被daemon task调用
|
||||
* @attention 由于HAL库的设计问题,串口开启DMA接收之后同时发送有概率出现__HAL_LOCK()导致的死锁,使得无法
|
||||
|
|
@ -130,24 +113,17 @@ void VisionSend()
|
|||
#ifdef VISION_USE_VCP
|
||||
|
||||
#include "bsp_usb.h"
|
||||
static uint8_t *vis_recv_buff; //接收到的原始数据
|
||||
static uint8_t *vis_recv_buff;
|
||||
|
||||
static void DecodeVision(uint16_t recv_len)
|
||||
{
|
||||
// uint16_t flag_register;
|
||||
// get_protocol_info(vis_recv_buff, &flag_register, (uint8_t *)&recv_data.pitch);
|
||||
// // TODO: code to resolve flag_register;
|
||||
if(vis_recv_buff[0]==0xA5)
|
||||
{
|
||||
if(VerifyCRC16CheckSum(vis_recv_buff,sizeof(recv_data)))
|
||||
{
|
||||
memcpy(&recv_data,vis_recv_buff,sizeof(recv_data));
|
||||
}
|
||||
}
|
||||
uint16_t flag_register;
|
||||
get_protocol_info(vis_recv_buff, &flag_register, (uint8_t *)&recv_data.pitch);
|
||||
// TODO: code to resolve flag_register;
|
||||
}
|
||||
|
||||
/* 视觉通信初始化 */
|
||||
RecievePacket_t *VisionInit(UART_HandleTypeDef *_handle)
|
||||
Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
|
||||
{
|
||||
UNUSED(_handle); // 仅为了消除警告
|
||||
USB_Init_Config_s conf = {.rx_cbk = DecodeVision};
|
||||
|
|
@ -166,23 +142,14 @@ RecievePacket_t *VisionInit(UART_HandleTypeDef *_handle)
|
|||
|
||||
void VisionSend()
|
||||
{
|
||||
// static uint16_t flag_register;
|
||||
// static uint8_t send_buff[VISION_SEND_SIZE];
|
||||
// static uint16_t tx_len;
|
||||
// // TODO: code to set flag_register
|
||||
// flag_register = 30 << 8 | 0b00000001;
|
||||
// // 将数据转化为seasky协议的数据包
|
||||
// get_protocol_send_data(0x02, flag_register, &send_data.yaw, 3, send_buff, &tx_len);
|
||||
// USBTransmit(send_buff, tx_len);
|
||||
static uint8_t send_buffer[24]={0};
|
||||
|
||||
send_data.header = 0x5A;
|
||||
// VisionSetFlag(data->detect_color);
|
||||
// VisionSetAim(data->aim_x,data->aim_y,data->aim_z);
|
||||
send_data.checksum = crc_16(&send_data.header,sizeof(send_data)-2);
|
||||
|
||||
memcpy(send_buffer,&send_data,sizeof(send_data));
|
||||
USBTransmit(send_buffer, sizeof(send_data));
|
||||
static uint16_t flag_register;
|
||||
static uint8_t send_buff[VISION_SEND_SIZE];
|
||||
static uint16_t tx_len;
|
||||
// TODO: code to set flag_register
|
||||
flag_register = 30 << 8 | 0b00000001;
|
||||
// 将数据转化为seasky协议的数据包
|
||||
get_protocol_send_data(0x02, flag_register, &send_data.yaw, 3, send_buff, &tx_len);
|
||||
USBTransmit(send_buff, tx_len);
|
||||
}
|
||||
|
||||
#endif // VISION_USE_VCP
|
||||
|
|
|
|||
|
|
@ -8,103 +8,77 @@
|
|||
#define VISION_SEND_SIZE 36u
|
||||
|
||||
#pragma pack(1)
|
||||
typedef enum {
|
||||
NO_FIRE = 0,
|
||||
AUTO_FIRE = 1,
|
||||
AUTO_AIM = 2
|
||||
typedef enum
|
||||
{
|
||||
NO_FIRE = 0,
|
||||
AUTO_FIRE = 1,
|
||||
AUTO_AIM = 2
|
||||
} Fire_Mode_e;
|
||||
|
||||
typedef enum {
|
||||
NO_TARGET = 0,
|
||||
TARGET_CONVERGING = 1,
|
||||
READY_TO_FIRE = 2
|
||||
typedef enum
|
||||
{
|
||||
NO_TARGET = 0,
|
||||
TARGET_CONVERGING = 1,
|
||||
READY_TO_FIRE = 2
|
||||
} Target_State_e;
|
||||
|
||||
typedef enum {
|
||||
NO_TARGET_NUM = 0,
|
||||
HERO1 = 1,
|
||||
ENGINEER2 = 2,
|
||||
INFANTRY3 = 3,
|
||||
INFANTRY4 = 4,
|
||||
INFANTRY5 = 5,
|
||||
OUTPOST = 6,
|
||||
SENTRY = 7,
|
||||
BASE = 8
|
||||
typedef enum
|
||||
{
|
||||
NO_TARGET_NUM = 0,
|
||||
HERO1 = 1,
|
||||
ENGINEER2 = 2,
|
||||
INFANTRY3 = 3,
|
||||
INFANTRY4 = 4,
|
||||
INFANTRY5 = 5,
|
||||
OUTPOST = 6,
|
||||
SENTRY = 7,
|
||||
BASE = 8
|
||||
} Target_Type_e;
|
||||
|
||||
typedef struct {
|
||||
Fire_Mode_e fire_mode;
|
||||
Target_State_e target_state;
|
||||
Target_Type_e target_type;
|
||||
typedef struct
|
||||
{
|
||||
Fire_Mode_e fire_mode;
|
||||
Target_State_e target_state;
|
||||
Target_Type_e target_type;
|
||||
|
||||
float pitch;
|
||||
float yaw;
|
||||
float pitch;
|
||||
float yaw;
|
||||
} Vision_Recv_s;
|
||||
|
||||
typedef enum {
|
||||
// COLOR_NONE = 0,
|
||||
// COLOR_BLUE = 1,
|
||||
// COLOR_RED = 2,
|
||||
ENEMY_COLOR_RED = 0,
|
||||
ENEMY_COLOR_BLUE = 1,
|
||||
typedef enum
|
||||
{
|
||||
COLOR_NONE = 0,
|
||||
COLOR_BLUE = 1,
|
||||
COLOR_RED = 2,
|
||||
} Enemy_Color_e;
|
||||
|
||||
typedef enum {
|
||||
VISION_MODE_AIM = 0,
|
||||
VISION_MODE_SMALL_BUFF = 1,
|
||||
VISION_MODE_BIG_BUFF = 2
|
||||
typedef enum
|
||||
{
|
||||
VISION_MODE_AIM = 0,
|
||||
VISION_MODE_SMALL_BUFF = 1,
|
||||
VISION_MODE_BIG_BUFF = 2
|
||||
} Work_Mode_e;
|
||||
|
||||
typedef enum {
|
||||
BULLET_SPEED_NONE = 0,
|
||||
BIG_AMU_10 = 10,
|
||||
SMALL_AMU_15 = 15,
|
||||
BIG_AMU_16 = 16,
|
||||
SMALL_AMU_18 = 18,
|
||||
SMALL_AMU_30 = 30,
|
||||
typedef enum
|
||||
{
|
||||
BULLET_SPEED_NONE = 0,
|
||||
BIG_AMU_10 = 10,
|
||||
SMALL_AMU_15 = 15,
|
||||
BIG_AMU_16 = 16,
|
||||
SMALL_AMU_18 = 18,
|
||||
SMALL_AMU_30 = 30,
|
||||
} Bullet_Speed_e;
|
||||
|
||||
typedef struct {
|
||||
Enemy_Color_e enemy_color;
|
||||
Work_Mode_e work_mode;
|
||||
Bullet_Speed_e bullet_speed;
|
||||
typedef struct
|
||||
{
|
||||
Enemy_Color_e enemy_color;
|
||||
Work_Mode_e work_mode;
|
||||
Bullet_Speed_e bullet_speed;
|
||||
|
||||
float yaw;
|
||||
float pitch;
|
||||
float roll;
|
||||
float yaw;
|
||||
float pitch;
|
||||
float roll;
|
||||
} Vision_Send_s;
|
||||
|
||||
typedef __packed struct {
|
||||
uint8_t header;//0x5A
|
||||
uint8_t detect_color: 1;
|
||||
uint8_t reserved: 7;
|
||||
float pitch;
|
||||
float yaw;
|
||||
float aim_x;
|
||||
float aim_y;
|
||||
float aim_z;
|
||||
uint16_t checksum;
|
||||
} SendPacket_t;
|
||||
|
||||
typedef __packed struct {
|
||||
uint8_t header; //= 0xA5;
|
||||
uint8_t tracking: 1;
|
||||
uint8_t id: 3; // 0-outpost 6-guard 7-base
|
||||
uint8_t armors_num: 3; // 2-balance 3-outpost 4-normal
|
||||
uint8_t reserved: 1;
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
float yaw;
|
||||
float vx;
|
||||
float vy;
|
||||
float vz;
|
||||
float v_yaw;
|
||||
float r1;
|
||||
float r2;
|
||||
float dz;
|
||||
uint16_t checksum;
|
||||
} RecievePacket_t;
|
||||
#pragma pack()
|
||||
|
||||
/**
|
||||
|
|
@ -112,8 +86,7 @@ typedef __packed struct {
|
|||
*
|
||||
* @param handle 用于和视觉通信的串口handle(C板上一般为USART1,丝印为USART2,4pin)
|
||||
*/
|
||||
//Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle);
|
||||
RecievePacket_t *VisionInit(UART_HandleTypeDef *_handle);
|
||||
Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle);
|
||||
|
||||
/**
|
||||
* @brief 发送视觉数据
|
||||
|
|
@ -128,8 +101,7 @@ void VisionSend();
|
|||
* @param work_mode
|
||||
* @param bullet_speed
|
||||
*/
|
||||
//void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed);
|
||||
void VisionSetFlag(Enemy_Color_e enemy_color);
|
||||
void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed);
|
||||
|
||||
/**
|
||||
* @brief 设置发送数据的姿态部分
|
||||
|
|
@ -137,8 +109,6 @@ void VisionSetFlag(Enemy_Color_e enemy_color);
|
|||
* @param yaw
|
||||
* @param pitch
|
||||
*/
|
||||
//void VisionSetAltitude(float yaw, float pitch, float roll);
|
||||
void VisionSetAltitude(float yaw, float pitch);
|
||||
void VisionSetAltitude(float yaw, float pitch, float roll);
|
||||
|
||||
void VisionSetAim(float aim_x, float aim_y,float aim_z);
|
||||
#endif // !MASTER_PROCESS_H
|
||||
|
|
@ -2,7 +2,6 @@
|
|||
#include "general_def.h"
|
||||
#include "bsp_dwt.h"
|
||||
#include "bsp_log.h"
|
||||
#include "user_lib.h"
|
||||
|
||||
static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
|
||||
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
|
||||
|
|
@ -20,27 +19,13 @@ static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在co
|
|||
* can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF
|
||||
* can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF
|
||||
*/
|
||||
static CANInstance sender_assignment[10] = {
|
||||
[0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
|
||||
0}},
|
||||
[1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
|
||||
0}},
|
||||
[2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
|
||||
0}},
|
||||
[3] = {.can_handle = &hcan2, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
|
||||
0}},
|
||||
[4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
|
||||
0}},
|
||||
[5] = {.can_handle = &hcan2, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
|
||||
0}},
|
||||
[6] = {.can_handle = &hcan1, .txconf.StdId = 0x1fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
|
||||
0}},
|
||||
[7] = {.can_handle = &hcan1, .txconf.StdId = 0x2fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
|
||||
0}},
|
||||
[8] = {.can_handle = &hcan2, .txconf.StdId = 0x1fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
|
||||
0}},
|
||||
[9] = {.can_handle = &hcan2, .txconf.StdId = 0x2fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
|
||||
0}}
|
||||
static CANInstance sender_assignment[6] = {
|
||||
[0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
|
||||
[1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
|
||||
[2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
|
||||
[3] = {.can_handle = &hcan2, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
|
||||
[4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
|
||||
[5] = {.can_handle = &hcan2, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -53,88 +38,78 @@ static uint8_t sender_enable_flag[6] = {0};
|
|||
* @brief 根据电调/拨码开关上的ID,根据说明书的默认id分配方式计算发送ID和接收ID,
|
||||
* 并对电机进行分组以便处理多电机控制命令
|
||||
*/
|
||||
static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *config) {
|
||||
static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *config)
|
||||
{
|
||||
uint8_t motor_id = config->tx_id - 1; // 下标从零开始,先减一方便赋值
|
||||
uint8_t motor_send_num;
|
||||
uint8_t motor_grouping;
|
||||
|
||||
switch (motor->motor_type) {
|
||||
case M2006:
|
||||
case M3508:
|
||||
if (motor_id < 4) // 根据ID分组
|
||||
switch (motor->motor_type)
|
||||
{
|
||||
case M2006:
|
||||
case M3508:
|
||||
if (motor_id < 4) // 根据ID分组
|
||||
{
|
||||
motor_send_num = motor_id;
|
||||
motor_grouping = config->can_handle == &hcan1 ? 1 : 4;
|
||||
}
|
||||
else
|
||||
{
|
||||
motor_send_num = motor_id - 4;
|
||||
motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
|
||||
}
|
||||
|
||||
// 计算接收id并设置分组发送id
|
||||
config->rx_id = 0x200 + motor_id + 1; // 把ID+1,进行分组设置
|
||||
sender_enable_flag[motor_grouping] = 1; // 设置发送标志位,防止发送空帧
|
||||
motor->message_num = motor_send_num;
|
||||
motor->sender_group = motor_grouping;
|
||||
|
||||
// 检查是否发生id冲突
|
||||
for (size_t i = 0; i < idx; ++i)
|
||||
{
|
||||
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
|
||||
{
|
||||
motor_send_num = motor_id;
|
||||
motor_grouping = config->can_handle == &hcan1 ? 1 : 4;
|
||||
} else {
|
||||
motor_send_num = motor_id - 4;
|
||||
motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
|
||||
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
|
||||
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
|
||||
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
|
||||
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
// 计算接收id并设置分组发送id
|
||||
config->rx_id = 0x200 + motor_id + 1; // 把ID+1,进行分组设置
|
||||
sender_enable_flag[motor_grouping] = 1; // 设置发送标志位,防止发送空帧
|
||||
motor->message_num = motor_send_num;
|
||||
motor->sender_group = motor_grouping;
|
||||
case GM6020:
|
||||
if (motor_id < 4)
|
||||
{
|
||||
motor_send_num = motor_id;
|
||||
motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
|
||||
}
|
||||
else
|
||||
{
|
||||
motor_send_num = motor_id - 4;
|
||||
motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
|
||||
}
|
||||
|
||||
// 检查是否发生id冲突
|
||||
for (size_t i = 0; i < idx; ++i) {
|
||||
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle &&
|
||||
dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) {
|
||||
LOGERROR(
|
||||
"[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
|
||||
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
|
||||
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
|
||||
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
|
||||
}
|
||||
}
|
||||
break;
|
||||
config->rx_id = 0x204 + motor_id + 1; // 把ID+1,进行分组设置
|
||||
sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
|
||||
motor->message_num = motor_send_num;
|
||||
motor->sender_group = motor_grouping;
|
||||
|
||||
case GM6020:
|
||||
if(motor->motor_control_type == CURRENT_CONTROL) //6020电流控制帧id 0x1fe 0x2fe
|
||||
for (size_t i = 0; i < idx; ++i)
|
||||
{
|
||||
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
|
||||
{
|
||||
if (motor_id < 4)
|
||||
{
|
||||
motor_send_num = motor_id;
|
||||
motor_grouping = config->can_handle == &hcan1 ? 6 : 8;
|
||||
}
|
||||
else
|
||||
{
|
||||
motor_send_num = motor_id - 4;
|
||||
motor_grouping = config->can_handle == &hcan1 ? 7 : 9;
|
||||
}
|
||||
}else{
|
||||
if (motor_id < 4)
|
||||
{
|
||||
motor_send_num = motor_id;
|
||||
motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
|
||||
}
|
||||
else
|
||||
{
|
||||
motor_send_num = motor_id - 4;
|
||||
motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
|
||||
}
|
||||
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
|
||||
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
|
||||
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
|
||||
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
config->rx_id = 0x204 + motor_id + 1; // 把ID+1,进行分组设置
|
||||
sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
|
||||
motor->message_num = motor_send_num;
|
||||
motor->sender_group = motor_grouping;
|
||||
|
||||
for (size_t i = 0; i < idx; ++i) {
|
||||
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle &&
|
||||
dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) {
|
||||
LOGERROR(
|
||||
"[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
|
||||
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
|
||||
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
|
||||
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
default: // other motors should not be registered here
|
||||
while (1)
|
||||
LOGERROR("[dji_motor]You must not register other motors using the API of DJI motor."); // 其他电机不应该在这里注册
|
||||
default: // other motors should not be registered here
|
||||
while (1)
|
||||
LOGERROR("[dji_motor]You must not register other motors using the API of DJI motor."); // 其他电机不应该在这里注册
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -144,11 +119,12 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
|
|||
*
|
||||
* @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例
|
||||
*/
|
||||
static void DecodeDJIMotor(CANInstance *_instance) {
|
||||
static void DecodeDJIMotor(CANInstance *_instance)
|
||||
{
|
||||
// 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
|
||||
// _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针
|
||||
uint8_t *rxbuff = _instance->rx_buff;
|
||||
DJIMotorInstance *motor = (DJIMotorInstance *) _instance->id;
|
||||
DJIMotorInstance *motor = (DJIMotorInstance *)_instance->id;
|
||||
DJI_Motor_Measure_s *measure = &motor->measure; // measure要多次使用,保存指针减小访存开销
|
||||
|
||||
DaemonReload(motor->daemon);
|
||||
|
|
@ -156,12 +132,12 @@ static void DecodeDJIMotor(CANInstance *_instance) {
|
|||
|
||||
// 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册
|
||||
measure->last_ecd = measure->ecd;
|
||||
measure->ecd = ((uint16_t) rxbuff[0]) << 8 | rxbuff[1];
|
||||
measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float) measure->ecd;
|
||||
measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1];
|
||||
measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float)measure->ecd;
|
||||
measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps +
|
||||
RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float) ((int16_t) (rxbuff[2] << 8 | rxbuff[3]));
|
||||
RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float)((int16_t)(rxbuff[2] << 8 | rxbuff[3]));
|
||||
measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current +
|
||||
CURRENT_SMOOTH_COEF * (float) ((int16_t) (rxbuff[4] << 8 | rxbuff[5]));
|
||||
CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5]));
|
||||
measure->temperature = rxbuff[6];
|
||||
|
||||
// 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了
|
||||
|
|
@ -172,21 +148,22 @@ static void DecodeDJIMotor(CANInstance *_instance) {
|
|||
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
|
||||
}
|
||||
|
||||
static void DJIMotorLostCallback(void *motor_ptr) {
|
||||
DJIMotorInstance *motor = (DJIMotorInstance *) motor_ptr;
|
||||
static void DJIMotorLostCallback(void *motor_ptr)
|
||||
{
|
||||
DJIMotorInstance *motor = (DJIMotorInstance *)motor_ptr;
|
||||
uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2;
|
||||
LOGWARNING("[dji_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id);
|
||||
}
|
||||
|
||||
// 电机初始化,返回一个电机实例
|
||||
DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) {
|
||||
DJIMotorInstance *instance = (DJIMotorInstance *) malloc(sizeof(DJIMotorInstance));
|
||||
DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
|
||||
{
|
||||
DJIMotorInstance *instance = (DJIMotorInstance *)malloc(sizeof(DJIMotorInstance));
|
||||
memset(instance, 0, sizeof(DJIMotorInstance));
|
||||
|
||||
// motor basic setting 电机基本设置
|
||||
instance->motor_type = config->motor_type; // 6020 or 2006 or 3508
|
||||
instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等
|
||||
instance->motor_control_type = config->motor_control_type; //电流控制or电压控制
|
||||
|
||||
// motor controller init 电机控制器初始化
|
||||
PIDInit(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
|
||||
|
|
@ -208,9 +185,9 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) {
|
|||
|
||||
// 注册守护线程
|
||||
Daemon_Init_Config_s daemon_config = {
|
||||
.callback = DJIMotorLostCallback,
|
||||
.owner_id = instance,
|
||||
.reload_count = 2, // 20ms未收到数据则丢失
|
||||
.callback = DJIMotorLostCallback,
|
||||
.owner_id = instance,
|
||||
.reload_count = 2, // 20ms未收到数据则丢失
|
||||
};
|
||||
instance->daemon = DaemonRegister(&daemon_config);
|
||||
|
||||
|
|
@ -220,7 +197,8 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) {
|
|||
}
|
||||
|
||||
/* 电流只能通过电机自带传感器监测,后续考虑加入力矩传感器应变片等 */
|
||||
void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type) {
|
||||
void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type)
|
||||
{
|
||||
if (loop == ANGLE_LOOP)
|
||||
motor->motor_settings.angle_feedback_source = type;
|
||||
else if (loop == SPEED_LOOP)
|
||||
|
|
@ -229,38 +207,31 @@ void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback
|
|||
LOGERROR("[dji_motor] loop type error, check memory access and func param"); // 检查是否传入了正确的LOOP类型,或发生了指针越界
|
||||
}
|
||||
|
||||
void DJIMotorStop(DJIMotorInstance *motor) {
|
||||
void DJIMotorStop(DJIMotorInstance *motor)
|
||||
{
|
||||
motor->stop_flag = MOTOR_STOP;
|
||||
}
|
||||
|
||||
void DJIMotorEnable(DJIMotorInstance *motor) {
|
||||
void DJIMotorEnable(DJIMotorInstance *motor)
|
||||
{
|
||||
motor->stop_flag = MOTOR_ENALBED;
|
||||
}
|
||||
|
||||
/* 修改电机的实际闭环对象 */
|
||||
void DJIMotorOuterLoop(DJIMotorInstance *motor, Closeloop_Type_e outer_loop) {
|
||||
void DJIMotorOuterLoop(DJIMotorInstance *motor, Closeloop_Type_e outer_loop)
|
||||
{
|
||||
motor->motor_settings.outer_loop_type = outer_loop;
|
||||
}
|
||||
|
||||
// 设置参考值
|
||||
void DJIMotorSetRef(DJIMotorInstance *motor, float ref) {
|
||||
void DJIMotorSetRef(DJIMotorInstance *motor, float ref)
|
||||
{
|
||||
motor->motor_controller.pid_ref = ref;
|
||||
}
|
||||
|
||||
static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f};
|
||||
//依据3508电机功率模型，预测电机输出功率
|
||||
static float EstimatePower(DJIMotorInstance* chassis_motor,float output)
|
||||
{
|
||||
float I_cmd = chassis_motor->motor_controller.current_PID.Output;
|
||||
float w = chassis_motor->measure.speed_aps /6 ;//aps to rpm
|
||||
|
||||
float power = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w + motor_power_K[2]*I_cmd*I_cmd;
|
||||
return power;
|
||||
}
|
||||
|
||||
|
||||
// 为所有电机实例计算三环PID,发送控制报文
|
||||
void DJIMotorControl() {
|
||||
void DJIMotorControl()
|
||||
{
|
||||
// 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性
|
||||
uint8_t group, num; // 电机组号和组内编号
|
||||
int16_t set; // 电机控制CAN发送设定值
|
||||
|
|
@ -271,7 +242,8 @@ void DJIMotorControl() {
|
|||
float pid_measure, pid_ref; // 电机PID测量值和设定值
|
||||
|
||||
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
|
||||
for (size_t i = 0; i < idx; ++i) { // 减小访存开销,先保存指针引用
|
||||
for (size_t i = 0; i < idx; ++i)
|
||||
{ // 减小访存开销,先保存指针引用
|
||||
motor = dji_motor_instance[i];
|
||||
motor_setting = &motor->motor_settings;
|
||||
motor_controller = &motor->motor_controller;
|
||||
|
|
@ -282,7 +254,8 @@ void DJIMotorControl() {
|
|||
|
||||
// pid_ref会顺次通过被启用的闭环充当数据的载体
|
||||
// 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
|
||||
if ((motor_setting->close_loop_type & ANGLE_LOOP) && (motor_setting->outer_loop_type == ANGLE_LOOP)) {
|
||||
if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP)
|
||||
{
|
||||
if (motor_setting->angle_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor_controller->other_angle_feedback_ptr;
|
||||
else
|
||||
|
|
@ -292,8 +265,8 @@ void DJIMotorControl() {
|
|||
}
|
||||
|
||||
// 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
|
||||
if ((motor_setting->close_loop_type & SPEED_LOOP) &&
|
||||
(motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))) {
|
||||
if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
|
||||
{
|
||||
if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD)
|
||||
pid_ref += *motor_controller->speed_feedforward_ptr;
|
||||
|
||||
|
|
@ -308,54 +281,22 @@ void DJIMotorControl() {
|
|||
// 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
|
||||
if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD)
|
||||
pid_ref += *motor_controller->current_feedforward_ptr;
|
||||
if (motor_setting->close_loop_type & CURRENT_LOOP) {
|
||||
//现在电调都有内置电流环，无需pid计算
|
||||
//pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
|
||||
if (motor_setting->close_loop_type & CURRENT_LOOP)
|
||||
{
|
||||
pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
|
||||
}
|
||||
|
||||
if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
|
||||
pid_ref *= -1;
|
||||
|
||||
//功率限制
|
||||
if(motor_setting->power_limit_flag == POWER_LIMIT_ON)
|
||||
{
|
||||
float I_cmd = pid_ref;
|
||||
float w = measure->speed_aps /6 ;//aps to rpm
|
||||
motor_controller->motor_power_predict = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w + motor_power_K[2]*I_cmd*I_cmd;
|
||||
//这里K应该使用所有底盘电机一起计算 (在底盘任务中)
|
||||
//float K = motor_controller->motor_power_max / motor_controller->motor_power_predict;
|
||||
float K = motor_controller->motor_power_scale;
|
||||
if(K>=1 || motor_controller->motor_power_predict < 0)//未超过最大功率 或做负功的电机 不做限制
|
||||
{
|
||||
|
||||
}
|
||||
else if(K<1)
|
||||
{
|
||||
float P_cmd = K * motor_controller->motor_power_predict; //对输出功率进行缩放
|
||||
|
||||
float a = motor_power_K[2] ;
|
||||
float b = motor_power_K[0] * w;
|
||||
float c = motor_power_K[1] * w * w - P_cmd;
|
||||
|
||||
if(pid_ref < 0)
|
||||
pid_ref = (-b - sqrtf(b*b-4*a*c))/(2*a);
|
||||
else
|
||||
pid_ref = (-b + sqrtf(b*b-4*a*c))/(2*a);
|
||||
}
|
||||
//if( motor_controller->motor_power_predict < )
|
||||
}
|
||||
|
||||
// 获取最终输出 此处注意set不要超过int16_t能表达的最大数 +-32767
|
||||
|
||||
pid_ref = float_constrain(pid_ref,-16384,16384);
|
||||
// 获取最终输出
|
||||
set = (int16_t) pid_ref;
|
||||
set = (int16_t)pid_ref;
|
||||
|
||||
// 分组填入发送数据
|
||||
group = motor->sender_group;
|
||||
num = motor->message_num;
|
||||
sender_assignment[group].tx_buff[2 * num] = (uint8_t) (set >> 8); // 低八位
|
||||
sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t) (set & 0x00ff); // 高八位
|
||||
sender_assignment[group].tx_buff[2 * num] = (uint8_t)(set >> 8); // 低八位
|
||||
sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位
|
||||
|
||||
// 若该电机处于停止状态,直接将buff置零
|
||||
if (motor->stop_flag == MOTOR_STOP)
|
||||
|
|
@ -363,8 +304,10 @@ void DJIMotorControl() {
|
|||
}
|
||||
|
||||
// 遍历flag,检查是否要发送这一帧报文
|
||||
for (size_t i = 0; i < 10; ++i) {
|
||||
if (sender_enable_flag[i]) {
|
||||
for (size_t i = 0; i < 6; ++i)
|
||||
{
|
||||
if (sender_enable_flag[i])
|
||||
{
|
||||
CANTransmit(&sender_assignment[i], 1);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -58,7 +58,6 @@ typedef struct
|
|||
uint8_t message_num;
|
||||
|
||||
Motor_Type_e motor_type; // 电机类型
|
||||
Motor_Control_Type_e motor_control_type;//电机控制方式
|
||||
Motor_Working_Type_e stop_flag; // 启停标志
|
||||
|
||||
DaemonInstance* daemon;
|
||||
|
|
|
|||
|
|
@ -1,228 +0,0 @@
|
|||
#include "dmmotor.h"
|
||||
#include "memory.h"
|
||||
#include "general_def.h"
|
||||
#include "user_lib.h"
|
||||
#include "cmsis_os.h"
|
||||
#include "string.h"
|
||||
#include "daemon.h"
|
||||
#include "stdlib.h"
|
||||
#include "bsp_log.h"
|
||||
|
||||
static uint8_t idx;
|
||||
static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT];
|
||||
static osThreadId dm_task_handle[DM_MOTOR_CNT];
|
||||
/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
|
||||
static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
|
||||
{
|
||||
float span = x_max - x_min;
|
||||
float offset = x_min;
|
||||
return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
|
||||
}
|
||||
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
|
||||
{
|
||||
float span = x_max - x_min;
|
||||
float offset = x_min;
|
||||
return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
|
||||
}
|
||||
|
||||
static void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
|
||||
{
|
||||
memset(motor->motor_can_instance->tx_buff, 0xff, 7); // 发送电机指令的时候前面7bytes都是0xff
|
||||
motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id
|
||||
CANTransmit(motor->motor_can_instance, 1);
|
||||
}
|
||||
|
||||
static void DMMotorDecode(CANInstance *motor_can)
|
||||
{
|
||||
uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
|
||||
uint8_t *rxbuff = motor_can->rx_buff;
|
||||
DMMotorInstance *motor = (DMMotorInstance *)motor_can->id;
|
||||
DM_Motor_Measure_s *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针
|
||||
|
||||
DaemonReload(motor->motor_daemon);
|
||||
|
||||
measure->last_position = measure->position;
|
||||
|
||||
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
|
||||
measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16);
|
||||
|
||||
measure->angle_single_round = RAD_2_DEGREE * (measure->position+3.141593);
|
||||
|
||||
tmp = (uint16_t)((rxbuff[3] << 4) | rxbuff[4] >> 4);
|
||||
measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12);
|
||||
|
||||
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
|
||||
measure->torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12);
|
||||
|
||||
measure->T_Mos = (float)rxbuff[6];
|
||||
measure->T_Rotor = (float)rxbuff[7];
|
||||
|
||||
// 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了
|
||||
if (measure->position - measure->last_position > 3.1425926)
|
||||
measure->total_round--;
|
||||
else if (measure->position - measure->last_position < -3.1415926)
|
||||
measure->total_round++;
|
||||
|
||||
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
|
||||
}
|
||||
|
||||
static void DMMotorLostCallback(void *motor_ptr)
|
||||
{
|
||||
DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
|
||||
uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2;
|
||||
LOGWARNING("[dm_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id);
|
||||
}
|
||||
void DMMotorCaliEncoder(DMMotorInstance *motor)
|
||||
{
|
||||
DMMotorSetMode(DM_CMD_ZERO_POSITION, motor);
|
||||
DWT_Delay(0.1);
|
||||
}
|
||||
DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
|
||||
{
|
||||
DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance));
|
||||
memset(motor, 0, sizeof(DMMotorInstance));
|
||||
|
||||
config->can_init_config.rx_id = config->can_init_config.rx_id;
|
||||
config->can_init_config.tx_id = config->can_init_config.tx_id;
|
||||
motor->motor_settings = config->controller_setting_init_config;
|
||||
PIDInit(&motor->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
|
||||
PIDInit(&motor->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
|
||||
PIDInit(&motor->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
|
||||
motor->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
|
||||
motor->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
|
||||
|
||||
config->can_init_config.can_module_callback = DMMotorDecode;
|
||||
config->can_init_config.id = motor;
|
||||
motor->motor_can_instance = CANRegister(&config->can_init_config);
|
||||
|
||||
Daemon_Init_Config_s conf = {
|
||||
.callback = DMMotorLostCallback,
|
||||
.owner_id = motor,
|
||||
.reload_count = 10,
|
||||
};
|
||||
motor->motor_daemon = DaemonRegister(&conf);
|
||||
|
||||
DMMotorEnable(motor);
|
||||
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
|
||||
DWT_Delay(0.1f);
|
||||
dm_motor_instance[idx++] = motor;
|
||||
return motor;
|
||||
}
|
||||
|
||||
void DMMotorSetRef(DMMotorInstance *motor, float ref)
|
||||
{
|
||||
motor->motor_controller.pid_ref = ref;
|
||||
}
|
||||
|
||||
void DMMotorEnable(DMMotorInstance *motor)
|
||||
{
|
||||
motor->stop_flag = MOTOR_ENALBED;
|
||||
}
|
||||
|
||||
void DMMotorStop(DMMotorInstance *motor)//不使用使能模式是因为需要收到反馈
|
||||
{
|
||||
motor->stop_flag = MOTOR_STOP;
|
||||
}
|
||||
|
||||
void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type)
|
||||
{
|
||||
motor->motor_settings.outer_loop_type = type;
|
||||
}
|
||||
|
||||
|
||||
//还得使用力矩控制
|
||||
void DMMotorTask(void const *argument)
|
||||
{
|
||||
float pid_ref, set, pid_measure;
|
||||
|
||||
DMMotorInstance *motor = (DMMotorInstance *)argument;
|
||||
Motor_Controller_s *motor_controller; // 电机控制器
|
||||
DM_Motor_Measure_s *measure = &motor->measure;
|
||||
motor_controller = &motor->motor_controller;
|
||||
Motor_Control_Setting_s *setting = &motor->motor_settings;
|
||||
//CANInstance *motor_can = motor->motor_can_instance;
|
||||
//uint16_t tmp;
|
||||
DMMotor_Send_s motor_send_mailbox;
|
||||
while (1)
|
||||
{
|
||||
pid_ref = motor->motor_controller.pid_ref;//保存设定值
|
||||
|
||||
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
|
||||
pid_ref *= -1;
|
||||
|
||||
// pid_ref会顺次通过被启用的闭环充当数据的载体
|
||||
// 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
|
||||
if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
|
||||
{
|
||||
if (setting->angle_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor_controller->other_angle_feedback_ptr;
|
||||
else
|
||||
pid_measure = measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
|
||||
// 更新pid_ref进入下一个环
|
||||
pid_ref = PIDCalculate(&motor_controller->angle_PID, pid_measure, pid_ref);
|
||||
}
|
||||
|
||||
// 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
|
||||
|
||||
|
||||
if ((setting->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
|
||||
{
|
||||
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
|
||||
pid_ref += *motor_controller->speed_feedforward_ptr;
|
||||
if (setting->speed_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor_controller->other_speed_feedback_ptr;
|
||||
else // MOTOR_FEED
|
||||
pid_measure = measure->velocity;
|
||||
// 更新pid_ref进入下一个环
|
||||
pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
|
||||
}
|
||||
|
||||
if (setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
|
||||
pid_ref *= -1;
|
||||
|
||||
set = pid_ref;
|
||||
|
||||
if (motor->stop_flag == MOTOR_STOP)
|
||||
set = 0;
|
||||
|
||||
LIMIT_MIN_MAX(set, DM_T_MIN, DM_T_MAX);
|
||||
motor_send_mailbox.position_des = float_to_uint(0, DM_P_MIN, DM_P_MAX, 16);
|
||||
motor_send_mailbox.velocity_des = float_to_uint(0, DM_V_MIN, DM_V_MAX, 12);
|
||||
motor_send_mailbox.torque_des = float_to_uint(set, DM_T_MIN, DM_T_MAX, 12);
|
||||
motor_send_mailbox.Kp = 0;//只使用力矩控制无需kpkd参数
|
||||
motor_send_mailbox.Kd = 0;//只使用力矩控制无需kpkd参数
|
||||
|
||||
|
||||
|
||||
motor->motor_can_instance->tx_buff[0] = (uint8_t)(motor_send_mailbox.position_des >> 8);
|
||||
motor->motor_can_instance->tx_buff[1] = (uint8_t)(motor_send_mailbox.position_des);
|
||||
motor->motor_can_instance->tx_buff[2] = (uint8_t)(motor_send_mailbox.velocity_des >> 4);
|
||||
motor->motor_can_instance->tx_buff[3] = (uint8_t)(((motor_send_mailbox.velocity_des & 0xF) << 4) | (motor_send_mailbox.Kp >> 8));
|
||||
motor->motor_can_instance->tx_buff[4] = (uint8_t)(motor_send_mailbox.Kp);
|
||||
motor->motor_can_instance->tx_buff[5] = (uint8_t)(motor_send_mailbox.Kd >> 4);
|
||||
motor->motor_can_instance->tx_buff[6] = (uint8_t)(((motor_send_mailbox.Kd & 0xF) << 4) | (motor_send_mailbox.torque_des >> 8));
|
||||
motor->motor_can_instance->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des);
|
||||
|
||||
|
||||
|
||||
|
||||
CANTransmit(motor->motor_can_instance, 1);
|
||||
|
||||
osDelay(2);
|
||||
}
|
||||
}
|
||||
void DMMotorControlInit()
|
||||
{
|
||||
char dm_task_name[5] = "dm";
|
||||
// 遍历所有电机实例,创建任务
|
||||
if (!idx)
|
||||
return;
|
||||
for (size_t i = 0; i < idx; i++)
|
||||
{
|
||||
char dm_id_buff[2] = {0};
|
||||
__itoa(i, dm_id_buff, 10);
|
||||
strcat(dm_task_name, dm_id_buff);
|
||||
osThreadDef(dm_task_name, DMMotorTask, osPriorityNormal, 0, 128);
|
||||
dm_task_handle[i] = osThreadCreate(osThread(dm_task_name), dm_motor_instance[i]);
|
||||
}
|
||||
}
|
||||
|
|
@ -1,75 +0,0 @@
|
|||
#ifndef DM4310_H
|
||||
#define DM4310_H
|
||||
#include <stdint.h>
|
||||
#include "bsp_can.h"
|
||||
#include "controller.h"
|
||||
#include "motor_def.h"
|
||||
#include "daemon.h"
|
||||
|
||||
#define DM_MOTOR_CNT 1
|
||||
|
||||
#define DM_P_MIN (-3.1415926f)
|
||||
#define DM_P_MAX 3.1415926f
|
||||
#define DM_V_MIN (-45.0f)
|
||||
#define DM_V_MAX 45.0f
|
||||
#define DM_T_MIN (-18.0f)
|
||||
#define DM_T_MAX 18.0f
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t id;
|
||||
uint8_t state;
|
||||
float velocity; //速度
|
||||
float last_position; //上次位置
|
||||
float position; //位置
|
||||
float torque; //力矩
|
||||
float T_Mos; //mos温度
|
||||
float T_Rotor; //电机温度
|
||||
float angle_single_round; //单圈角度
|
||||
int32_t total_round; //总圈数
|
||||
float total_angle; //总角度
|
||||
}DM_Motor_Measure_s;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint16_t position_des;
|
||||
uint16_t velocity_des;
|
||||
uint16_t torque_des;
|
||||
uint16_t Kp;
|
||||
uint16_t Kd;
|
||||
}DMMotor_Send_s;
|
||||
typedef struct
|
||||
{
|
||||
DM_Motor_Measure_s measure;
|
||||
Motor_Control_Setting_s motor_settings;
|
||||
Motor_Controller_s motor_controller;//电机控制器
|
||||
|
||||
CANInstance *motor_can_instance;//电机can实例
|
||||
|
||||
Motor_Type_e motor_type;//电机类型
|
||||
Motor_Working_Type_e stop_flag;
|
||||
|
||||
DaemonInstance* motor_daemon;
|
||||
uint32_t lost_cnt;
|
||||
}DMMotorInstance;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
DM_CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令
|
||||
DM_CMD_RESET_MODE = 0xfd, // 停止
|
||||
DM_CMD_ZERO_POSITION = 0xfe, // 将当前的位置设置为编码器零位
|
||||
DM_CMD_CLEAR_ERROR = 0xfb // 清除电机过热错误
|
||||
}DMMotor_Mode_e;
|
||||
|
||||
DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config);
|
||||
|
||||
void DMMotorSetRef(DMMotorInstance *motor, float ref);
|
||||
|
||||
void DMMotorOuterLoop(DMMotorInstance *motor,Closeloop_Type_e closeloop_type);
|
||||
|
||||
void DMMotorEnable(DMMotorInstance *motor);
|
||||
|
||||
void DMMotorStop(DMMotorInstance *motor);
|
||||
void DMMotorCaliEncoder(DMMotorInstance *motor);
|
||||
void DMMotorControlInit();
|
||||
#endif // !DMMOTOR
|
||||
|
|
@ -68,12 +68,6 @@ typedef enum
|
|||
MOTOR_ENALBED = 1,
|
||||
} Motor_Working_Type_e;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
NO_POWER_LIMIT = 0,
|
||||
POWER_LIMIT_ON = 1,
|
||||
} Power_Limit_Type_e;
|
||||
|
||||
/* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
|
||||
typedef struct
|
||||
{
|
||||
|
|
@ -84,19 +78,9 @@ typedef struct
|
|||
Feedback_Source_e angle_feedback_source; // 角度反馈类型
|
||||
Feedback_Source_e speed_feedback_source; // 速度反馈类型
|
||||
Feedfoward_Type_e feedforward_flag; // 前馈标志
|
||||
Power_Limit_Type_e power_limit_flag; //功率限制标志
|
||||
|
||||
} Motor_Control_Setting_s;
|
||||
|
||||
/* 电机控制方式枚举 */
|
||||
typedef enum
|
||||
{
|
||||
CONTROL_TYPE_NONE = 0,
|
||||
CURRENT_CONTROL,
|
||||
VOLTAGE_CONTROL,
|
||||
} Motor_Control_Type_e;
|
||||
|
||||
|
||||
/* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/
|
||||
// 后续增加前馈数据指针
|
||||
typedef struct
|
||||
|
|
@ -111,10 +95,6 @@ typedef struct
|
|||
PIDInstance angle_PID;
|
||||
|
||||
float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环
|
||||
|
||||
float motor_power_max; //每个电机分配的功率上限
|
||||
float motor_power_predict; //根据模型预测的电机功率
|
||||
float motor_power_scale; //电机功率缩放比例
|
||||
} Motor_Controller_s;
|
||||
|
||||
/* 电机类型枚举 */
|
||||
|
|
@ -153,7 +133,6 @@ typedef struct
|
|||
Motor_Control_Setting_s controller_setting_init_config;
|
||||
Motor_Type_e motor_type;
|
||||
CAN_Init_Config_s can_init_config;
|
||||
Motor_Control_Type_e motor_control_type;
|
||||
} Motor_Init_Config_s;
|
||||
|
||||
#endif // !MOTOR_DEF_H
|
||||
|
|
|
|||
|
|
@ -33,20 +33,20 @@ void Servo_Motor_FreeAngle_Set(ServoInstance *Servo_Motor, int16_t S_angle)
|
|||
{
|
||||
switch (Servo_Motor->Servo_type)
|
||||
{
|
||||
case Servo180:
|
||||
if (S_angle > 180)
|
||||
S_angle = 180;
|
||||
break;
|
||||
case Servo270:
|
||||
if (S_angle > 270)
|
||||
S_angle = 270;
|
||||
break;
|
||||
case Servo360:
|
||||
if (S_angle > 100)
|
||||
S_angle = 100;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
case Servo180:
|
||||
if (S_angle > 180)
|
||||
S_angle = 180;
|
||||
break;
|
||||
case Servo270:
|
||||
if (S_angle > 270)
|
||||
S_angle = 270;
|
||||
break;
|
||||
case Servo360:
|
||||
if (S_angle > 100)
|
||||
S_angle = 100;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
if (S_angle < 0)
|
||||
S_angle = 0;
|
||||
|
|
@ -92,31 +92,31 @@ void ServeoMotorControl()
|
|||
|
||||
switch (Servo_Motor->Servo_type)
|
||||
{
|
||||
case Servo180:
|
||||
if (Servo_Motor->Servo_Angle_Type == Start_mode)
|
||||
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Init_angle * 20000 / 20 / 90;
|
||||
if (Servo_Motor->Servo_Angle_Type == Final_mode)
|
||||
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Final_angle * 20000 / 20 / 90;
|
||||
if (Servo_Motor->Servo_Angle_Type == Free_Angle_mode)
|
||||
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.free_angle * 20000 / 20 / 90;
|
||||
__HAL_TIM_SET_COMPARE(Servo_Motor->htim, Servo_Motor->Channel, compare_value[i]);
|
||||
break;
|
||||
case Servo270:
|
||||
if (Servo_Motor->Servo_Angle_Type == Start_mode)
|
||||
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Init_angle * 20000 / 20 / 135;
|
||||
if (Servo_Motor->Servo_Angle_Type == Final_mode)
|
||||
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Final_angle * 20000 / 20 / 135;
|
||||
if (Servo_Motor->Servo_Angle_Type == Free_Angle_mode)
|
||||
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.free_angle * 20000 / 20 / 135;
|
||||
__HAL_TIM_SET_COMPARE(Servo_Motor->htim, Servo_Motor->Channel, compare_value[i]);
|
||||
break;
|
||||
case Servo360:
|
||||
/*500-2500的占空比 500-1500对应正向转速 1500-2500对于反向转速*/
|
||||
compare_value[i] = 500 + 20 * Servo_Motor->Servo_Angle.servo360speed;
|
||||
__HAL_TIM_SET_COMPARE(Servo_Motor->htim, Servo_Motor->Channel, compare_value[i]);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
case Servo180:
|
||||
if (Servo_Motor->Servo_Angle_Type == Start_mode)
|
||||
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Init_angle * 20000 / 20 / 90;
|
||||
if (Servo_Motor->Servo_Angle_Type == Final_mode)
|
||||
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Final_angle * 20000 / 20 / 90;
|
||||
if (Servo_Motor->Servo_Angle_Type == Free_Angle_mode)
|
||||
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.free_angle * 20000 / 20 / 90;
|
||||
__HAL_TIM_SET_COMPARE(Servo_Motor->htim, Servo_Motor->Channel, compare_value[i]);
|
||||
break;
|
||||
case Servo270:
|
||||
if (Servo_Motor->Servo_Angle_Type == Start_mode)
|
||||
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Init_angle * 20000 / 20 / 135;
|
||||
if (Servo_Motor->Servo_Angle_Type == Final_mode)
|
||||
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Final_angle * 20000 / 20 / 135;
|
||||
if (Servo_Motor->Servo_Angle_Type == Free_Angle_mode)
|
||||
compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.free_angle * 20000 / 20 / 135;
|
||||
__HAL_TIM_SET_COMPARE(Servo_Motor->htim, Servo_Motor->Channel, compare_value[i]);
|
||||
break;
|
||||
case Servo360:
|
||||
/*500-2500的占空比 500-1500对应正向转速 1500-2500对于反向转速*/
|
||||
compare_value[i] = 500 + 20 * Servo_Motor->Servo_Angle.servo360speed;
|
||||
__HAL_TIM_SET_COMPARE(Servo_Motor->htim, Servo_Motor->Channel, compare_value[i]);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -14,7 +14,7 @@
|
|||
|
||||
#include "main.h"
|
||||
#include "tim.h"
|
||||
#include <stdint.h>
|
||||
#include <stdint-gcc.h>
|
||||
|
||||
#define SERVO_MOTOR_CNT 7
|
||||
|
||||
|
|
|
|||
|
|
@ -157,32 +157,37 @@ typedef struct
|
|||
uint8_t supply_projectile_num;
|
||||
} ext_supply_projectile_action_t;
|
||||
|
||||
/* ID: 0X0201 Byte: 13 V1.6.1 机器人性能体系数据 */
|
||||
/* ID: 0X0201 Byte: 27 机器人状态数据 */
|
||||
typedef struct
|
||||
{
|
||||
uint8_t robot_id;
|
||||
uint8_t robot_level;
|
||||
uint16_t current_HP;
|
||||
uint16_t maximum_HP;
|
||||
uint16_t shooter_barrel_cooling_value;
|
||||
uint16_t shooter_barrel_heat_limit;
|
||||
uint16_t chassis_power_limit;
|
||||
|
||||
uint8_t power_management_gimbal_output : 1;
|
||||
uint8_t power_management_chassis_output : 1;
|
||||
uint8_t power_management_shooter_output : 1;
|
||||
uint8_t robot_id;
|
||||
uint8_t robot_level;
|
||||
uint16_t remain_HP;
|
||||
uint16_t max_HP;
|
||||
uint16_t shooter_id1_17mm_cooling_rate;
|
||||
uint16_t shooter_id1_17mm_cooling_limit;
|
||||
uint16_t shooter_id1_17mm_speed_limit;
|
||||
uint16_t shooter_id2_17mm_cooling_rate;
|
||||
uint16_t shooter_id2_17mm_cooling_limit;
|
||||
uint16_t shooter_id2_17mm_speed_limit;
|
||||
uint16_t shooter_id1_42mm_cooling_rate;
|
||||
uint16_t shooter_id1_42mm_cooling_limit;
|
||||
uint16_t shooter_id1_42mm_speed_limit;
|
||||
uint16_t chassis_power_limit;
|
||||
uint8_t mains_power_gimbal_output : 1;
|
||||
uint8_t mains_power_chassis_output : 1;
|
||||
uint8_t mains_power_shooter_output : 1;
|
||||
} ext_game_robot_state_t;
|
||||
|
||||
/* ID: 0X0202 Byte: 14 v1.6.1 实时功率热量数据 */
|
||||
/* ID: 0X0202 Byte: 14 实时功率热量数据 */
|
||||
typedef struct
|
||||
{
|
||||
uint16_t chassis_voltage;
|
||||
uint16_t chassis_current;
|
||||
float chassis_power;
|
||||
uint16_t buffer_energy;
|
||||
uint16_t shooter_17mm_1_barrel_heat;
|
||||
uint16_t shooter_17mm_2_barrel_heat;
|
||||
uint16_t shooter_42mm_barrel_heat;
|
||||
uint16_t chassis_volt;
|
||||
uint16_t chassis_current;
|
||||
float chassis_power; // 瞬时功率
|
||||
uint16_t chassis_power_buffer; // 60焦耳缓冲能量
|
||||
uint16_t shooter_heat0; // 17mm
|
||||
uint16_t shooter_heat1;
|
||||
} ext_power_heat_data_t;
|
||||
|
||||
/* ID: 0x0203 Byte: 16 机器人位置数据 */
|
||||
|
|
|
|||
|
|
@ -14,13 +14,9 @@
|
|||
#include "referee_UI.h"
|
||||
#include "string.h"
|
||||
#include "cmsis_os.h"
|
||||
#include "chassis.h"
|
||||
#include "super_cap.h"
|
||||
|
||||
extern SuperCapInstance *cap; // 超级电容
|
||||
static Referee_Interactive_info_t *Interactive_data; // UI绘制需要的机器人状态数据
|
||||
static referee_info_t *referee_recv_info; // 接收到的裁判系统数据
|
||||
Referee_Interactive_info_t ui_data;
|
||||
uint8_t UI_Seq; // 包序号，供整个referee文件使用
|
||||
// @todo 不应该使用全局变量
|
||||
|
||||
|
|
@ -32,7 +28,7 @@ uint8_t UI_Seq; // 包序号，供整个ref
|
|||
*/
|
||||
static void DeterminRobotID()
|
||||
{
|
||||
// id小于7是红色,大于7是蓝色 #define Robot_Red 0 #define Robot_Blue 1
|
||||
// id小于7是红色,大于7是蓝色,0为红色，1为蓝色 #define Robot_Red 0 #define Robot_Blue 1
|
||||
referee_recv_info->referee_id.Robot_Color = referee_recv_info->GameRobotState.robot_id > 7 ? Robot_Blue : Robot_Red;
|
||||
referee_recv_info->referee_id.Robot_ID = referee_recv_info->GameRobotState.robot_id;
|
||||
referee_recv_info->referee_id.Cilent_ID = 0x0100 + referee_recv_info->referee_id.Robot_ID; // 计算客户端ID
|
||||
|
|
@ -53,15 +49,14 @@ referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Int
|
|||
|
||||
void UITask()
|
||||
{
|
||||
//RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
|
||||
RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
|
||||
MyUIRefresh(referee_recv_info, Interactive_data);
|
||||
}
|
||||
|
||||
static Graph_Data_t UI_shoot_line[10]; // 射击准线
|
||||
static Graph_Data_t UI_shoot_yuan[2]; // 射击圆心
|
||||
static Graph_Data_t UI_Energy[4]; // 电容能量条
|
||||
static String_Data_t UI_State_sta[7]; // 机器人状态,静态只需画一次
|
||||
static String_Data_t UI_State_dyn[7]; // 机器人状态,动态先add才能change
|
||||
static Graph_Data_t UI_Energy[3]; // 电容能量条
|
||||
static String_Data_t UI_State_sta[6]; // 机器人状态,静态只需画一次
|
||||
static String_Data_t UI_State_dyn[6]; // 机器人状态,动态先add才能change
|
||||
static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565};
|
||||
|
||||
void MyUIInit()
|
||||
|
|
@ -74,13 +69,13 @@ void MyUIInit()
|
|||
DeterminRobotID(); // 确定ui要发送到的目标客户端
|
||||
UIDelete(&referee_recv_info->referee_id, UI_Data_Del_ALL, 0); // 清空UI
|
||||
|
||||
// 绘制发射基准线和基准圆
|
||||
// 绘制发射基准线
|
||||
UILineDraw(&UI_shoot_line[0], "sl0", UI_Graph_ADD, 7, UI_Color_White, 3, 710, shoot_line_location[0], 1210, shoot_line_location[0]);
|
||||
UILineDraw(&UI_shoot_line[1], "sl1", UI_Graph_ADD, 7, UI_Color_White, 3, shoot_line_location[1], 340, shoot_line_location[1], 740);
|
||||
UILineDraw(&UI_shoot_line[2], "sl2", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[2], 1110, shoot_line_location[2]);
|
||||
UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Orange, 2, 960, 200, 960, 600);
|
||||
UICircleDraw(&UI_shoot_yuan[0],"sy0",UI_Graph_ADD,7,UI_Color_Yellow,2,960,540,10);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_shoot_line[0], UI_shoot_line[1], UI_shoot_line[2], UI_shoot_line[3],UI_shoot_yuan[0]);
|
||||
UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[3], 1110, shoot_line_location[3]);
|
||||
UILineDraw(&UI_shoot_line[4], "sl4", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[4], 1110, shoot_line_location[4]);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_shoot_line[0], UI_shoot_line[1], UI_shoot_line[2], UI_shoot_line[3], UI_shoot_line[4]);
|
||||
|
||||
// 绘制车辆状态标志指示
|
||||
UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 150, 750, "chassis:");
|
||||
|
|
@ -89,9 +84,9 @@ void MyUIInit()
|
|||
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[1]);
|
||||
UICharDraw(&UI_State_sta[2], "ss2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 150, 650, "shoot:");
|
||||
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[2]);
|
||||
UICharDraw(&UI_State_sta[3], "ss3", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 600, "F.frict:");
|
||||
UICharDraw(&UI_State_sta[3], "ss3", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 600, "frict:");
|
||||
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[3]);
|
||||
UICharDraw(&UI_State_sta[4], "ss4", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 550, "Q.lid:");
|
||||
UICharDraw(&UI_State_sta[4], "ss4", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 550, "lid:");
|
||||
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[4]);
|
||||
|
||||
// 绘制车辆状态标志，动态
|
||||
|
|
@ -108,23 +103,17 @@ void MyUIInit()
|
|||
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
|
||||
|
||||
// 底盘功率显示，静态
|
||||
UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 620, 230, "Power_MAX:");
|
||||
UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 7, UI_Color_Green, 18, 2, 620, 230, "Power:");
|
||||
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[5]);
|
||||
|
||||
// 能量条框
|
||||
UIRectangleDraw(&UI_Energy[0], "ss6", UI_Graph_ADD, 7, UI_Color_Green, 2, 720, 140, 1220, 180);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[0]);
|
||||
|
||||
// 底盘功率显示,动态
|
||||
UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 2, 850, 230, referee_recv_info->GameRobotState.chassis_power_limit*1000);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[1]);
|
||||
|
||||
UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 2, 750, 230, 24000);
|
||||
// 能量条初始状态
|
||||
UILineDraw(&UI_Energy[2], "sd6", UI_Graph_ADD, 8, UI_Color_Pink, 30, 720, 160, 1020, 160);
|
||||
|
||||
//超级电容电压
|
||||
// UIIntDraw(&UI_Energy[3],"sd7",UI_Graph_ADD,8,UI_Color_Pink,18,2,1000,300,cap->cap_msg.cap_vol);
|
||||
// UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[3]);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
|
||||
}
|
||||
|
||||
// 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
|
||||
|
|
@ -144,23 +133,39 @@ static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测
|
|||
{
|
||||
_Interactive_data->chassis_mode = CHASSIS_ZERO_FORCE;
|
||||
_Interactive_data->gimbal_mode = GIMBAL_ZERO_FORCE;
|
||||
_Interactive_data->shoot_mode = SHOOT_ON;
|
||||
_Interactive_data->friction_mode = FRICTION_ON;
|
||||
_Interactive_data->lid_mode = LID_OPEN;
|
||||
_Interactive_data->Chassis_Power_Data.chassis_power_mx += 3.5;
|
||||
if (_Interactive_data->Chassis_Power_Data.chassis_power_mx >= 18)
|
||||
_Interactive_data->Chassis_Power_Data.chassis_power_mx = 0;
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
_Interactive_data->chassis_mode = CHASSIS_ROTATE;
|
||||
_Interactive_data->gimbal_mode = GIMBAL_FREE_MODE;
|
||||
_Interactive_data->shoot_mode = SHOOT_OFF;
|
||||
_Interactive_data->friction_mode = FRICTION_OFF;
|
||||
_Interactive_data->lid_mode = LID_CLOSE;
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
{
|
||||
_Interactive_data->chassis_mode = CHASSIS_NO_FOLLOW;
|
||||
_Interactive_data->gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
_Interactive_data->shoot_mode = SHOOT_ON;
|
||||
_Interactive_data->friction_mode = FRICTION_ON;
|
||||
_Interactive_data->lid_mode = LID_OPEN;
|
||||
break;
|
||||
}
|
||||
case 3:
|
||||
{
|
||||
_Interactive_data->chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
_Interactive_data->gimbal_mode = GIMBAL_ZERO_FORCE;
|
||||
_Interactive_data->shoot_mode = SHOOT_OFF;
|
||||
_Interactive_data->friction_mode = FRICTION_OFF;
|
||||
_Interactive_data->lid_mode = LID_CLOSE;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
|
|
@ -217,6 +222,35 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
|
|||
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
|
||||
_Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0;
|
||||
}
|
||||
// shoot
|
||||
if (_Interactive_data->Referee_Interactive_Flag.shoot_flag == 1)
|
||||
{
|
||||
UICharDraw(&UI_State_dyn[2], "sd2", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 650, _Interactive_data->shoot_mode == SHOOT_ON ? "on " : "off");
|
||||
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]);
|
||||
_Interactive_data->Referee_Interactive_Flag.shoot_flag = 0;
|
||||
}
|
||||
// friction
|
||||
if (_Interactive_data->Referee_Interactive_Flag.friction_flag == 1)
|
||||
{
|
||||
UICharDraw(&UI_State_dyn[3], "sd3", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 600, _Interactive_data->friction_mode == FRICTION_ON ? "on " : "off");
|
||||
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[3]);
|
||||
_Interactive_data->Referee_Interactive_Flag.friction_flag = 0;
|
||||
}
|
||||
// lid
|
||||
if (_Interactive_data->Referee_Interactive_Flag.lid_flag == 1)
|
||||
{
|
||||
UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 550, _Interactive_data->lid_mode == LID_OPEN ? "open " : "close");
|
||||
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
|
||||
_Interactive_data->Referee_Interactive_Flag.lid_flag = 0;
|
||||
}
|
||||
// power
|
||||
if (_Interactive_data->Referee_Interactive_Flag.Power_flag == 1)
|
||||
{
|
||||
UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_Change, 8, UI_Color_Green, 18, 2, 2, 750, 230, _Interactive_data->Chassis_Power_Data.chassis_power_mx * 1000);
|
||||
UILineDraw(&UI_Energy[2], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)750 + _Interactive_data->Chassis_Power_Data.chassis_power_mx * 30, 160);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
|
||||
_Interactive_data->Referee_Interactive_Flag.Power_flag = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -238,4 +272,28 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
|
|||
_Interactive_data->Referee_Interactive_Flag.gimbal_flag = 1;
|
||||
_Interactive_data->gimbal_last_mode = _Interactive_data->gimbal_mode;
|
||||
}
|
||||
|
||||
if (_Interactive_data->shoot_mode != _Interactive_data->shoot_last_mode)
|
||||
{
|
||||
_Interactive_data->Referee_Interactive_Flag.shoot_flag = 1;
|
||||
_Interactive_data->shoot_last_mode = _Interactive_data->shoot_mode;
|
||||
}
|
||||
|
||||
if (_Interactive_data->friction_mode != _Interactive_data->friction_last_mode)
|
||||
{
|
||||
_Interactive_data->Referee_Interactive_Flag.friction_flag = 1;
|
||||
_Interactive_data->friction_last_mode = _Interactive_data->friction_mode;
|
||||
}
|
||||
|
||||
if (_Interactive_data->lid_mode != _Interactive_data->lid_last_mode)
|
||||
{
|
||||
_Interactive_data->Referee_Interactive_Flag.lid_flag = 1;
|
||||
_Interactive_data->lid_last_mode = _Interactive_data->lid_mode;
|
||||
}
|
||||
|
||||
if (_Interactive_data->Chassis_Power_Data.chassis_power_mx != _Interactive_data->Chassis_last_Power_Data.chassis_power_mx)
|
||||
{
|
||||
_Interactive_data->Referee_Interactive_Flag.Power_flag = 1;
|
||||
_Interactive_data->Chassis_last_Power_Data.chassis_power_mx = _Interactive_data->Chassis_Power_Data.chassis_power_mx;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -8,7 +8,6 @@
|
|||
* @brief 初始化裁判系统交互任务(UI和多机通信)
|
||||
*
|
||||
*/
|
||||
extern Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
|
||||
referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data);
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -7,8 +7,6 @@
|
|||
#include "bsp_usart.h"
|
||||
#include "FreeRTOS.h"
|
||||
|
||||
|
||||
|
||||
extern uint8_t UI_Seq;
|
||||
|
||||
#pragma pack(1)
|
||||
|
|
@ -56,8 +54,6 @@ typedef struct
|
|||
uint32_t lid_flag : 1;
|
||||
uint32_t friction_flag : 1;
|
||||
uint32_t Power_flag : 1;
|
||||
uint32_t aim_flag : 1;
|
||||
uint32_t cap_flag : 1;
|
||||
} Referee_Interactive_Flag_t;
|
||||
|
||||
// 此结构体包含UI绘制与机器人车间通信的需要的其他非裁判系统数据
|
||||
|
|
@ -67,12 +63,18 @@ typedef struct
|
|||
// 为UI绘制以及交互数据所用
|
||||
chassis_mode_e chassis_mode; // 底盘模式
|
||||
gimbal_mode_e gimbal_mode; // 云台模式
|
||||
shoot_mode_e shoot_mode; // 发射模式设置
|
||||
friction_mode_e friction_mode; // 摩擦轮关闭
|
||||
lid_mode_e lid_mode; // 弹舱盖打开
|
||||
Chassis_Power_Data_s Chassis_Power_Data; // 功率控制
|
||||
|
||||
// 上一次的模式，用于flag判断
|
||||
chassis_mode_e chassis_last_mode;
|
||||
gimbal_mode_e gimbal_last_mode;
|
||||
|
||||
uint8_t aim_last_fire;
|
||||
shoot_mode_e shoot_last_mode;
|
||||
friction_mode_e friction_last_mode;
|
||||
lid_mode_e lid_last_mode;
|
||||
Chassis_Power_Data_s Chassis_last_Power_Data;
|
||||
|
||||
} Referee_Interactive_info_t;
|
||||
|
||||
|
|
|
|||
|
|
@ -1,8 +1,14 @@
|
|||
/*
|
||||
* @Descripttion:
|
||||
* @version:
|
||||
* @Author: Chenfu
|
||||
* @Date: 2022-12-02 21:32:47
|
||||
* @LastEditTime: 2022-12-05 15:29:49
|
||||
*/
|
||||
#include "super_cap.h"
|
||||
#include "memory.h"
|
||||
#include "stdlib.h"
|
||||
|
||||
|
||||
static SuperCapInstance *super_cap_instance = NULL; // 可以由app保存此指针
|
||||
|
||||
static void SuperCapRxCallback(CANInstance *_instance)
|
||||
|
|
@ -11,10 +17,9 @@ static void SuperCapRxCallback(CANInstance *_instance)
|
|||
SuperCap_Msg_s *Msg;
|
||||
rxbuff = _instance->rx_buff;
|
||||
Msg = &super_cap_instance->cap_msg;
|
||||
Msg->input_vol = (int16_t)(rxbuff[1] << 8 | rxbuff[0]);
|
||||
Msg->cap_vol = (int16_t)(rxbuff[3] << 8 | rxbuff[2]);
|
||||
Msg->input_cur = (int16_t)(rxbuff[5] << 8 | rxbuff[4]);
|
||||
Msg->power_set = (int16_t)(rxbuff[7] << 8 | rxbuff[6]);
|
||||
Msg->vol = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]);
|
||||
Msg->current = (uint16_t)(rxbuff[2] << 8 | rxbuff[3]);
|
||||
Msg->power = (uint16_t)(rxbuff[4] << 8 | rxbuff[5]);
|
||||
}
|
||||
|
||||
SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config)
|
||||
|
|
@ -33,15 +38,6 @@ void SuperCapSend(SuperCapInstance *instance, uint8_t *data)
|
|||
CANTransmit(instance->can_ins,1);
|
||||
}
|
||||
|
||||
void SuperCapSetPower(SuperCapInstance *instance, float power_set)
|
||||
{
|
||||
uint16_t tmpPower = (uint16_t)(power_set * 100);
|
||||
uint8_t data[8] = {0};
|
||||
data[0] = tmpPower >> 8;
|
||||
data[1] = tmpPower;
|
||||
SuperCapSend(instance,data);
|
||||
}
|
||||
|
||||
SuperCap_Msg_s SuperCapGet(SuperCapInstance *instance)
|
||||
{
|
||||
return instance->cap_msg;
|
||||
|
|
|
|||
|
|
@ -1,3 +1,10 @@
|
|||
/*
|
||||
* @Descripttion:
|
||||
* @version:
|
||||
* @Author: Chenfu
|
||||
* @Date: 2022-12-02 21:32:47
|
||||
* @LastEditTime: 2022-12-05 15:25:46
|
||||
*/
|
||||
#ifndef SUPER_CAP_H
|
||||
#define SUPER_CAP_H
|
||||
|
||||
|
|
@ -6,10 +13,9 @@
|
|||
#pragma pack(1)
|
||||
typedef struct
|
||||
{
|
||||
int16_t input_vol; // 输入电压
|
||||
int16_t cap_vol; // 电容电压
|
||||
int16_t input_cur; // 输入电流
|
||||
int16_t power_set; // 设定功率
|
||||
uint16_t vol; // 电压
|
||||
uint16_t current; // 电流
|
||||
uint16_t power; // 功率
|
||||
} SuperCap_Msg_s;
|
||||
#pragma pack()
|
||||
|
||||
|
|
@ -42,12 +48,4 @@ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config);
|
|||
*/
|
||||
void SuperCapSend(SuperCapInstance *instance, uint8_t *data);
|
||||
|
||||
/**
|
||||
* @brief 发送超级电容目标功率
|
||||
*
|
||||
* @param instance 超级电容实例
|
||||
* @param power_set 超级电容目标功率
|
||||
*/
|
||||
void SuperCapSetPower(SuperCapInstance *instance, float power_set);
|
||||
|
||||
#endif // !SUPER_CAP_Hd
|
||||
|
|
|
|||
|
|
@ -6,7 +6,6 @@
|
|||
* @LastEditTime: 2022-12-05 14:15:53
|
||||
*/
|
||||
#include "vofa.h"
|
||||
#include "usbd_cdc_if.h"
|
||||
|
||||
/*VOFA浮点协议*/
|
||||
void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart )
|
||||
|
|
@ -31,5 +30,4 @@ void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart
|
|||
send_data[4 * num + 3] = 0x7f; //加上协议要求的4个尾巴
|
||||
|
||||
HAL_UART_Transmit(huart, (uint8_t *)send_data, 4 * num + 4, 100);
|
||||
//CDC_Transmit_FS((uint8_t *)send_data,4 * num + 4);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue