diff --git a/CMakeLists.txt b/CMakeLists.txt index b5c42ce..534fad9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,9 +52,9 @@ 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/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/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor + modules/led modules/master_machine modules/message_center modules/motor modules/oled modules/referee modules/remote modules/RGB modules/standard_cmd + modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa modules/auto_aim + modules/motor/DJImotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor modules/motor/ECmotor modules/motor/DMmotor application application/chassis application/cmd application/gimbal application/shoot Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config) diff --git a/Inc/main.h b/Inc/main.h index 20ac85f..3a962e4 100644 --- a/Inc/main.h +++ b/Inc/main.h @@ -31,7 +31,8 @@ extern "C" { /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ - +#include "robot.h" +#include "bsp_log.h" /* USER CODE END Includes */ /* Exported types ------------------------------------------------------------*/ diff --git a/Src/main.c b/Src/main.c index 5367de8..1c57007 100644 --- a/Src/main.c +++ b/Src/main.c @@ -35,8 +35,7 @@ /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ -#include "robot.h" -#include "bsp_log.h" + /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 61b43d8..b1f6486 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -22,12 +22,17 @@ #include "bsp_dwt.h" #include "referee_UI.h" #include "arm_math.h" +#include "user_lib.h" + +#include "vofa.h" /* 根据robot_def.h中的macro自动计算的参数 */ #define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距 #define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f) // 半轮距 #define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长 +#define P_MAX 50.0f//功率控制 单位：W + /* 底盘应用包含的模块和信息存储,底盘是单例模式,因此不需要为底盘建立单独的结构体 */ #ifdef CHASSIS_BOARD // 如果是底盘板,使用板载IMU获取底盘转动角速度 #include "can_comm.h" @@ -41,11 +46,13 @@ static Subscriber_t *chassis_sub; // 用于订阅底盘的控 #endif // !ONE_BOARD static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令 static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据 +static Chassis_Ctrl_Cmd_s Chassis_Power_Mx; +const static float CHASSIS_ACCEL_X_NUM=0.1f; +const static float CHASSIS_ACCEL_Y_NUM=0.1f; -static referee_info_t* referee_data; // 用于获取裁判系统的数据 -static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI - -static SuperCapInstance *cap; // 超级电容 +// 超级电容 +SuperCapInstance *cap; // 超级电容全局 +static uint16_t last_chassis_power_limit=0;//超级电容更新 static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back /* 用于自旋变速策略的时间变量 */ @@ -54,40 +61,46 @@ static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left righ /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */ static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘 static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅 +first_order_filter_type_t vx_filter; +first_order_filter_type_t vy_filter; +static float P_cmd; -void ChassisInit() -{ + +void ChassisInit() { // 四个轮子的参数一样,改tx_id和反转标志位即可 Motor_Init_Config_s chassis_motor_config = { - .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, + .can_init_config.can_handle = &hcan1, + .controller_param_init_config = { + .speed_PID = { + .Kp = 5.0f, + .Ki = 0.01f, + .Kd = 0.002f, + .IntegralLimit = 3000, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, + .MaxOut = 30000, + }, + .current_PID = { + .Kp = 0.0f, + .Ki = 0.0f, + .Kd = 0.0f, + .IntegralLimit = 3000, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, + .MaxOut = 15000, + }, }, - .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, + .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, + .power_limit_flag = POWER_LIMIT_ON, }, - }, - .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, + .motor_type = M3508, }; // @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改. - chassis_motor_config.can_init_config.tx_id = 1; + // 四个轮子pid分开 + //右前 + chassis_motor_config.can_init_config.tx_id = 3; chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_lf = DJIMotorInit(&chassis_motor_config); @@ -96,23 +109,35 @@ void ChassisInit() motor_rf = DJIMotorInit(&chassis_motor_config); chassis_motor_config.can_init_config.tx_id = 4; + chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_lb = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id = 3; + chassis_motor_config.can_init_config.tx_id = 1; chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_rb = DJIMotorInit(&chassis_motor_config); - 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在其他人看来是反的 - }}; + .can_config = { + .can_handle = &hcan1, + .tx_id = 0x210, + .rx_id = 0x211, + + }, + .buffer_config_pid = { + .Kp = 1.0f, + .Ki = 0, + .Kd = 0, + .MaxOut = 300, + }, + }; cap = SuperCapInit(&cap_conf); // 超级电容初始化 + //用一阶滤波代替斜波函数生成 //增大更能刹住 + first_order_filter_init(&vx_filter, 0.007f, &CHASSIS_ACCEL_X_NUM); + first_order_filter_init(&vy_filter, 0.007f, &CHASSIS_ACCEL_Y_NUM); + // 发布订阅初始化,如果为双板,则需要can comm来传递消息 #ifdef CHASSIS_BOARD Chassis_IMU_data = INS_Init(); // 底盘IMU初始化 @@ -143,46 +168,106 @@ void ChassisInit() * @brief 计算每个轮毂电机的输出,正运动学解算 * 用宏进行预替换减小开销,运动解算具体过程参考教程 */ -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; +//全向轮解算 +static void OmniCalculate() { + vt_rf = HALF_WHEEL_BASE * chassis_cmd_recv.wz + chassis_vx * 0.707f + chassis_vy * 0.707f; + vt_rb = HALF_WHEEL_BASE * chassis_cmd_recv.wz + chassis_vx * 0.707f - chassis_vy * 0.707f; + vt_lb = HALF_WHEEL_BASE * chassis_cmd_recv.wz - chassis_vx * 0.707f - chassis_vy * 0.707f; + vt_lf = HALF_WHEEL_BASE * chassis_cmd_recv.wz - chassis_vx * 0.707f + chassis_vy * 0.707f; + + vt_rf = (vt_rf / RADIUS_WHEEL) * 180 / PI * REDUCTION_RATIO_WHEEL; + vt_rb = (vt_rb / RADIUS_WHEEL) * 180 / PI * REDUCTION_RATIO_WHEEL; + vt_lb = (vt_lb / RADIUS_WHEEL) * 180 / PI * REDUCTION_RATIO_WHEEL; + vt_lf = (vt_lf / RADIUS_WHEEL) * 180 / PI * REDUCTION_RATIO_WHEEL; } +static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f}; +float input; + +float P_max = 0; + +///依据3508电机功率模型，预测电机输出功率 +static float EstimatePower(DJIMotorInstance* chassis_motor) +{ + + 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; +} /** * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值 * */ static void LimitChassisOutput() { - // 功率限制待添加 - // referee_data->PowerHeatData.chassis_power; - // referee_data->PowerHeatData.chassis_power_buffer; +// float Plimit = 1.0f; + P_cmd = motor_rf->motor_controller.motor_power_predict + + motor_rb->motor_controller.motor_power_predict + + motor_lb->motor_controller.motor_power_predict + + motor_lf->motor_controller.motor_power_predict + 3.6f; + +// if(chassis_cmd_recv.buffer_energy<50&&chassis_cmd_recv.buffer_energy>=40) Plimit=0.9f; +// else if(chassis_cmd_recv.buffer_energy<40&&chassis_cmd_recv.buffer_energy>=35) Plimit=0.75f; +// else if(chassis_cmd_recv.buffer_energy<35&&chassis_cmd_recv.buffer_energy>=30) Plimit=0.5f; +// else if(chassis_cmd_recv.buffer_energy<30&&chassis_cmd_recv.buffer_energy>=20) Plimit=0.25f; +// else if(chassis_cmd_recv.buffer_energy<20&&chassis_cmd_recv.buffer_energy>=10) Plimit=0.125f; +// else if(chassis_cmd_recv.buffer_energy<10&&chassis_cmd_recv.buffer_energy>=0) Plimit=0.05f; +// else if(chassis_cmd_recv.buffer_energy==60) Plimit=1.0f; + + if (cap->cap_msg.cap_vol>1800) + { + P_max = input + chassis_cmd_recv.buffer_supercap ; + } + else + { + P_max = input; + } + float K = P_max / P_cmd; + + motor_rf->motor_controller.motor_power_scale = K; + motor_rb->motor_controller.motor_power_scale = K; + motor_lf->motor_controller.motor_power_scale = K; + motor_lb->motor_controller.motor_power_scale = K; + + { + DJIMotorSetRef(motor_lf, vt_lf); + DJIMotorSetRef(motor_rf, vt_rf); + DJIMotorSetRef(motor_lb, vt_lb); + DJIMotorSetRef(motor_rb, vt_rb); + } - // 完成功率限制后进行电机参考输入设定 - DJIMotorSetRef(motor_lf, vt_lf); - DJIMotorSetRef(motor_rf, vt_rf); - DJIMotorSetRef(motor_lb, vt_lb); - DJIMotorSetRef(motor_rb, vt_rb); } +/** + * @brief 超电功率对缓冲功率进行闭环 + * + * + */ +static void SuperCapSetUpdate() +{ + PIDCalculate(&cap->buffer_pid, chassis_cmd_recv.buffer_energy,15);//对缓冲功率进行闭环 + input = chassis_cmd_recv.chassis_power_limit - cap->buffer_pid.Output; + LIMIT_MIN_MAX(input, 30, 130); + SuperCapSetPower(cap,input); + +} /** * @brief 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算 * 对于双板的情况,考虑增加来自底盘板IMU的数据 * */ -static void EstimateSpeed() -{ +static void EstimateSpeed() { // 根据电机速度和陀螺仪的角速度进行解算,还可以利用加速度计判断是否打滑(如果有) // chassis_feedback_data.vx vy wz = // ... } - +static chassis_mode_e last_chassis_mode; +static float rotate_speed = 70000; /* 机器人底盘控制核心任务 */ -void ChassisTask() -{ +void ChassisTask() { // 后续增加没收到消息的处理(双板的情况) // 获取新的控制信息 #ifdef ONE_BOARD @@ -192,15 +277,12 @@ void ChassisTask() chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm); #endif // CHASSIS_BOARD - if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) - { // 如果出现重要模块离线或遥控器设置为急停,让电机停止 + if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) { // 如果出现重要模块离线或遥控器设置为急停,让电机停止 DJIMotorStop(motor_lf); DJIMotorStop(motor_rf); DJIMotorStop(motor_lb); DJIMotorStop(motor_rb); - } - else - { // 正常工作 + } else { // 正常工作 DJIMotorEnable(motor_lf); DJIMotorEnable(motor_rf); DJIMotorEnable(motor_lb); @@ -208,50 +290,79 @@ 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 = -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; + 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 = 40.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle); + LIMIT_MIN_MAX(chassis_cmd_recv.wz,-35000,35000); + break; + case CHASSIS_SIDEWAYS: // 侧向,不单独设置pid,以误差角度平方为速度输出 + chassis_cmd_recv.wz = 20.0f * (chassis_cmd_recv.offset_angle - 45)* abs(chassis_cmd_recv.offset_angle - 45); + LIMIT_MIN_MAX(chassis_cmd_recv.wz,-35000,35000); + break; + case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 + if(last_chassis_mode != CHASSIS_ROTATE){ + rotate_speed = -rotate_speed; + } + chassis_cmd_recv.wz = rotate_speed; + break; + default: + break; } + last_chassis_mode = chassis_cmd_recv.chassis_mode; // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上 // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东) 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.vx = vx_filter.out; + chassis_cmd_recv.vy = vy_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_vx = (1.0f - 0.30f) * chassis_vx + 0.30f * (chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta); +// chassis_vy = (1.0f - 0.30f) * chassis_vy + 0.30f * (chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta); + + // 根据控制模式进行正运动学解算,计算底盘输出 + //MecanumCalculate(); + OmniCalculate(); + ////对缓冲功率进行闭环 + SuperCapSetUpdate(); // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值 LimitChassisOutput(); +// float vofa_send_data[2]; +// vofa_send_data[0] = motor_lb->motor_controller.speed_PID.Ref; +// vofa_send_data[1] = motor_lb->motor_controller.speed_PID.Measure; +// vofa_justfloat_output(vofa_send_data, 8, &huart1); + // 根据电机的反馈速度和IMU(如果有)计算真实速度 EstimateSpeed(); - // // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送 - // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red - // 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; + //todo: 裁判系统信息移植到消息中心发送 + // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送 + // 发送敌方方颜色id + //chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color; + // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况 + //chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit; + + chassis_feedback_data.cap_vol = cap->cap_msg.cap_vol; // 推送反馈消息 #ifdef ONE_BOARD - PubPushMessage(chassis_pub, (void *)&chassis_feedback_data); + PubPushMessage(chassis_pub, (void *) &chassis_feedback_data); #endif #ifdef CHASSIS_BOARD CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data); #endif // CHASSIS_BOARD -} \ No newline at end of file +} diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 6a989ce..a33b912 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -8,9 +8,15 @@ #include "message_center.h" #include "general_def.h" #include "dji_motor.h" +#include "auto_aim.h" +#include "super_cap.h" +#include "user_lib.h" // bsp #include "bsp_dwt.h" #include "bsp_log.h" +#include "referee_task.h" +#include "vision_transfer.h" + // 私有宏,自动将编码器转换成角度值 #define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360 @@ -19,6 +25,8 @@ /* cmd应用包含的模块实例指针和交互信息存储*/ #ifdef GIMBAL_BOARD // 对双板的兼容,条件编译 #include "can_comm.h" +#include "user_lib.h" + static CANCommInstance *cmd_can_comm; // 双板通信 #endif #ifdef ONE_BOARD @@ -28,10 +36,22 @@ 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 VT_ctrl_t *vt_data; // 图传链路下发的键鼠遥控数据 与遥控器数据格式保持一致 -static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回 -static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回 -static Vision_Send_s vision_send_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 uint8_t choose_amor_id = 0; +static float yaw_err_for_test; static Publisher_t *gimbal_cmd_pub; // 云台控制消息发布者 static Subscriber_t *gimbal_feed_sub; // 云台反馈信息订阅者 @@ -45,10 +65,14 @@ static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息 static Robot_Status_e robot_state; // 机器人整体工作状态 -void RobotCMDInit() -{ +static referee_info_t *referee_data; // 用于获取裁判系统的数据 +static uint8_t loader_flag = 0; //拨弹模式选择标志位 + +void RobotCMDInit() { rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个 - vision_recv_data = VisionInit(&huart1); // 视觉通信串口 + vision_recv_data = VisionInit(); // 视觉通信串口 + vt_data = VTRefereeInit(&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)); @@ -81,8 +105,7 @@ void RobotCMDInit() * 单圈绝对角度的范围是0~360,说明文档中有图示 * */ -static void CalcOffsetAngle() -{ +static void CalcOffsetAngle() { // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数 static float angle; angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度 @@ -103,146 +126,485 @@ static void CalcOffsetAngle() #endif } +//功能：死亡后清除小陀螺的状态 +static void death_check() { + if (referee_data->GameRobotState.current_HP <= 0 || + referee_data->GameRobotState.power_management_chassis_output == 0) { + rc_data[TEMP].key_count[KEY_PRESS][Key_E] = 0; + gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; + } +} + +//功能：等级提升弹频提高 +static void shoot_rate_improve() { + if (referee_data->GameRobotState.robot_level < 5) + shoot_cmd_send.shoot_rate = 12; + if (referee_data->GameRobotState.robot_level >= 5) + shoot_cmd_send.shoot_rate = 25; +} + +static void update_ui_data() { + ui_data.chassis_mode = chassis_cmd_send.chassis_mode; + //ui_data.gimbal_mode = gimbal_cmd_send.gimbal_mode; + ui_data.friction_mode = shoot_cmd_send.friction_mode; + //ui_data.shoot_mode = shoot_cmd_send.shoot_mode; + ui_data.lid_mode = shoot_cmd_send.lid_mode; + ui_data.heat_mode = shoot_cmd_send.heat_mode; + ui_data.aim_fire = aim_select.suggest_fire; + //ui_data.loader_mode = shoot_cmd_send.loader_mode; + + ui_data.Chassis_Power_Data.chassis_power_mx = referee_data->GameRobotState.chassis_power_limit; + ui_data.Cap_Data.cap_vol = chassis_fetch_data.cap_vol; +} + +static void auto_aim_mode() { + trajectory_cal.v0 = 26; //弹速30 + if (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) { + aim_select.suggest_fire = 0; + //未发现目标 + no_find_cnt++; + + if (no_find_cnt >= 2000) { + //gimbal_scan_flag = 1; + //auto_aim_flag = 0; + } + //else + //auto_aim_flag = 1; + } else { + //弹道解算 + no_find_cnt = 0; + auto_aim_flag = 1; + + choose_amor_id = auto_aim(&aim_select, &trajectory_cal, vision_recv_data); + + VisionSetAim(aim_select.aim_point[0], aim_select.aim_point[1], aim_select.aim_point[2]); + + float single_angle_yaw_now = gimbal_fetch_data.gimbal_imu_data.Yaw; + float diff_yaw = trajectory_cal.cmd_yaw * 180 / PI - single_angle_yaw_now; + float yaw_err = diff_yaw; + yaw_err_for_test = yaw_err; + + if (diff_yaw > 180) + diff_yaw -= 360; + else if (diff_yaw < -180) + diff_yaw += 360; + + gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle + diff_yaw; + + gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI; + + if (yaw_err <= 2) //3度 + { + aim_select.suggest_fire = 1; + } else + aim_select.suggest_fire = 0; + } +} + /** * @brief 控制输入为遥控器(调试时)的模式和控制量设置 * */ -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)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 + + } 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; + chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + shoot_cmd_send.lid_mode = LID_CLOSE; + } else if (switch_is_up(rc_data[TEMP].rc.switch_right))// 右侧开关状态[上] + { + shoot_cmd_send.lid_mode = LID_OPEN; } // 云台参数,确定云台控制数据 - if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式 + 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)) // 左侧开关状态为[中],或视觉未识别到目标,纯遥控器拨杆控制 { // 待添加,视觉会发来和目标的误差,同样将其转化为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; + + float delta_yaw_total = gimbal_cmd_send.yaw - gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; + float delta_yaw = theta_format(delta_yaw_total); + gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle + delta_yaw; + + 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) || 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; + // 左侧开关状态为[下],视觉模式 + if (switch_is_down(rc_data[TEMP].rc.switch_left)) { + auto_aim_mode(); } // 云台软件限位 - // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 - chassis_cmd_send.vx = 10.0f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向 - chassis_cmd_send.vy = 10.0f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向 + // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整，遥控器输入灵敏度 + chassis_cmd_send.vx = 0.001f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向 + chassis_cmd_send.vy = 0.001f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向 // 发射参数 if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 ; // 弹舱舵机控制,待添加servo_motor模块,开启 - else - ; // 弹舱舵机控制,待添加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; + if (rc_data[TEMP].rc.dial < -500)// + shoot_cmd_send.loader_mode = LOAD_BURSTFIRE; else - shoot_cmd_send.load_mode = LOAD_STOP; + shoot_cmd_send.loader_mode = LOAD_STOP; + + // 视觉控制发射 +// if (aim_select.suggest_fire == 1) { +// //shoot_cmd_send.friction_mode = FRICTION_ON; +// //shoot_cmd_send.shoot_mode = SHOOT_ON; +// shoot_cmd_send.load_mode = LOAD_BURSTFIRE; +// } else { +// //shoot_cmd_send.friction_mode = FRICTION_OFF; +// shoot_cmd_send.load_mode = LOAD_STOP; +// } + // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频, - shoot_cmd_send.shoot_rate = 8; + shoot_cmd_send.shoot_rate = 18; + + if (shoot_fetch_data.stalled_flag == 1) + shoot_cmd_send.loader_mode = LOAD_REVERSE; +} + +static void hand_aim_mode() { +// gimbal_cmd_send.yaw += (float) rc_data[TEMP].mouse.x / 660 * 6; // 系数待测 +// gimbal_cmd_send.pitch -= (float) rc_data[TEMP].mouse.y / 660 * 6; } /** * @brief 输入为键鼠时模式和控制量设置 * */ -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; +static void MouseKeySet() { + chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 1 - rc_data[TEMP].key[KEY_PRESS].s * 1; // 系数待测 + chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 1 - rc_data[TEMP].key[KEY_PRESS].d * 1; - gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测 - gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10; + gimbal_cmd_send.yaw -= (float) rc_data[TEMP].mouse.x / 660 * 4; // 系数待测 + gimbal_cmd_send.pitch += (float) rc_data[TEMP].mouse.y / 660 * 6; - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速 + float delta_yaw_total = gimbal_cmd_send.yaw - gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; + float delta_yaw = theta_format(delta_yaw_total); + gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle + delta_yaw; + + 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; + + aim_select.suggest_fire = 0; + if (rc_data[TEMP].mouse.press_l && (!rc_data[TEMP].mouse.press_r)) // 左键发射模式 { - 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; + if (shoot_cmd_send.friction_mode == FRICTION_ON) { + shoot_cmd_send.shoot_mode = SHOOT_ON; + if (loader_flag == 0) { + shoot_cmd_send.loader_mode = LOAD_BURSTFIRE; + } else if (loader_flag == 1) { + shoot_cmd_send.loader_mode = LOAD_3_BULLET; + } else + shoot_cmd_send.loader_mode = LOAD_1_BULLET; + } + } else if ((!rc_data[TEMP].mouse.press_l) && (!rc_data[TEMP].mouse.press_r)) { + shoot_cmd_send.loader_mode = LOAD_STOP; } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式 + if (rc_data[TEMP].mouse.press_r) // 右键自瞄模式 { - 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; + if ((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)) { + shoot_cmd_send.loader_mode = LOAD_STOP; + } else { + auto_aim_mode(); + if (aim_select.suggest_fire == 1 && rc_data[TEMP].mouse.press_l && + shoot_cmd_send.friction_mode == FRICTION_ON) { + shoot_cmd_send.shoot_mode = SHOOT_ON; + shoot_cmd_send.loader_mode = LOAD_BURSTFIRE; + } else { + shoot_cmd_send.loader_mode = LOAD_STOP; + } + } } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱 + + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键手动刷新UI { - case 0: - shoot_cmd_send.lid_mode = LID_OPEN; - break; - default: - shoot_cmd_send.lid_mode = LID_CLOSE; - break; + case 1: + MyUIInit(); + rc_data[TEMP].key_count[KEY_PRESS][Key_R]++; + break; + default: + break; + } + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_V] % 2) // V键开关红点激光 + { + 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; } 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; + case 0: + shoot_cmd_send.shoot_mode = SHOOT_OFF; + shoot_cmd_send.friction_mode = FRICTION_OFF; + break; + default: + shoot_cmd_send.shoot_mode = SHOOT_ON; + shoot_cmd_send.friction_mode = FRICTION_ON; + break; } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度 + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_B] % 2) // B键开关弹舱盖 { - 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; + case 0: + shoot_cmd_send.lid_mode = LID_CLOSE; + break; + default: + shoot_cmd_send.lid_mode = LID_OPEN; + break; } - switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量 +// switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺 +// { +// 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; +// } + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关热控 { - case 1: - - break; - - default: - - break; + case 0: + shoot_cmd_send.heat_mode = HEAT_OPEN; + break; + default: + shoot_cmd_send.heat_mode = HEAT_CLOSE; + break; } + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 2) // C键侧向 + { + case 0: + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺 + { + 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; + } + break; + case 1: + chassis_cmd_send.chassis_mode = CHASSIS_SIDEWAYS; + break; + } +// switch (rc_data[TEMP].key_count[KEY_PRESS][Key_G] % 3) // G键切换发射模式 +// { +// case 0: +// loader_flag = 0; +// break; +// case 1: +// loader_flag = 1; +// break; +// default: +// loader_flag = 3; +// break; +// } + switch (rc_data[TEMP].key[KEY_PRESS].shift) //按shift允许消耗超级电容能量 + { + case 0: + chassis_cmd_send.buffer_supercap = 5; + break; + case 1: + chassis_cmd_send.buffer_supercap = 200; + chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 15000 - rc_data[TEMP].key[KEY_PRESS].d * 15000; + break; + } + + shoot_rate_improve(); + + if (shoot_fetch_data.stalled_flag == 1) + shoot_cmd_send.loader_mode = LOAD_REVERSE; + + death_check(); } +/** + * @brief 输入为（图传链路）键鼠时模式和控制量设置 + * + */ +static void VTMouseKeySet() { + chassis_cmd_send.vy = + vt_data[TEMP].key[KEY_PRESS].w * 1 - vt_data[TEMP].key[KEY_PRESS].s * 1; // 系数待测 小 3000 3000 + chassis_cmd_send.vx = vt_data[TEMP].key[KEY_PRESS].a * 1 - vt_data[TEMP].key[KEY_PRESS].d * 1; + + gimbal_cmd_send.yaw -= (float) vt_data[TEMP].mouse.x / 660 * 4; // 系数待测 + gimbal_cmd_send.pitch += (float) vt_data[TEMP].mouse.y / 660 * 6; + + 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; + + aim_select.suggest_fire = 0; + if (vt_data[TEMP].mouse.press_l && (!vt_data[TEMP].mouse.press_r)) // 左键发射模式 + { + if (shoot_cmd_send.friction_mode == FRICTION_ON) { + shoot_cmd_send.shoot_mode = SHOOT_ON; + shoot_cmd_send.loader_mode = LOAD_BURSTFIRE; + } + } else if ((!vt_data[TEMP].mouse.press_l) && (!vt_data[TEMP].mouse.press_r)) { + shoot_cmd_send.loader_mode = LOAD_STOP; + + } + if (vt_data[TEMP].mouse.press_r) // 右键自瞄模式 + { + if ((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)) { + shoot_cmd_send.loader_mode = LOAD_STOP; + } else { + auto_aim_mode(); + if (aim_select.suggest_fire == 1 && vt_data[TEMP].mouse.press_l && + shoot_cmd_send.friction_mode == FRICTION_ON) { + shoot_cmd_send.shoot_mode = SHOOT_ON; + shoot_cmd_send.loader_mode = LOAD_BURSTFIRE; + } else { + shoot_cmd_send.loader_mode = LOAD_STOP; + } + } + } + + switch (vt_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键手动刷新UI + { + case 1: + MyUIInit(); + vt_data[TEMP].key_count[KEY_PRESS][Key_R]++; + break; + default: + break; + } + switch (vt_data[TEMP].key_count[KEY_PRESS][Key_V] % 2) // V键开关红点激光 + { + 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; + } + switch (vt_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮 + { + case 0: + shoot_cmd_send.shoot_mode = SHOOT_OFF; + shoot_cmd_send.friction_mode = FRICTION_OFF; + break; + default: + shoot_cmd_send.shoot_mode = SHOOT_ON; + shoot_cmd_send.friction_mode = FRICTION_ON; + break; + } + switch (vt_data[TEMP].key_count[KEY_PRESS][Key_B] % 2) // B键开关弹舱盖 + { + case 0: + shoot_cmd_send.lid_mode = LID_CLOSE; + break; + default: + shoot_cmd_send.lid_mode = LID_OPEN; + break; + } +// switch (vt_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺 +// { +// 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; +// } + switch (vt_data[TEMP].key_count[KEY_PRESS][Key_C] % 2) // C键侧向 + { + case 0: + switch (vt_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺 + { + 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; + } + break; + case 1: + chassis_cmd_send.chassis_mode = CHASSIS_SIDEWAYS; + break; + } + switch (vt_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关热控 + { + case 0: + shoot_cmd_send.heat_mode = HEAT_OPEN; + break; + default: + shoot_cmd_send.heat_mode = HEAT_CLOSE; + break; + } +// switch (rc_data[TEMP].key_count[KEY_PRESS][Key_G] % 3) // G键切换发射模式 +// { +// case 0: +// loader_flag = 0; +// break; +// case 1: +// loader_flag = 1; +// break; +// default: +// loader_flag = 3; +// break; +// } + switch (vt_data[TEMP].key[KEY_PRESS].shift) //按shift允许消耗超级电容能量 + { + case 0: + chassis_cmd_send.buffer_supercap = 5; + break; + case 1: + chassis_cmd_send.buffer_supercap = 200; + chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 1 - rc_data[TEMP].key[KEY_PRESS].d * 1; + break; + } + + shoot_rate_improve(); + + if (shoot_fetch_data.stalled_flag == 1) + shoot_cmd_send.loader_mode = LOAD_REVERSE; + + death_check(); +} + + /** * @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等 * 停止的阈值'300'待修改成合适的值,或改为开关控制. @@ -250,8 +612,7 @@ static void MouseKeySet() * @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现 * */ -static void EmergencyHandler() -{ +static void EmergencyHandler() { // 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正 if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断 { @@ -260,53 +621,79 @@ static void EmergencyHandler() 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; + shoot_cmd_send.loader_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; + gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; 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(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)) // 遥控器左侧开关状态为[下],遥控器控制 - RemoteControlSet(); - else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制 - MouseKeySet(); - EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 + if (robot_state != ROBOT_STOP) { + // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠 + if (switch_is_down(rc_data[TEMP].rc.switch_left) || + switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],[中],遥控器控制 + RemoteControlSet(); + else if (switch_is_up(rc_data[TEMP].rc.switch_left) && + switch_is_mid(rc_data[TEMP].rc.switch_right)) // 遥控器左侧开关状态为[上],键盘控制,遥控器右侧开关状态为[中],遥控器通道 + MouseKeySet(); + else if (switch_is_up(rc_data[TEMP].rc.switch_left) && + switch_is_up(rc_data[TEMP].rc.switch_right)) // 遥控器左侧开关状态为[上],键盘控制，遥控器右侧开关状态为[上],图传链路通道 + VTMouseKeySet(); + } // 设置视觉发送数据,还需增加加速度和角速度数据 - // VisionSetFlag(chassis_fetch_data.enemy_color,,chassis_fetch_data.bullet_speed) + VisionSetFlag(!referee_data->referee_id.Robot_Color); + + //根据裁判系统反馈确定底盘功率上限 + chassis_cmd_send.chassis_power_limit = referee_data->GameRobotState.chassis_power_limit; + //根据裁判系统反馈确定缓冲功率 + chassis_cmd_send.buffer_energy = referee_data->PowerHeatData.buffer_energy; + //根据裁判系统反馈确定枪管热量控制 + shoot_cmd_send.heat = referee_data->PowerHeatData.shooter_17mm_1_barrel_heat; + shoot_cmd_send.heat_limit = referee_data->GameRobotState.shooter_barrel_heat_limit; + shoot_cmd_send.heat_cool = referee_data->GameRobotState.shooter_barrel_cooling_value; + + if (referee_data->GameRobotState.power_management_chassis_output == 0) { + chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; + } + if (referee_data->GameRobotState.power_management_gimbal_output == 0) { + gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; + } + if (referee_data->GameRobotState.power_management_shooter_output == 0) { + shoot_cmd_send.shoot_mode = SHOOT_OFF; + } + + death_check(); // 推送消息,双板通信,视觉通信等 // 其他应用所需的控制数据在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(shoot_cmd_pub, (void *)&shoot_cmd_send); - PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send); - VisionSend(&vision_send_data); + PubPushMessage(shoot_cmd_pub, (void *) &shoot_cmd_send); + PubPushMessage(gimbal_cmd_pub, (void *) &gimbal_cmd_send); + } diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index e1a070f..0587b63 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -4,8 +4,8 @@ #include "ins_task.h" #include "message_center.h" #include "general_def.h" - #include "bmi088.h" +#include "vofa.h" static attitude_t *gimba_IMU_data; // 云台IMU数据 static DJIMotorInstance *yaw_motor, *pitch_motor; @@ -15,81 +15,110 @@ static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息 -void GimbalInit() -{ +static float gravity_current = 0; +static void LimitPitchAngleAuto() { + /** 注意电机角度与陀螺仪角度方向是否相同 + * 目前 add > 0, 向下转动 + * 电机角度值减小 + * 陀螺仪角度值增大 + **/ + float add; + float angle_set; + + add = gimbal_cmd_recv.pitch - gimba_IMU_data->Pitch; + + if(pitch_motor->measure.angle_single_round - add > PITCH_MAX_RELATIVE_ANGLE){ + if(add < 0.0f ){ + add = PITCH_MAX_ANGLE - pitch_motor->measure.angle_single_round; + } + }else if(pitch_motor->measure.angle_single_round - add < PITCH_MIN_RELATIVE_ANGLE){ + if(add > 0.0f){ + add = PITCH_MIN_RELATIVE_ANGLE - pitch_motor->measure.angle_single_round; + } + } + angle_set = gimba_IMU_data->Pitch; + DJIMotorSetRef(pitch_motor, angle_set+add); +} + +void GimbalInit() { gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // YAW Motor_Init_Config_s yaw_config = { - .can_init_config = { - .can_handle = &hcan1, - .tx_id = 1, - }, - .controller_param_init_config = { - .angle_PID = { - .Kp = 8, // 8 - .Ki = 0, - .Kd = 0, - .DeadBand = 0.1, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 100, - - .MaxOut = 500, + .can_init_config = { + .can_handle = &hcan1, + .tx_id = 1, }, - .speed_PID = { - .Kp = 50, // 50 - .Ki = 200, // 200 - .Kd = 0, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 3000, - .MaxOut = 20000, + .controller_param_init_config = { + .angle_PID = { + .Kp = 0.8f,//1.2 + .Ki = 0.0f,//0 + .Kd = 0.04f,//0.05 + .DeadBand = 0.0f, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, + .IntegralLimit = 100, + .MaxOut = 500, + }, + .speed_PID = { + .Kp = 5000, //4000 + .Ki = 100.0f, //100 + .Kd = 0.03f, //0 + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, + .IntegralLimit = 10000, + .MaxOut = 15000, + }, + .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle, + // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 + .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2], }, - .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle, - // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 - .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2], - }, - .controller_setting_init_config = { - .angle_feedback_source = OTHER_FEED, - .speed_feedback_source = OTHER_FEED, - .outer_loop_type = ANGLE_LOOP, - .close_loop_type = ANGLE_LOOP | SPEED_LOOP, - .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, - }, - .motor_type = GM6020}; + .controller_setting_init_config = { + .angle_feedback_source = OTHER_FEED, + .speed_feedback_source = OTHER_FEED, + .outer_loop_type = ANGLE_LOOP, + .close_loop_type = SPEED_LOOP | ANGLE_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, + }, + .motor_type = GM6020, + .motor_control_type = CURRENT_CONTROL + }; // PITCH Motor_Init_Config_s pitch_config = { - .can_init_config = { - .can_handle = &hcan2, - .tx_id = 2, - }, - .controller_param_init_config = { - .angle_PID = { - .Kp = 10, // 10 - .Ki = 0, - .Kd = 0, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 100, - .MaxOut = 500, + .can_init_config = { + .can_handle = &hcan2, + .tx_id = 1, }, - .speed_PID = { - .Kp = 50, // 50 - .Ki = 350, // 350 - .Kd = 0, // 0 - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 2500, - .MaxOut = 20000, + .controller_param_init_config = { + .angle_PID = { + .Kp = 1.0f, + .Ki = 0.0f, + .Kd = 0.02f, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, + .IntegralLimit = 100, + .MaxOut = 500, + }, + .speed_PID = { + .Kp = 6000.0f, + .Ki = 900.0f, + .Kd = 0.0f, // 0 + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, + .IntegralLimit = 10000, + .MaxOut = 15000, + }, + .other_angle_feedback_ptr = &gimba_IMU_data->Roll, + // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 + .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[1], + .current_feedforward_ptr = &gravity_current, }, - .other_angle_feedback_ptr = &gimba_IMU_data->Pitch, - // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 - .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]), - }, - .controller_setting_init_config = { - .angle_feedback_source = OTHER_FEED, - .speed_feedback_source = OTHER_FEED, - .outer_loop_type = ANGLE_LOOP, - .close_loop_type = SPEED_LOOP | ANGLE_LOOP, - .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, - }, - .motor_type = GM6020, + .controller_setting_init_config = { + .angle_feedback_source = OTHER_FEED, + .speed_feedback_source = OTHER_FEED, + .outer_loop_type = ANGLE_LOOP, + .close_loop_type = SPEED_LOOP | ANGLE_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_REVERSE, + .feedforward_flag = CURRENT_FEEDFORWARD, + .feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE + }, + .motor_type = GM6020, + .motor_control_type = CURRENT_CONTROL }; // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 yaw_motor = DJIMotorInit(&yaw_config); @@ -100,55 +129,72 @@ void GimbalInit() } /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ -void GimbalTask() -{ +void GimbalTask() { + //红点激光 + //HAL_GPIO_WritePin(GPIOC,GPIO_PIN_8,GPIO_PIN_SET); // 获取云台控制数据 // 后续增加未收到数据的处理 SubGetMessage(gimbal_sub, &gimbal_cmd_recv); // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘 // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref - switch (gimbal_cmd_recv.gimbal_mode) - { - // 停止 - case GIMBAL_ZERO_FORCE: - DJIMotorStop(yaw_motor); - DJIMotorStop(pitch_motor); - break; - // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 - case GIMBAL_GYRO_MODE: // 后续只保留此模式 - DJIMotorEnable(yaw_motor); - DJIMotorEnable(pitch_motor); - DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 - DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); - break; - // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关 - case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) - DJIMotorEnable(yaw_motor); - DJIMotorEnable(pitch_motor); - DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 - DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); - break; - default: - break; + switch (gimbal_cmd_recv.gimbal_mode) { + // 停止 + case GIMBAL_ZERO_FORCE: + DJIMotorStop(yaw_motor); + DJIMotorStop(pitch_motor); + break; + // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 + case GIMBAL_GYRO_MODE: // 后续只保留此模式 + DJIMotorEnable(yaw_motor); + DJIMotorEnable(pitch_motor); + DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); + DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); + DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); + DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); + DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 + DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); + //LimitPitchAngleAuto(); + break; + // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关 + case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) + DJIMotorEnable(yaw_motor); + DJIMotorEnable(pitch_motor); + DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); + DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); + DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); + DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); + DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 + DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); + break; + default: + break; } // 在合适的地方添加pitch重力补偿前馈力矩 // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩 // ... + float theta = - gimba_IMU_data->Roll/180*PI; + gravity_current = (5000)*arm_cos_f32(theta); + +// float vofa_send_data[4]; +// vofa_send_data[0] = pitch_motor->motor_controller.speed_PID.Ref; +// vofa_send_data[1] = pitch_motor->motor_controller.speed_PID.Measure; +// vofa_send_data[2] = pitch_motor->motor_controller.angle_PID.Ref; +// vofa_send_data[3] = pitch_motor->motor_controller.angle_PID.Measure; +// vofa_justfloat_output(vofa_send_data, 16, &huart1); +// float vofa_send_data[4]; +// vofa_send_data[0] = yaw_motor->motor_controller.speed_PID.Ref; +// vofa_send_data[1] = yaw_motor->motor_controller.speed_PID.Measure; +// vofa_send_data[2] = yaw_motor->motor_controller.angle_PID.Ref; +// vofa_send_data[3] = yaw_motor->motor_controller.angle_PID.Measure; +// vofa_justfloat_output(vofa_send_data, 16, &huart1); + // 设置反馈数据,主要是imu和yaw的ecd gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round; // 推送消息 - PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data); + PubPushMessage(gimbal_pub, (void *) &gimbal_feedback_data); } \ No newline at end of file diff --git a/application/robot.c b/application/robot.c index 2b5f8cb..eedfa6d 100644 --- a/application/robot.c +++ b/application/robot.c @@ -31,6 +31,7 @@ void RobotInit() #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) RobotCMDInit(); + GimbalInit(); ShootInit(); #endif @@ -49,6 +50,7 @@ void RobotTask() { #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) RobotCMDTask(); + //暂时注释云台和发射任务 调试底盘 GimbalTask(); ShootTask(); #endif diff --git a/application/robot.h b/application/robot.h index 36f9809..bc660d7 100644 --- a/application/robot.h +++ b/application/robot.h @@ -1,5 +1,8 @@ #ifndef ROBOT_H #define ROBOT_H +#ifdef __cplusplus +extern "C"{ +#endif /* Robot利用robot_def.h中的宏对不同的机器人进行了大量的兼容,同时兼容了两个开发板(云台板和底盘板)的配置 */ @@ -15,4 +18,8 @@ void RobotInit(); */ void RobotTask(); +#ifdef __cplusplus +} +#endif + #endif \ No newline at end of file diff --git a/application/robot_def.h b/application/robot_def.h index 3457352..5ad671b 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -26,21 +26,25 @@ /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ // 云台参数 -#define YAW_CHASSIS_ALIGN_ECD 2711 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 +#define YAW_CHASSIS_ALIGN_ECD 2053 // 小 1443 大2053 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 #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 PITCH_HORIZON_ECD 4422 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 +#define PITCH_MAX_ANGLE 25 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) +#define PITCH_MIN_ANGLE -24 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) + +#define PITCH_MAX_RELATIVE_ANGLE 123 // 云台相对底盘最大角度 +#define PITCH_MIN_RELATIVE_ANGLE 80 // 云台相对底盘最小角度 + // 发射参数 -#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 -#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f -#define NUM_PER_CIRCLE 10 // 拨盘一圈的装载量 -// 机器人底盘修改的参数,单位为mm(毫米) -#define WHEEL_BASE 350 // 纵向轴距(前进后退方向) -#define TRACK_WIDTH 300 // 横向轮距(左右平移方向) +#define ONE_BULLET_DELTA_ANGLE 1620 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 +#define REDUCTION_RATIO_LOADER 36.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f 2006-36.0f +#define NUM_PER_CIRCLE 8 // 拨盘一圈的装载量 +// 机器人底盘修改的参数,单位为m(米) +#define WHEEL_BASE 0.1f // 纵向轴距(前进后退方向) +#define TRACK_WIDTH 0.1f // 横向轮距(左右平移方向) #define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0 #define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0 -#define RADIUS_WHEEL 60 // 轮子半径 +#define RADIUS_WHEEL 0.08f // 轮子半径 #define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换 #define GYRO2GIMBAL_DIR_YAW 1 // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反 @@ -86,6 +90,7 @@ typedef enum CHASSIS_ROTATE, // 小陀螺模式 CHASSIS_NO_FOLLOW, // 不跟随，允许全向平移 CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式，底盘叠加角度环控制 + CHASSIS_SIDEWAYS, // 侧向 } chassis_mode_e; // 云台模式设置 @@ -110,7 +115,7 @@ typedef enum typedef enum { - LID_OPEN = 0, // 弹舱盖打开 + LID_OPEN , // 弹舱盖打开 LID_CLOSE, // 弹舱盖关闭 } lid_mode_e; @@ -122,13 +127,22 @@ typedef enum LOAD_3_BULLET, // 三发 LOAD_BURSTFIRE, // 连发 } loader_mode_e; - +typedef enum +{ + HEAT_OPEN , // 热控打开 + HEAT_CLOSE, // 热控关闭 +}heat_mode_e; // 功率限制,从裁判系统获取,是否有必要保留? typedef struct -{ // 功率控制 - float chassis_power_mx; +{ + uint16_t chassis_power_mx; // 功率限制 } Chassis_Power_Data_s; +// 电容信息 +typedef struct +{ + int16_t cap_vol; +} Cap_Data_s; /* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */ /** * @brief 对于双板情况,遥控器和pc在云台,裁判系统在底盘 @@ -144,6 +158,11 @@ typedef struct float offset_angle; // 底盘和归中位置的夹角 chassis_mode_e chassis_mode; int chassis_speed_buff; + + uint16_t chassis_power_limit; + uint16_t buffer_energy; + uint16_t buffer_supercap; + uint8_t chassic_flag; // UI部分 // ... @@ -163,12 +182,16 @@ typedef struct typedef struct { shoot_mode_e shoot_mode; - loader_mode_e load_mode; + loader_mode_e loader_mode; lid_mode_e lid_mode; friction_mode_e friction_mode; + heat_mode_e heat_mode; Bullet_Speed_e bullet_speed; // 弹速枚举 - uint8_t rest_heat; float shoot_rate; // 连续发射的射频,unit per s,发/秒 + + uint16_t heat; // 实时热量 + uint16_t heat_limit; // 热量上限 + uint16_t heat_cool; // 热量每秒冷却值 } Shoot_Ctrl_Cmd_s; /* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/ @@ -187,25 +210,26 @@ typedef struct // float real_vy; // float real_wz; - uint8_t rest_heat; // 剩余枪口热量 Bullet_Speed_e bullet_speed; // 弹速限制 - Enemy_Color_e enemy_color; // 0 for blue, 1 for red - + Enemy_Color_e enemy_color; // 0 for red, 1 for blue + uint16_t chassis_power_mx; + int16_t cap_vol; } Chassis_Upload_Data_s; typedef struct { attitude_t gimbal_imu_data; - uint16_t yaw_motor_single_round_angle; + float yaw_motor_single_round_angle; } Gimbal_Upload_Data_s; typedef struct { // code to go here // ... + uint8_t stalled_flag; //堵转标志 } Shoot_Upload_Data_s; #pragma pack() // 开启字节对齐,结束前面的#pragma pack(1) -#endif // !ROBOT_DEF_H \ No newline at end of file +#endif // !ROBOT_DEF_H diff --git a/application/robot_task.h b/application/robot_task.h index 252e77b..f0032be 100644 --- a/application/robot_task.h +++ b/application/robot_task.h @@ -17,6 +17,8 @@ #include "bsp_log.h" + + osThreadId insTaskHandle; osThreadId robotTaskHandle; osThreadId motorTaskHandle; @@ -119,9 +121,9 @@ __attribute__((noreturn)) void StartROBOTTASK(void const *argument) robot_start = DWT_GetTimeline_ms(); RobotTask(); robot_dt = DWT_GetTimeline_ms() - robot_start; - if (robot_dt > 5) + if (robot_dt > 1) LOGERROR("[freeRTOS] ROBOT core Task is being DELAY! dt = [%f]", &robot_dt); - osDelay(5); + osDelay(1); } } diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index e19af5c..a03250c 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -5,10 +5,11 @@ #include "message_center.h" #include "bsp_dwt.h" #include "general_def.h" +#include "servo_motor.h" /* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */ static DJIMotorInstance *friction_l, *friction_r, *loader; // 拨盘电机 -// static servo_instance *lid; 需要增加弹舱盖 +static ServoInstance *lid; static Publisher_t *shoot_pub; static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自cmd的发射控制信息 @@ -16,105 +17,166 @@ static Subscriber_t *shoot_sub; static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息 // dwt定时,计算冷却用 -static float hibernate_time = 0, dead_time = 0; +static float hibernate_time = 0,last_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, + .can_init_config = { + .can_handle = &hcan2, }, - .current_PID = { - .Kp = 0, // 0.7 - .Ki = 0, // 0.1 - .Kd = 0, - .Improve = PID_Integral_Limit, - .IntegralLimit = 10000, - .MaxOut = 15000, + .controller_param_init_config = { + .speed_PID = { + .Kp = 1.5f, + .Ki = 0.2f, + .Kd = 0, + .Improve = PID_Integral_Limit, + .IntegralLimit = 10000, + .MaxOut = 15000, + }, + .current_PID = { + .Kp = 0, + .Ki = 0, + .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, + .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}; + friction_config.can_init_config.tx_id = 2, + friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL; friction_l = DJIMotorInit(&friction_config); - friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行 + friction_config.can_init_config.tx_id = 3; // 右摩擦轮,改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, + .can_init_config = { + .can_handle = &hcan2, + .tx_id = 1 }, - .speed_PID = { - .Kp = 0, // 10 - .Ki = 0, // 1 - .Kd = 0, - .Improve = PID_Integral_Limit, - .IntegralLimit = 5000, - .MaxOut = 5000, + .controller_param_init_config = { + .angle_PID = { + // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降 + .Kp = 500, + .Ki = 100, + .Kd = 0, + .MaxOut = 5000, + }, + .speed_PID = { + .Kp = 2, // 3 + .Ki = 0, // 1 + .Kd = 0, + .Improve = PID_Integral_Limit, + .IntegralLimit = 5000, + .MaxOut = 10000, + }, + .current_PID = { + .Kp = 0, // 0.7 + .Ki = 0, // 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 = SPEED_LOOP | ANGLE_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向 + //.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE, }, - }, - .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 + .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)); + + Servo_Init_Config_s lid_config = { + .htim = &htim1, + .Channel = TIM_CHANNEL_1, + .Servo_type = Servo180, + .Servo_Angle_Type = Free_Angle_mode, + }; + + lid = ServoInit(&lid_config); + +// SPI_Init_Config_s rgb_config = { +// .spi_handle = ; +// .GPIOx = GPIOx; +// .cs_pin = cs_pin; +// .spi_work_mode = spi_work_mode; +// .callback = callback; +// .id = 1; +// } } +//卡弹检测 +void stalled_detect() +{ + static float last_detect_time = 0,detect_time = 0; + static float last_total_angle = 0; + static uint8_t stalled_cnt = 0; + + detect_time = DWT_GetTimeline_ms(); + + //last_detect_time + 200 > detect_time + if(detect_time - last_detect_time < 200) // 200ms 检测一次 + return; +// reference_angle = (detect_time - last_detect_time) * loader->motor_controller.speed_PID.Ref * 1e-3f; +// real_angle = loader->measure.total_angle - last_total_angle; + if(shoot_cmd_recv.loader_mode == LOAD_BURSTFIRE) + { + //if(real_angle < reference_angle * 0.2f) + if(abs(loader->measure.real_current)>=9500) + { + //stalled_cnt ++; + shoot_feedback_data.stalled_flag = 1; + + } + last_detect_time = DWT_GetTimeline_ms(); + } +// last_detect_time = DWT_GetTimeline_ms(); +// last_total_angle = loader->measure.total_angle; +} + +// 热量控制 +//void Heat_control() +//{ +// if(shoot_cmd_recv.heat>=(0.9*shoot_cmd_recv.heat_limit)) +// { +// DJIMotorStop(loader); +// } +// else +// { +// DJIMotorEnable(loader); +// } +//} + /* 机器人发射机构控制核心任务 */ void ShootTask() { - // 从cmd获取控制数据 + //从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); + DJIMotorSetRef(friction_l, 0); + DJIMotorSetRef(friction_r, 0); DJIMotorStop(loader); } else // 恢复运行 @@ -126,71 +188,76 @@ void ShootTask() // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可 // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发) - // if (hibernate_time + dead_time > DWT_GetTimeline_ms()) - // return; + if (hibernate_time + dead_time > DWT_GetTimeline_ms()) + return; // 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换 - switch (shoot_cmd_recv.load_mode) + switch (shoot_cmd_recv.loader_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) - ; // 未知模式,停止运行,检查指针越界,内存溢出等问题 + // 停止拨盘 + 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 = 1000; // 完成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); + DJIMotorSetRef(loader, 8 * 360 * REDUCTION_RATIO_LOADER / 8); // 固定速度反转 + hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 + dead_time = 100; // 翻转500ms + shoot_feedback_data.stalled_flag = 0 ; //清除堵转标志 + // ... + 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; - } +// 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, 39000); + DJIMotorSetRef(friction_r, 39000); +// break; +// } } else // 关闭摩擦轮 { @@ -201,13 +268,33 @@ void ShootTask() // 开关弹舱盖 if (shoot_cmd_recv.lid_mode == LID_CLOSE) { - //... + Servo_Motor_FreeAngle_Set(lid,115);// 小107 大115 } else if (shoot_cmd_recv.lid_mode == LID_OPEN) { - //... + Servo_Motor_FreeAngle_Set(lid,10); } + // 开关热控 + if(shoot_cmd_recv.heat_mode == HEAT_OPEN) + { + if(shoot_cmd_recv.heat>=(0.85*shoot_cmd_recv.heat_limit)) + { + DJIMotorStop(loader); + } + else + { + DJIMotorEnable(loader); + } + } + else if(shoot_cmd_recv.heat_mode == HEAT_CLOSE) { + DJIMotorEnable(loader); + } + //卡弹检测 + stalled_detect(); + // 热量控制 + //Heat_control(); + // 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈 PubPushMessage(shoot_pub, (void *)&shoot_feedback_data); } \ No newline at end of file diff --git a/bsp/usb/bsp_usb.c b/bsp/usb/bsp_usb.c index 9c4ad2f..045fcb5 100644 --- a/bsp/usb/bsp_usb.c +++ b/bsp/usb/bsp_usb.c @@ -27,5 +27,5 @@ uint8_t *USBInit(USB_Init_Config_s usb_conf) void USBTransmit(uint8_t *buffer, uint16_t len) { - CDC_Transmit_FS(buffer, len); // 发送 + //CDC_Transmit_FS(buffer, len); // 发送 } diff --git a/modules/algorithm/crc16.c b/modules/algorithm/crc16.c index 2833689..b9ff7f7 100644 --- a/modules/algorithm/crc16.c +++ b/modules/algorithm/crc16.c @@ -1,7 +1,29 @@ #include "crc16.h" static uint8_t crc_tab16_init = 0; -static uint16_t crc_tab16[256]; +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}; /* * uint16_t crc_16( const unsigned char *input_str, size_t num_bytes ); @@ -11,21 +33,29 @@ 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; +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; } /* @@ -36,8 +66,7 @@ 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; @@ -48,10 +77,9 @@ 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; } @@ -62,11 +90,10 @@ 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]; } /* @@ -77,18 +104,15 @@ 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 @@ -99,3 +123,12 @@ 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]); +} diff --git a/modules/algorithm/crc16.h b/modules/algorithm/crc16.h index 7baffd8..cd587f2 100644 --- a/modules/algorithm/crc16.h +++ b/modules/algorithm/crc16.h @@ -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 diff --git a/modules/algorithm/user_lib.c b/modules/algorithm/user_lib.c index 5029109..a3fc9b1 100644 --- a/modules/algorithm/user_lib.c +++ b/modules/algorithm/user_lib.c @@ -212,3 +212,63 @@ 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; +} + +/** + * @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; +} diff --git a/modules/algorithm/user_lib.h b/modules/algorithm/user_lib.h index 9a15097..f4c9c9d 100644 --- a/modules/algorithm/user_lib.h +++ b/modules/algorithm/user_lib.h @@ -53,6 +53,13 @@ void MatInit(mat *m, uint8_t row, uint8_t col); #ifndef PI #define PI 3.14159265354f #endif +typedef struct +{ + float input; //输入数据 + float out; //滤波输出的数据 + float num[1]; //滤波参数 + float frame_period; //滤波的时间间隔 单位 s +} first_order_filter_type_t; #define VAL_LIMIT(val, min, max) \ do \ @@ -120,6 +127,10 @@ 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) diff --git a/modules/auto_aim/auto_aim.c b/modules/auto_aim/auto_aim.c new file mode 100644 index 0000000..3b07ff0 --- /dev/null +++ b/modules/auto_aim/auto_aim.c @@ -0,0 +1,302 @@ +// +// 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]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; + } + + + 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.025;//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; +} diff --git a/modules/auto_aim/auto_aim.h b/modules/auto_aim/auto_aim.h new file mode 100644 index 0000000..a73424c --- /dev/null +++ b/modules/auto_aim/auto_aim.h @@ -0,0 +1,77 @@ +// +// 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 diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index ec084b6..5c635b8 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -20,13 +20,19 @@ #include "general_def.h" #include "master_process.h" +#include "vofa.h" + static INS_t INS; static IMU_Param_t IMU_Param; static PIDInstance TempCtrl = {0}; -const float xb[3] = {1, 0, 0}; +//const float xb[3] = {1, 0, 0}; +//const float yb[3] = {0, 1, 0}; +//const float zb[3] = {0, 0, 1}; + +const float xb[3] = {0, 0, 1}; const float yb[3] = {0, 1, 0}; -const float zb[3] = {0, 0, 1}; +const float zb[3] = {-1, 0, 0}; // 用于获取两次采样之间的时间间隔 static uint32_t INS_DWT_Count = 0; @@ -60,9 +66,9 @@ static void InitQuaternion(float *init_q4) for (uint8_t i = 0; i < 100; ++i) { BMI088_Read(&BMI088); - acc_init[X] += BMI088.Accel[X]; + acc_init[X] += -BMI088.Accel[2]; acc_init[Y] += BMI088.Accel[Y]; - acc_init[Z] += BMI088.Accel[Z]; + acc_init[Z] += BMI088.Accel[0]; DWT_Delay(0.001); } for (uint8_t i = 0; i < 3; ++i) @@ -72,6 +78,9 @@ static void InitQuaternion(float *init_q4) float angle = acosf(Dot3d(acc_init, gravity_norm)); Cross3d(acc_init, gravity_norm, axis_rot); Norm3d(axis_rot); + + //q = cos (a/2) + i(x * sin(a/2)) + j(y * sin(a/2)) + k(z * sin(a/2)) + //其中a表示旋转角度，(x,y,z)表示旋转轴 计算由实际重力加速度方向(0,0,1)到当前加速度方向的旋转 init_q4[0] = cosf(angle / 2.0f); for (uint8_t i = 0; i < 2; ++i) init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量) @@ -97,7 +106,7 @@ attitude_t *INS_Init(void) IMU_Param.flag = 1; float init_quaternion[4] = {0}; - InitQuaternion(init_quaternion); + InitQuaternion(init_quaternion);//计算由实际重力加速度方向(0,0,1)到当前加速度方向的旋转 IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0); // imu heat init PID_Init_Config_s config = {.MaxOut = 2000, @@ -129,12 +138,12 @@ void INS_Task(void) { BMI088_Read(&BMI088); - INS.Accel[X] = BMI088.Accel[X]; - INS.Accel[Y] = BMI088.Accel[Y]; - INS.Accel[Z] = BMI088.Accel[Z]; - INS.Gyro[X] = BMI088.Gyro[X]; - INS.Gyro[Y] = BMI088.Gyro[Y]; - INS.Gyro[Z] = BMI088.Gyro[Z]; + INS.Accel[X] = -BMI088.Accel[2]; + INS.Accel[Y] = BMI088.Accel[1]; + INS.Accel[Z] = BMI088.Accel[0]; + INS.Gyro[X] = -BMI088.Gyro[2]; + INS.Gyro[Y] = BMI088.Gyro[1]; + INS.Gyro[Z] = BMI088.Gyro[0]; // demo function,用于修正安装误差,可以不管,本demo暂时没用 IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel); @@ -162,12 +171,36 @@ void INS_Task(void) } BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n + INS.Yaw = QEKF_INS.Yaw; INS.Pitch = QEKF_INS.Pitch; INS.Roll = QEKF_INS.Roll; + +// // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺) +// if (INS.Yaw - INS.YawAngleLast > 180.0f) +// { +// INS.YawRoundCount--; +// } +// else if (INS.Yaw - INS.YawAngleLast < -180.0f) +// { +// INS.YawRoundCount++; +// } +// INS.YawTotalAngle = 360.0f * INS.YawRoundCount + INS.Yaw; INS.YawTotalAngle = QEKF_INS.YawTotalAngle; - VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll); +// float vofa_send_data[9]; +// vofa_send_data[0]=INS.Yaw; +// vofa_send_data[1]=INS.Pitch; +// vofa_send_data[2]=INS.Roll; +// vofa_send_data[3]=INS.Gyro[X]; +// vofa_send_data[4]=INS.Gyro[Y]; +// vofa_send_data[5]=INS.Gyro[Z]; +// vofa_send_data[6]=BMI088.Accel[X]; +// vofa_send_data[7]=BMI088.Accel[Y]; +// vofa_send_data[8]=BMI088.Accel[Z]; +// vofa_justfloat_output(vofa_send_data,36,&huart1); + + VisionSetAltitude(INS.Yaw, INS.Pitch); } // temperature control diff --git a/modules/imu/ins_task.h b/modules/imu/ins_task.h index 4acac19..f42decf 100644 --- a/modules/imu/ins_task.h +++ b/modules/imu/ins_task.h @@ -23,6 +23,10 @@ #define Y 1 #define Z 2 +//#define X 2 +//#define Y 1 +//#define Z 0 + #define INS_TASK_PERIOD 1 typedef struct @@ -34,6 +38,7 @@ typedef struct float Pitch; float Yaw; float YawTotalAngle; + } attitude_t; // 最终解算得到的角度,以及yaw转动的总角度(方便多圈控制) typedef struct @@ -63,6 +68,9 @@ typedef struct float Yaw; float YawTotalAngle; + float YawAngleLast; + float YawRoundCount; + uint8_t init; } INS_t; diff --git a/modules/master_machine/master_process.c b/modules/master_machine/master_process.c index 057c8fd..d7b22e9 100644 --- a/modules/master_machine/master_process.c +++ b/modules/master_machine/master_process.c @@ -13,25 +13,42 @@ #include "daemon.h" #include "bsp_log.h" #include "robot_def.h" +#include "crc16.h" -static Vision_Recv_s recv_data; -static Vision_Send_s send_data; +static RecievePacket_t recv_data; +static SendPacket_t send_data; static DaemonInstance *vision_daemon_instance; -void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed) +//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) { - send_data.enemy_color = enemy_color; - send_data.work_mode = work_mode; - send_data.bullet_speed = bullet_speed; + send_data.detect_color = enemy_color; + send_data.reserved = 0; } -void VisionSetAltitude(float yaw, float pitch, float roll) +//void VisionSetAltitude(float yaw, float pitch) +//{ +// send_data.yaw = yaw; +// send_data.pitch = pitch; +//} +void VisionSetAltitude(float yaw, float pitch) { 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()导致的死锁,使得无法 @@ -113,19 +130,26 @@ 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; +// 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)); + } + } } /* 视觉通信初始化 */ -Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle) +RecievePacket_t *VisionInit(void) { - UNUSED(_handle); // 仅为了消除警告 +// UNUSED(_handle); // 仅为了消除警告 USB_Init_Config_s conf = {.rx_cbk = DecodeVision}; vis_recv_buff = USBInit(conf); @@ -142,14 +166,23 @@ Vision_Recv_s *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 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)); } #endif // VISION_USE_VCP diff --git a/modules/master_machine/master_process.h b/modules/master_machine/master_process.h index e8668be..b401469 100644 --- a/modules/master_machine/master_process.h +++ b/modules/master_machine/master_process.h @@ -8,77 +8,103 @@ #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, +typedef enum { +// COLOR_NONE = 0, +// COLOR_BLUE = 1, +// COLOR_RED = 2, + ENEMY_COLOR_RED = 0, + ENEMY_COLOR_BLUE = 1, } 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 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 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() /** @@ -86,7 +112,8 @@ typedef struct * * @param handle 用于和视觉通信的串口handle(C板上一般为USART1,丝印为USART2,4pin) */ -Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle); +//Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle); +RecievePacket_t *VisionInit(void); /** * @brief 发送视觉数据 @@ -101,7 +128,8 @@ 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, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed); +void VisionSetFlag(Enemy_Color_e enemy_color); /** * @brief 设置发送数据的姿态部分 @@ -109,6 +137,8 @@ void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Spee * @param yaw * @param pitch */ -void VisionSetAltitude(float yaw, float pitch, float roll); +//void VisionSetAltitude(float yaw, float pitch, float roll); +void VisionSetAltitude(float yaw, float pitch); +void VisionSetAim(float aim_x, float aim_y,float aim_z); #endif // !MASTER_PROCESS_H \ No newline at end of file diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index e9345cc..0895c0d 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -2,6 +2,7 @@ #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()进行 */ @@ -19,13 +20,27 @@ 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[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}}, +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}} }; /** @@ -38,78 +53,88 @@ 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分组 - { - 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) + switch (motor->motor_type) { + case M2006: + case M3508: + if (motor_id < 4) // 根据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); + 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; } - } - break; - 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并设置分组发送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; - 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; + // 检查是否发生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; - 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) + case GM6020: + if(motor->motor_control_type == CURRENT_CONTROL) //6020电流控制帧id 0x1fe 0x2fe { - 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); + 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; + } } - } - 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."); // 其他电机不应该在这里注册 + 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."); // 其他电机不应该在这里注册 } } @@ -119,12 +144,11 @@ 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); @@ -132,12 +156,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°,自己画个图就清楚计算过程了 @@ -148,22 +172,21 @@ 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); @@ -185,9 +208,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 = 100, // 20ms未收到数据则丢失 }; instance->daemon = DaemonRegister(&daemon_config); @@ -197,8 +220,7 @@ 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) @@ -207,31 +229,38 @@ 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; } -// 为所有电机实例计算三环PID,发送控制报文 -void DJIMotorControl() +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() { // 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性 uint8_t group, num; // 电机组号和组内编号 int16_t set; // 电机控制CAN发送设定值 @@ -242,8 +271,7 @@ 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; @@ -254,8 +282,7 @@ 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 @@ -265,15 +292,21 @@ 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; if (motor_setting->speed_feedback_source == OTHER_FEED) pid_measure = *motor_controller->other_speed_feedback_ptr; else // MOTOR_FEED - pid_measure = measure->speed_aps; + { + if(motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE) + pid_measure = - measure->speed_aps; + else //NORMAL + pid_measure = measure->speed_aps; + } + // 更新pid_ref进入下一个环 pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref); } @@ -281,22 +314,54 @@ 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_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref); + if (motor_setting->close_loop_type & CURRENT_LOOP) { + //现在电调都有内置电流环，无需pid计算 + //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) @@ -304,10 +369,8 @@ void DJIMotorControl() } // 遍历flag,检查是否要发送这一帧报文 - for (size_t i = 0; i < 6; ++i) - { - if (sender_enable_flag[i]) - { + for (size_t i = 0; i < 10; ++i) { + if (sender_enable_flag[i]) { CANTransmit(&sender_assignment[i], 1); } } diff --git a/modules/motor/DJImotor/dji_motor.h b/modules/motor/DJImotor/dji_motor.h index bd07c1a..1130754 100644 --- a/modules/motor/DJImotor/dji_motor.h +++ b/modules/motor/DJImotor/dji_motor.h @@ -58,6 +58,7 @@ 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; diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c new file mode 100644 index 0000000..7cd2acc --- /dev/null +++ b/modules/motor/DMmotor/dmmotor.c @@ -0,0 +1,321 @@ +#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; +} + +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); + //0 失能 1 使能 8 超压 9 欠压 A 过流 B MOS过压 C 电机线圈过温 D 通信丢失 E 过载 + tmp = (uint8_t)(rxbuff[0]>>4); + measure->state = tmp; + if(measure->state){ + 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); + measure->angle_single_round = (measure->position); + + 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 > PI) //3.1425926 + measure->total_round--; + else if (measure->position - measure->last_position < -PI) + measure->total_round++; + + //measure->total_angle = measure->total_round * (DM_P_MAX * DEGREE_2_RAD) + measure->angle_single_round; + measure->total_angle = measure->total_round * (DM_P_MAX*2) + 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); + + DMMotorEnable(motor); + DMMotorSetMode(DM_CMD_MOTOR_MODE, motor); + LOGWARNING("[dm_motor] Tring to restart motor, can bus [%d] , id [%d]\"", can_bus, motor->motor_can_instance->tx_id); +} +void DMMotorCaliEncoder(DMMotorInstance *motor) +{ + DMMotorSetMode(DM_CMD_RESET_MODE, motor); + DWT_Delay(0.1); + + DMMotorSetMode(DM_CMD_ZERO_POSITION, motor); + DWT_Delay(0.1); + + DMMotorSetMode(DM_CMD_MOTOR_MODE, 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; + + motor->motor_controller.current_feedforward_ptr = config->controller_param_init_config.current_feedforward_ptr; + motor->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_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->position; // 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->feedforward_flag & CURRENT_FEEDFORWARD) + pid_ref += *motor_controller->current_feedforward_ptr; + + 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); + osDelay(5); + } +} + + +void DMMotorControl(){ + DMMotorInstance *motor; + Motor_Controller_s *motor_controller; // 电机控制器 + DM_Motor_Measure_s *measure; + Motor_Control_Setting_s *setting; + //CANInstance *motor_can = motor->motor_can_instance; + //uint16_t tmp; + DMMotor_Send_s motor_send_mailbox; + float pid_ref, set, pid_measure; + + for (size_t i = 0; i < idx; ++i) + { + motor = dm_motor_instance[i]; + setting = &motor->motor_settings; + motor_controller = &motor->motor_controller; + measure = &motor->measure; + + 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->position; // 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->feedforward_flag & CURRENT_FEEDFORWARD) + pid_ref += *motor_controller->current_feedforward_ptr; + + 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); + } + +} diff --git a/modules/motor/DMmotor/dmmotor.h b/modules/motor/DMmotor/dmmotor.h new file mode 100644 index 0000000..239aa79 --- /dev/null +++ b/modules/motor/DMmotor/dmmotor.h @@ -0,0 +1,76 @@ +#ifndef DM4310_H +#define DM4310_H +#include +#include "bsp_can.h" +#include "controller.h" +#include "motor_def.h" +#include "daemon.h" + +#define DM_MOTOR_CNT 6 + +#define DM_P_MIN (-12.56637)//(-3.1415926f) +#define DM_P_MAX 12.56637//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 DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor); +void DMMotorStop(DMMotorInstance *motor); +void DMMotorCaliEncoder(DMMotorInstance *motor); +void DMMotorControlInit(); +void DMMotorControl(); +#endif // !DMMOTOR \ No newline at end of file diff --git a/modules/motor/ECmotor/ECA8210.c b/modules/motor/ECmotor/ECA8210.c new file mode 100644 index 0000000..47ad42d --- /dev/null +++ b/modules/motor/ECmotor/ECA8210.c @@ -0,0 +1,179 @@ +// +// Created by SJQ on 2023/12/11. +// + +#include "ECA8210.h" + +ECMotor_Report_t ECMotor_Report; +ECMotor_SpdCMD_t ECMotor_SpdCMD; +ECMotor_PosCMD_t ECMotor_PosCMD; +static uint8_t idx; +static ECMotorInstance *ecmotor_instance[EC_MOTOR_MX_CNT] = {NULL}; + +static void swap ( uint8_t *a ,uint8_t *b ) +{ + uint8_t temp = *a; + *a = *b; + *b = temp; +} +/** + * @brief 电机反馈报文解析 + * + * @param _instance 发生中断的caninstance + */ +static void ECMotorDecode(CANInstance *_instance) +{ + ECMotorInstance *motor = (ECMotorInstance *)_instance->id; // 通过caninstance保存的father id获取对应的motorinstance + ECMotor_Measure_t *measure = &motor->measure; + uint8_t *rx_buff = _instance->rx_buff; + + swap(rx_buff+1,rx_buff+2);// 输出端位置 高位在前 + + memcpy(&ECMotor_Report,rx_buff,sizeof(ECMotor_Report)); + //高位在前 这两个12位数据需要做特殊处理 + ECMotor_Report.speed_raw = (uint16_t)(rx_buff[3]<<4|(rx_buff[4]>>4)); + ECMotor_Report.current_raw = (uint16_t)(rx_buff[4]<<8|(rx_buff[5])); + + DaemonReload(motor->daemon); // 喂狗 + measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt); + + measure->angle_single_round = (float)(ECMotor_Report.angle_raw-32768) * 0.0003814697; + measure->speed_rads = (float)(ECMotor_Report.speed_raw-2048) * 0.008789; + measure->real_current = (float)(ECMotor_Report.current_raw-2048) * 0.014648; + measure->temperature = (ECMotor_Report.temperature_raw - 50) /2; +} + +static void ECMotorLostCallback(void *motor_ptr) +{ + ECMotorInstance *motor = (ECMotorInstance *)motor_ptr; + LOGWARNING("[ECMotor] motor lost, id: %d", motor->motor_can_ins->tx_id); +} + +ECMotorInstance *ECMotorInit(Motor_Init_Config_s *config) +{ + ECMotorInstance *motor = (ECMotorInstance *)malloc(sizeof(ECMotorInstance)); + motor = (ECMotorInstance *)malloc(sizeof(ECMotorInstance)); + memset(motor, 0, sizeof(ECMotorInstance)); + + motor->motor_settings = config->controller_setting_init_config; + PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID); + PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID); + PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID); + motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr; + motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr; + + config->can_init_config.id = motor; + config->can_init_config.can_module_callback = ECMotorDecode; + config->can_init_config.rx_id = config->can_init_config.tx_id; + config->can_init_config.tx_id = config->can_init_config.tx_id; + motor->motor_can_ins = CANRegister(&config->can_init_config); + + ECMotorEnable(motor); + + DWT_GetDeltaT(&motor->measure.feed_dwt_cnt); + ecmotor_instance[idx++] = motor; + + Daemon_Init_Config_s daemon_config = { + .callback = ECMotorLostCallback, + .owner_id = motor, + .reload_count = 5, // 50ms + }; + motor->daemon = DaemonRegister(&daemon_config); + + return motor; +} + + + +void ECMotorControl() +{ + float pid_measure, pid_ref; + int16_t set; + ECMotorInstance *motor; + ECMotor_Measure_t *measure; + Motor_Control_Setting_s *setting; + + for (size_t i = 0; i < idx; ++i) + { + motor = ecmotor_instance[i]; + measure = &motor->measure; + setting = &motor->motor_settings; + pid_ref = motor->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->other_angle_feedback_ptr; + else + pid_measure = measure->real_current; + pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref); + if (setting->feedforward_flag & SPEED_FEEDFORWARD) + pid_ref += *motor->speed_feedforward_ptr; + } + + if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)) + { + if (setting->angle_feedback_source == OTHER_FEED) + pid_measure = *motor->other_speed_feedback_ptr; + else + pid_measure = measure->speed_rads; + pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref); + if (setting->feedforward_flag & CURRENT_FEEDFORWARD) + pid_ref += *motor->current_feedforward_ptr; + } + + if (setting->close_loop_type & CURRENT_LOOP) + { + pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref); + } + + set = pid_ref; + if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) + set *= -1; + + ECMotor_SpdCMD.mode = 0b010; + ECMotor_SpdCMD.reserve = 0b000; + ECMotor_SpdCMD.ack_type = 0b01; + ECMotor_SpdCMD.spd_target = set; + ECMotor_SpdCMD.max_current = 10; + // 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance + //memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t)); + CANSetDLC(motor->motor_can_ins,7); + memcpy(motor->motor_can_ins->tx_buff,&ECMotor_SpdCMD,sizeof(ECMotor_SpdCMD)); + //发送数据高位在前！！ 交换一下顺序 + swap(motor->motor_can_ins->tx_buff+1,motor->motor_can_ins->tx_buff+4); + swap(motor->motor_can_ins->tx_buff+2,motor->motor_can_ins->tx_buff+3); + swap(motor->motor_can_ins->tx_buff+5,motor->motor_can_ins->tx_buff+6); + + //memcpy(motor->motor_can_ins->tx_buff+1,&ECMotor_SpdCMD.spd_target,sizeof(float32_t)); + + if (motor->stop_flag == MOTOR_STOP) + { // 若该电机处于停止状态,直接将发送buff置零 + memcpy(motor->motor_can_ins->tx_buff,0,sizeof(ECMotor_SpdCMD)); + } + + CANTransmit(motor->motor_can_ins,0.2); + } + + +} + +void ECMotorStop(ECMotorInstance *motor) +{ + motor->stop_flag = MOTOR_STOP; +} + +void ECMotorEnable(ECMotorInstance *motor) +{ + motor->stop_flag = MOTOR_ENALBED; +} + +void ECMotorSetRef(ECMotorInstance *motor, float ref) +{ + motor->pid_ref = ref; +} + +uint8_t ECMotorIsOnline(ECMotorInstance *motor) +{ + return DaemonIsOnline(motor->daemon); +} diff --git a/modules/motor/ECmotor/ECA8210.h b/modules/motor/ECmotor/ECA8210.h new file mode 100644 index 0000000..e5e4ee4 --- /dev/null +++ b/modules/motor/ECmotor/ECA8210.h @@ -0,0 +1,130 @@ +// +// Created by SJQ on 2023/12/11. +// + +#ifndef BASIC_FRAMEWORK_ECA8210_H +#define BASIC_FRAMEWORK_ECA8210_H + +#include "stdint.h" +#include "bsp_can.h" +#include "controller.h" +#include "motor_def.h" +#include "daemon.h" + +#define EC_MOTOR_MX_CNT 4 + +#pragma pack(1) +typedef struct +{ + uint8_t bug_info:5; //错误信息 + uint8_t report_type:3; //报文类别 + + uint16_t angle_raw:16; //输出端位置数值范围：0~65536 对应-12.5f~12.5f，单位 rad + uint16_t speed_raw:12; //输出端转速数值范围：0~4095 对应-18.0f~18.0f，单位 rad/s + uint16_t current_raw:12; //实际电流：0~4095 对应-30~30A + uint8_t temperature_raw:8;//电机温度：反馈的数值数据类型为无符号 8 位，数值等于实际温度*2+50 +}ECMotor_Report_t; +#pragma pack() +typedef struct +{ + uint8_t mode:3; //控制模式 + float32_t pos_target; //错误信息 + uint16_t spd_target:15; //输出端位置数值范围：0~65536 对应-12.5f~12.5f，单位 rad + uint16_t max_current:12; //输出端转速数值范围：0~4095 对应-18.0f~18.0f，单位 rad/s + uint8_t ack_type:2; //实际电流：0~4095 对应-30~30A + +}ECMotor_PosCMD_t; +#pragma pack(1) +typedef struct +{ + uint8_t ack_type:2; + uint8_t reserve:3; + uint8_t mode:3; //控制模式 + + + float32_t spd_target; //输出端速度指令 单位RPM + uint16_t max_current; //电流限幅 电机电流阈值数值解释：0~65536 对应 0~6553.6A，比例为10 + +}ECMotor_SpdCMD_t; + +#pragma pack() + +typedef struct // EC-A8120 +{ + uint16_t last_ecd; // 上一次读取的编码器值 + uint16_t ecd; // 当前编码器值 + float angle_single_round; // 单圈角度 + float speed_rads; // speed rad/s + float real_current; // 实际电流 + uint8_t temperature; // 温度,C° + + float total_angle; // 总角度 + int32_t total_round; // 总圈数 + + float feed_dt; + uint32_t feed_dwt_cnt; +} ECMotor_Measure_t; + +typedef struct +{ + ECMotor_Measure_t measure; + + Motor_Control_Setting_s motor_settings; + + float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针 + float *other_speed_feedback_ptr; + float *speed_feedforward_ptr; // 速度前馈数据指针,可以通过此指针设置速度前馈值,或LQR等时作为速度状态变量的输入 + float *current_feedforward_ptr; // 电流前馈指针 + PIDInstance current_PID; + PIDInstance speed_PID; + PIDInstance angle_PID; + float pid_ref; + + Motor_Working_Type_e stop_flag; // 启停标志 + + CANInstance *motor_can_ins; + + DaemonInstance *daemon; + +} ECMotorInstance; + +/** + * @brief 初始化LK电机 + * + * @param config 电机配置 + * @return LKMotorInstance* 返回实例指针 + */ +ECMotorInstance *ECMotorInit(Motor_Init_Config_s *config); + +/** + * @brief 设置参考值 + * @attention 注意此函数设定的ref是最外层闭环的输入,若要设定内层闭环的值请通过前馈数据指针设置 + * + * @param motor 要设置的电机 + * @param ref 设定值 + */ +void ECMotorSetRef(ECMotorInstance *motor, float ref); + +/** + * @brief 为所有EC电机计算pid/反转/模式控制,并通过bspcan发送电流值(发送CAN报文) + * + */ +void ECMotorControl(); + +/** + * @brief 停止EC电机,之后电机不会响应任何指令 + * + * @param motor + */ +void ECMotorStop(ECMotorInstance *motor); + +/** + * @brief 启动EC电机 + * + * @param motor + */ +void ECMotorEnable(ECMotorInstance *motor); + +uint8_t ECMotorIsOnline(ECMotorInstance *motor); + +#endif //BASIC_FRAMEWORK_ECA8210_H diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index d73476e..bf5b782 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -68,6 +68,11 @@ 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 { @@ -79,6 +84,8 @@ typedef struct Feedback_Source_e speed_feedback_source; // 速度反馈类型 Feedfoward_Type_e feedforward_flag; // 前馈标志 + Power_Limit_Type_e power_limit_flag; //功率限制标志 + } Motor_Control_Setting_s; /* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/ @@ -95,6 +102,10 @@ typedef struct PIDInstance angle_PID; float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环 + + float motor_power_max; //每个电机分配的功率上限 + float motor_power_predict; //根据模型预测的电机功率 + float motor_power_scale; //电机功率缩放比例 } Motor_Controller_s; /* 电机类型枚举 */ @@ -106,8 +117,17 @@ typedef enum M2006, LK9025, HT04, + ECA8210, } Motor_Type_e; +/* 电机控制方式枚举 */ +typedef enum +{ + CONTROL_TYPE_NONE = 0, + CURRENT_CONTROL, + VOLTAGE_CONTROL, +} Motor_Control_Type_e; + /** * @brief 电机控制器初始化结构体,包括三环PID的配置以及两个反馈数据来源指针 * 如果不需要某个控制环,可以不设置对应的pid config @@ -133,6 +153,8 @@ 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 diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c index 47ff232..0e14f0f 100644 --- a/modules/motor/motor_task.c +++ b/modules/motor/motor_task.c @@ -1,9 +1,11 @@ #include "motor_task.h" #include "LK9025.h" +#include "ECA8210.h" #include "HT04.h" #include "dji_motor.h" #include "step_motor.h" #include "servo_motor.h" +#include "dmmotor.h" void MotorControlTask() { @@ -14,7 +16,8 @@ void MotorControlTask() /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ LKMotorControl(); - + ECMotorControl(); + //DMMotorControl(); // legacy support // 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞 // 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换 diff --git a/modules/power_meter/power_meter.c b/modules/power_meter/power_meter.c new file mode 100644 index 0000000..595af75 --- /dev/null +++ b/modules/power_meter/power_meter.c @@ -0,0 +1,38 @@ +// +// Created by SJQ on 2023/12/19. +// + +#include "power_meter.h" +#include "memory.h" +#include "stdlib.h" + +static PowerMeterInstance *power_meter_instance = NULL; // 可以由app保存此指针 + +static void PowerMeterRxCallback(CANInstance *_instance) +{ + uint8_t *rxbuff; + PowerMeter_Msg_s *Msg; + rxbuff = _instance->rx_buff; + Msg = &power_meter_instance->power_msg; + Msg->vol = (uint16_t)(rxbuff[1] << 8 | rxbuff[0]); + Msg->current = (uint16_t)(rxbuff[3] << 8 | rxbuff[2]); + +} + +PowerMeterInstance *PowerMeterInit(PowerMeter_Init_Config_s *PowerMeter_config) +{ + power_meter_instance = (PowerMeterInstance *)malloc(sizeof(PowerMeterInstance)); + memset(power_meter_instance, 0, sizeof(PowerMeterInstance)); + + PowerMeter_config->can_config.can_module_callback = PowerMeterRxCallback; + power_meter_instance->can_ins = CANRegister(&PowerMeter_config->can_config); + return power_meter_instance; +} + +float PowerMeterGet(PowerMeterInstance *instance) +{ + float power = instance->power_msg.current*instance->power_msg.vol/1e4; + return power; +} + +#include "power_meter.h" diff --git a/modules/power_meter/power_meter.h b/modules/power_meter/power_meter.h new file mode 100644 index 0000000..f5464ee --- /dev/null +++ b/modules/power_meter/power_meter.h @@ -0,0 +1,39 @@ +// +// Created by SJQ on 2023/12/19. +// + +#ifndef BASIC_FRAMEWORK_POWER_METER_H +#define BASIC_FRAMEWORK_POWER_METER_H +#include "bsp_can.h" + +#pragma pack(1) +typedef struct +{ + uint16_t vol; // 电压 + uint16_t current; // 电流 +} PowerMeter_Msg_s; +#pragma pack() + +/* 超级电容实例 */ +typedef struct +{ + CANInstance *can_ins; // CAN实例 + PowerMeter_Msg_s power_msg; // 功率计反馈信息 +} PowerMeterInstance; + +/* 超级电容初始化配置 */ +typedef struct +{ + CAN_Init_Config_s can_config; +} PowerMeter_Init_Config_s; + +/** + * @brief 初始化功率计 + * + * @param PowerMeter_config 功率计初始化配置 + * @return PowerMeterInstance* 功率计实例指针 + */ +PowerMeterInstance *PowerMeterInit(PowerMeter_Init_Config_s *PowerMeter_config); +float PowerMeterGet(PowerMeterInstance *instance); + +#endif //BASIC_FRAMEWORK_POWER_METER_H diff --git a/modules/referee/referee_protocol.h b/modules/referee/referee_protocol.h index 22416b3..aa8e15a 100644 --- a/modules/referee/referee_protocol.h +++ b/modules/referee/referee_protocol.h @@ -65,7 +65,7 @@ typedef struct /****************************cmd_id命令码说明****************************/ /****************************cmd_id命令码说明****************************/ - +// V1.6.1 裁判系统串口协议 /* 命令码ID,用来判断接收的是什么数据 */ typedef enum { @@ -83,24 +83,28 @@ typedef enum ID_robot_hurt = 0x0206, // 伤害状态数据 ID_shoot_data = 0x0207, // 实时射击数据 ID_student_interactive = 0x0301, // 机器人间交互数据 + ID_custom_robot = 0x302, // 自定义控制器数据（图传链路） + ID_remote_control = 0x304, // 键鼠遥控数据（图传链路） } CmdID_e; /* 命令码数据段长,根据官方协议来定义长度，还有自定义数据长度 */ typedef enum { - LEN_game_state = 3, // 0x0001 + LEN_game_state = 11, // 0x0001 LEN_game_result = 1, // 0x0002 - LEN_game_robot_HP = 2, // 0x0003 + LEN_game_robot_HP = 32, // 0x0003 LEN_event_data = 4, // 0x0101 LEN_supply_projectile_action = 4, // 0x0102 - LEN_game_robot_state = 27, // 0x0201 - LEN_power_heat_data = 14, // 0x0202 + LEN_game_robot_state = 13, // 0x0201 + LEN_power_heat_data = 16, // 0x0202 LEN_game_robot_pos = 16, // 0x0203 - LEN_buff_musk = 1, // 0x0204 - LEN_aerial_robot_energy = 1, // 0x0205 + LEN_buff_musk = 6, // 0x0204 + LEN_aerial_robot_energy = 2, // 0x0205 LEN_robot_hurt = 1, // 0x0206 LEN_shoot_data = 7, // 0x0207 LEN_receive_data = 6 + Communicate_Data_LEN, // 0x0301 + LEN__custom_robot = 30, // 0x0302 + LEN_remote_control = 12, // 0x0304 } JudgeDataLength_e; @@ -124,23 +128,23 @@ typedef struct /* ID: 0x0003 Byte: 32 比赛机器人血量数据 */ typedef struct { - uint16_t red_1_robot_HP; - uint16_t red_2_robot_HP; - uint16_t red_3_robot_HP; - uint16_t red_4_robot_HP; - uint16_t red_5_robot_HP; - uint16_t red_7_robot_HP; - uint16_t red_outpost_HP; - uint16_t red_base_HP; - uint16_t blue_1_robot_HP; - uint16_t blue_2_robot_HP; - uint16_t blue_3_robot_HP; - uint16_t blue_4_robot_HP; - uint16_t blue_5_robot_HP; - uint16_t blue_7_robot_HP; - uint16_t blue_outpost_HP; - uint16_t blue_base_HP; -} ext_game_robot_HP_t; + uint16_t red_1_robot_HP; + uint16_t red_2_robot_HP; + uint16_t red_3_robot_HP; + uint16_t red_4_robot_HP; + uint16_t red_5_robot_HP; + uint16_t red_7_robot_HP; + uint16_t red_outpost_HP; + uint16_t red_base_HP; + uint16_t blue_1_robot_HP; + uint16_t blue_2_robot_HP; + uint16_t blue_3_robot_HP; + uint16_t blue_4_robot_HP; + uint16_t blue_5_robot_HP; + uint16_t blue_7_robot_HP; + uint16_t blue_outpost_HP; + uint16_t blue_base_HP; +}ext_game_robot_HP_t; /* ID: 0x0101 Byte: 4 场地事件数据 */ typedef struct @@ -148,7 +152,7 @@ typedef struct uint32_t event_type; } ext_event_data_t; -/* ID: 0x0102 Byte: 3 场地补给站动作标识数据 */ +/* ID: 0x0102 Byte: 4 场地补给站动作标识数据 */ typedef struct { uint8_t supply_projectile_id; @@ -157,37 +161,32 @@ typedef struct uint8_t supply_projectile_num; } ext_supply_projectile_action_t; -/* ID: 0X0201 Byte: 27 机器人状态数据 */ +/* ID: 0X0201 Byte: 13 机器人性能体系数据 */ typedef struct { - 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; + 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; } ext_game_robot_state_t; -/* ID: 0X0202 Byte: 14 实时功率热量数据 */ +/* ID: 0X0202 Byte: 16 实时功率热量数据 */ typedef struct { - 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; + 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; } ext_power_heat_data_t; /* ID: 0x0203 Byte: 16 机器人位置数据 */ @@ -199,16 +198,22 @@ typedef struct float yaw; } ext_game_robot_pos_t; -/* ID: 0x0204 Byte: 1 机器人增益数据 */ +/* ID: 0x0204 Byte: 6 机器人增益数据 */ typedef struct { - uint8_t power_rune_buff; + uint8_t recovery_buff; + uint8_t cooling_buff; + uint8_t defence_buff; + uint8_t vulnerability_buff; + uint16_t attack_buff; + uint8_t power_rune_buff; } ext_buff_musk_t; -/* ID: 0x0205 Byte: 1 空中机器人能量状态数据 */ +/* ID: 0x0205 Byte: 2 空中机器人能量状态数据 */ typedef struct { - uint8_t attack_time; + uint8_t airforce_status; + uint8_t time_remain; } aerial_robot_energy_t; /* ID: 0x0206 Byte: 1 伤害状态数据 */ @@ -227,6 +232,20 @@ typedef struct float bullet_speed; } ext_shoot_data_t; +/****************************图传链路数据****************************/ +/* ID: 0x0304 Byte: 12 图传链路键鼠遥控数据 */ +typedef struct +{ + int16_t mouse_x; + int16_t mouse_y; + int16_t mouse_z; + int8_t left_button_down; + int8_t right_button_down; + uint16_t keyboard_value; + uint16_t reserved; +}vision_transfer_t; +/****************************图传链路数据****************************/ + /****************************机器人交互数据****************************/ /****************************机器人交互数据****************************/ /* 发送的内容数据段最大为 113 检测是否超出大小限制?实际上图形段不会超，数据段最多30个，也不会超*/ diff --git a/modules/referee/referee_task.c b/modules/referee/referee_task.c index 7971265..2bc5570 100644 --- a/modules/referee/referee_task.c +++ b/modules/referee/referee_task.c @@ -14,10 +14,15 @@ #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 不应该使用全局变量 /** @@ -28,7 +33,7 @@ uint8_t UI_Seq; // 包序号，供整个ref */ static void DeterminRobotID() { - // id小于7是红色,大于7是蓝色,0为红色，1为蓝色 #define Robot_Red 0 #define Robot_Blue 1 + // id小于7是红色,大于7是蓝色 #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 @@ -49,15 +54,17 @@ 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_Energy[3]; // 电容能量条 -static String_Data_t UI_State_sta[6]; // 机器人状态,静态只需画一次 -static String_Data_t UI_State_dyn[6]; // 机器人状态,动态先add才能change +static Graph_Data_t UI_shoot_yuan[2]; // 射击圆心 +static Graph_Data_t UI_Energy[6]; // 电容能量条 +static String_Data_t UI_State_sta[7]; // 机器人状态,静态只需画一次 +static String_Data_t UI_State_dyn[7]; // 机器人状态,动态先add才能change static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565}; +static String_Data_t UI_prompt_sta[3]; // 操作手提示 void MyUIInit() { @@ -69,108 +76,79 @@ 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_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]); + 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,80); + 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[4], "sl4", UI_Graph_ADD,7, UI_Color_Green, 2, 1020, 450, 1020, 600); + UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_shoot_line[4]); - // 绘制车辆状态标志指示 - UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 150, 750, "chassis:"); + // 绘制车辆状态标志指示，静态 + UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 20, 2, 10, 750, "E.spin:"); UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[0]); - UICharDraw(&UI_State_sta[1], "ss1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 150, 700, "gimbal:"); - 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, "frict:"); +// UICharDraw(&UI_State_sta[1], "ss1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 150, 700, "gimbal:"); +// 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, 20, 2, 10, 700, "F.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, "lid:"); + UICharDraw(&UI_State_sta[4], "ss4", UI_Graph_ADD, 8, UI_Color_Main, 20, 2, 10, 650, "B.lid:"); UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[4]); + UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 8, UI_Color_Pink, 20, 2, 10, 600, "Q.heat:"); + UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[5]); + UICharDraw(&UI_State_sta[6], "ss6", UI_Graph_ADD, 8, UI_Color_Pink, 20, 2, 10, 550, "C.ce:"); + UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[6]); +// UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 800, "load:"); +// UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[5]); + + // 操作手提示 + UICharDraw(&UI_prompt_sta[0], "ss8", UI_Graph_ADD,8, UI_Color_Pink, 30, 4, 1650, 700, "bie huang"); + UICharRefresh(&referee_recv_info->referee_id, UI_prompt_sta[0]); + + // 底盘功率显示，静态 +// UICharDraw(&UI_State_sta[6], "ss6", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 620, 230, "MAX:"); +// UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[6]); // 绘制车辆状态标志，动态 // 由于初始化时xxx_last_mode默认为0，所以此处对应UI也应该设为0时对应的UI，防止模式不变的情况下无法置位flag，导致UI无法刷新 - UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce"); + UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 20, 2, 170, 750, "off"); UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]); - UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce"); - UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]); - UICharDraw(&UI_State_dyn[2], "sd2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 270, 650, "off"); - UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]); - UICharDraw(&UI_State_dyn[3], "sd3", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 600, "off"); +// UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce"); +// UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]); +// UICharDraw(&UI_State_dyn[2], "sd2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 270, 650, "off"); +// UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]); + UICharDraw(&UI_State_dyn[3], "sd3", UI_Graph_ADD, 8, UI_Color_Pink, 20, 2, 170, 700, "off"); UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[3]); - UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 550, "open "); + UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_ADD, 8, UI_Color_Pink, 20, 2, 170, 650, "off"); UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]); + UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_ADD, 8, UI_Color_Pink, 20, 2, 170, 600, "on "); + UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[5]); + UICharDraw(&UI_State_dyn[6], "sd8", UI_Graph_ADD, 8, UI_Color_Pink, 20, 2, 170, 550, "off"); + UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[6]); +// UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 800, "999"); +// UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[5]); - // 底盘功率显示，静态 - 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]); +// 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, 750, 230, 24000); - // 能量条初始状态 - UILineDraw(&UI_Energy[2], "sd6", UI_Graph_ADD, 8, UI_Color_Pink, 30, 720, 160, 1020, 160); - UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]); -} + UIFloatDraw(&UI_Energy[1], "sd6", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 2, 850, 230, Interactive_data->Chassis_Power_Data.chassis_power_mx * 1000); + UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[1]); -// 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常 -static uint8_t count = 0; -static uint16_t count1 = 0; -static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测试用函数，实现模式自动变化 -{ - count++; - if (count >= 50) - { - count = 0; - count1++; - } - switch (count1 % 4) - { - case 0: - { - _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: - break; - } + // 电容电压 + UIFloatDraw(&UI_Energy[2], "sd7", UI_Graph_ADD, 8, UI_Color_Pink, 18, 2,2, 850, 280,Interactive_data->Cap_Data.cap_vol); + UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[2]); + // 电容能量条 +// UILineDraw(&UI_Energy[3], "sd8", UI_Graph_ADD, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)(720 + (Interactive_data->Cap_Data.cap_vol-1200) * 0.416), 160); +// UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[3]); + + // 发弹量条框 + UILineDraw(&UI_Energy[4], "ss7", UI_Graph_ADD, 7, UI_Color_Pink, 2, 1325, 500, 1480, 500); + UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[4]); } static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data) @@ -180,77 +158,126 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i if (_Interactive_data->Referee_Interactive_Flag.chassis_flag == 1) { switch (_Interactive_data->chassis_mode) - { - case CHASSIS_ZERO_FORCE: - UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce"); - break; - case CHASSIS_ROTATE: - UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "rotate "); // 此处注意字数对齐问题，字数相同才能覆盖掉 - break; - case CHASSIS_NO_FOLLOW: - UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "nofollow "); - break; - case CHASSIS_FOLLOW_GIMBAL_YAW: - UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow "); - break; + { + case CHASSIS_ZERO_FORCE: + UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 20, 2, 170, 750, "off"); + UICharDraw(&UI_State_dyn[6], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 550, "off"); + break; + case CHASSIS_ROTATE: + UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 20, 2, 170, 750, "on "); + UICharDraw(&UI_State_dyn[6], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 550, "off"); + break; + case CHASSIS_SIDEWAYS: + UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 20, 2, 170, 750, "off"); + UICharDraw(&UI_State_dyn[6], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 550, "on "); + break; + case CHASSIS_NO_FOLLOW: + UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 20, 2, 170, 750, "off"); + UICharDraw(&UI_State_dyn[6], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 550, "off"); + break; + case CHASSIS_FOLLOW_GIMBAL_YAW: + UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 20, 2, 170, 750, "off"); + UICharDraw(&UI_State_dyn[6], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 550, "off"); + break; } - UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]); + UICharRefresh(&referee_recv_info->referee_id,UI_State_dyn[0]); + UICharRefresh(&referee_recv_info->referee_id,UI_State_dyn[6]); _Interactive_data->Referee_Interactive_Flag.chassis_flag = 0; } // gimbal - if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1) - { - switch (_Interactive_data->gimbal_mode) - { - case GIMBAL_ZERO_FORCE: - { - UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce"); - break; - } - case GIMBAL_FREE_MODE: - { - UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "free "); - break; - } - case GIMBAL_GYRO_MODE: - { - UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "gyro "); - break; - } - } - UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]); - _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0; - } +// if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1) +// { +// switch (_Interactive_data->gimbal_mode) +// { +// case GIMBAL_ZERO_FORCE: +// { +// UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce"); +// break; +// } +// case GIMBAL_FREE_MODE: +// { +// UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "free "); +// break; +// } +// case GIMBAL_GYRO_MODE: +// { +// UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "gyro "); +// break; +// } +// } +// 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; - } +// 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"); + UICharDraw(&UI_State_dyn[3], "sd3", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 700, _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"); + UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 650, _Interactive_data->lid_mode == LID_OPEN ? "on " : "off"); UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]); _Interactive_data->Referee_Interactive_Flag.lid_flag = 0; } + // heat + if (_Interactive_data->Referee_Interactive_Flag.heat_flag == 1) + { + UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 600, _Interactive_data->heat_mode == HEAT_OPEN ? "on " : "off"); + UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[5]); + _Interactive_data->Referee_Interactive_Flag.heat_flag = 0; + } + // loader_mode +// if (_Interactive_data->Referee_Interactive_Flag.load_flag == 1) +// { +// switch (loader_flag) +// { +// case 2: +// { +// UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 800, " 1 "); +// break; +// } +// case 1: +// { +// UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 800, " 3 "); +// break; +// } +// case 0: +// { +// UICharDraw(&UI_State_dyn[5], "sd5", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 800, "999"); +// break; +// } +// } +// UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[5]); +// _Interactive_data->Referee_Interactive_Flag.load_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]); + UIFloatDraw(&UI_Energy[1], "sd6", UI_Graph_Change, 8, UI_Color_Green, 18, 2, 2, 850, 230, _Interactive_data->Chassis_Power_Data.chassis_power_mx * 1000); + UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1]); _Interactive_data->Referee_Interactive_Flag.Power_flag = 0; } + UIFloatDraw(&UI_Energy[2], "sd7", UI_Graph_Change, 8, UI_Color_Pink, 18, 2,2, 850, 280,Interactive_data->Cap_Data.cap_vol); + UIGraphRefresh(&referee_recv_info->referee_id, 1,UI_Energy[2]); +// UILineDraw(&UI_Energy[3], "sd8", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)(720 + (Interactive_data->Cap_Data.cap_vol-1200) * 0.416), 160); +// UIGraphRefresh(&referee_recv_info->referee_id, 1, &UI_Energy[3]); + //绘制开火建议 + if (_Interactive_data->Referee_Interactive_Flag.aim_flag == 1) { + UICircleDraw(&UI_shoot_yuan[0], "sy0", UI_Graph_Change, 7, _Interactive_data->aim_fire == 0 ? UI_Color_Yellow : UI_Color_Main, 2, 960, 540, 80); + UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_shoot_yuan[0]); + _Interactive_data->Referee_Interactive_Flag.aim_flag = 0; + } } /** @@ -267,17 +294,17 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data) _Interactive_data->chassis_last_mode = _Interactive_data->chassis_mode; } - if (_Interactive_data->gimbal_mode != _Interactive_data->gimbal_last_mode) - { - _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 1; - _Interactive_data->gimbal_last_mode = _Interactive_data->gimbal_mode; - } +// if (_Interactive_data->gimbal_mode != _Interactive_data->gimbal_last_mode) +// { +// _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->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) { @@ -291,9 +318,33 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data) _Interactive_data->lid_last_mode = _Interactive_data->lid_mode; } + if (_Interactive_data->heat_mode != _Interactive_data->heat_last_mode) + { + _Interactive_data->Referee_Interactive_Flag.heat_flag = 1; + _Interactive_data->heat_last_mode = _Interactive_data->heat_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; } + +// if (_Interactive_data->Cap_Data.cap_vol != _Interactive_data->Cap_last_Data.cap_vol) +// { +// _Interactive_data->Referee_Interactive_Flag.cap_flag = 1; +// _Interactive_data->Cap_last_Data.cap_vol = _Interactive_data->Cap_Data.cap_vol; +// } + + if (_Interactive_data->aim_fire != _Interactive_data->aim_last_fire) + { + _Interactive_data->Referee_Interactive_Flag.aim_flag = 1; + _Interactive_data->aim_last_fire = _Interactive_data->aim_fire; + } + +// if (_Interactive_data->loader_mode != _Interactive_data->loader_last_mode) +// { +// _Interactive_data->Referee_Interactive_Flag.load_flag = 1; +// _Interactive_data->loader_last_mode = _Interactive_data->loader_mode; +// } } diff --git a/modules/referee/referee_task.h b/modules/referee/referee_task.h index 1eec04b..99568d5 100644 --- a/modules/referee/referee_task.h +++ b/modules/referee/referee_task.h @@ -8,6 +8,7 @@ * @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); /** diff --git a/modules/referee/rm_referee.h b/modules/referee/rm_referee.h index e935e94..e852271 100644 --- a/modules/referee/rm_referee.h +++ b/modules/referee/rm_referee.h @@ -7,6 +7,8 @@ #include "bsp_usart.h" #include "FreeRTOS.h" + + extern uint8_t UI_Seq; #pragma pack(1) @@ -54,6 +56,10 @@ 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; + uint32_t load_flag : 1; + uint32_t heat_flag : 1; } Referee_Interactive_Flag_t; // 此结构体包含UI绘制与机器人车间通信的需要的其他非裁判系统数据 @@ -63,10 +69,14 @@ typedef struct // 为UI绘制以及交互数据所用 chassis_mode_e chassis_mode; // 底盘模式 gimbal_mode_e gimbal_mode; // 云台模式 - shoot_mode_e shoot_mode; // 发射模式设置 + shoot_mode_e shoot_mode; // 发射开关设置 friction_mode_e friction_mode; // 摩擦轮关闭 lid_mode_e lid_mode; // 弹舱盖打开 Chassis_Power_Data_s Chassis_Power_Data; // 功率控制 + uint8_t aim_fire; // 开火建议 + loader_mode_e loader_mode; // 发射模式 + heat_mode_e heat_mode; // 热量控制 + Cap_Data_s Cap_Data; // 电容信息 // 上一次的模式，用于flag判断 chassis_mode_e chassis_last_mode; @@ -75,6 +85,10 @@ typedef struct friction_mode_e friction_last_mode; lid_mode_e lid_last_mode; Chassis_Power_Data_s Chassis_last_Power_Data; + uint8_t aim_last_fire; + loader_mode_e loader_last_mode; + heat_mode_e heat_last_mode; + Cap_Data_s Cap_last_Data; } Referee_Interactive_info_t; diff --git a/modules/referee/vision_transfer.c b/modules/referee/vision_transfer.c new file mode 100644 index 0000000..6700666 --- /dev/null +++ b/modules/referee/vision_transfer.c @@ -0,0 +1,162 @@ +/** + * @file rm_referee.C + * @author kidneygood (you@domain.com) + * @brief + * @version 0.1 + * @date 2022-11-18 + * + * @copyright Copyright (c) 2022 + * + */ + +#include "vision_transfer.h" +#include "string.h" +#include "crc_ref.h" +#include "bsp_usart.h" +#include "task.h" +#include "daemon.h" +#include "bsp_log.h" +#include "cmsis_os.h" +#include "remote_control.h" + +#define RE_RX_BUFFER_SIZE 128u // 裁判系统接收缓冲区大小 + +static USARTInstance *vt_usart_instance; // 裁判系统串口实例 +static DaemonInstance *vision_transfer_daemon; // 裁判系统守护进程 +static referee_info_vt_t referee_info_vt; // 裁判系统数据 +static VT_ctrl_t vt_ctrl[2]; //[0]:当前数据TEMP,[1]:上一次的数据LAST.用于按键持续按下和切换的判断 +/** + * @brief 读取裁判数据,中断中读取保证速度 + * @param buff: 读取到的裁判系统原始数据 + * @retval 是否对正误判断做处理 + * @attention 在此判断帧头和CRC校验,无误再写入数据，不重复判断帧头 + */ +static void JudgeReadVTData(uint8_t *buff) +{ + uint16_t judge_length; // 统计一帧数据长度 + if (buff == NULL) // 空数据包，则不作任何处理 + return; + + // 写入帧头数据(5-byte),用于判断是否开始存储裁判数据 + memcpy(&referee_info_vt.FrameHeader, buff, LEN_HEADER); + + // 判断帧头数据(0)是否为0xA5 + if (buff[SOF] == REFEREE_SOF) + { + // 帧头CRC8校验 + if (Verify_CRC8_Check_Sum(buff, LEN_HEADER) == TRUE) + { + // 统计一帧数据长度(byte),用于CR16校验 + judge_length = buff[DATA_LENGTH] + LEN_HEADER + LEN_CMDID + LEN_TAIL; + // 帧尾CRC16校验 + if (Verify_CRC16_Check_Sum(buff, judge_length) == TRUE) + { + // 2个8位拼成16位int + referee_info_vt.CmdID = (buff[6] << 8 | buff[5]); + // 解析数据命令码,将数据拷贝到相应结构体中(注意拷贝数据的长度) + // 第8个字节开始才是数据 data=7 + switch (referee_info_vt.CmdID) + { + case ID_custom_robot: //0x0302 + memcpy(&referee_info_vt.ReceiveData, (buff + DATA_Offset), LEN__custom_robot); + break; + case ID_remote_control: //0x0304 + memcpy(&referee_info_vt.VisionTransfer, (buff + DATA_Offset), LEN_remote_control); + break; + } + } + } + // 首地址加帧长度,指向CRC16下一字节,用来判断是否为0xA5,从而判断一个数据包是否有多帧数据 + if (*(buff + sizeof(xFrameHeader) + LEN_CMDID + referee_info_vt.FrameHeader.DataLength + LEN_TAIL) == 0xA5) + { // 如果一个数据包出现了多帧数据,则再次调用解析函数,直到所有数据包解析完毕 + JudgeReadVTData(buff + sizeof(xFrameHeader) + LEN_CMDID + referee_info_vt.FrameHeader.DataLength + LEN_TAIL); + } + } +} +static void vt_to_rc(void) +{ + // 鼠标解析 + vt_ctrl[TEMP].mouse.x = referee_info_vt.VisionTransfer.mouse_x; //!< Mouse X axis + vt_ctrl[TEMP].mouse.y = referee_info_vt.VisionTransfer.mouse_y; //!< Mouse Y axis + vt_ctrl[TEMP].mouse.press_l = referee_info_vt.VisionTransfer.left_button_down; //!< Mouse Left Is Press ? + vt_ctrl[TEMP].mouse.press_r = referee_info_vt.VisionTransfer.right_button_down; //!< Mouse Right Is Press ? + +// 位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位,msb在最后 + *(uint16_t *)&vt_ctrl[TEMP].key[KEY_PRESS] = referee_info_vt.VisionTransfer.keyboard_value; + + if (vt_ctrl[TEMP].key[KEY_PRESS].ctrl) // ctrl键按下 + vt_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL] = vt_ctrl[TEMP].key[KEY_PRESS]; + else + memset(&vt_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL], 0, sizeof(Key_t)); + if (vt_ctrl[TEMP].key[KEY_PRESS].shift) // shift键按下 + vt_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT] = vt_ctrl[TEMP].key[KEY_PRESS]; + else + memset(&vt_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT], 0, sizeof(Key_t)); + + uint16_t key_now = vt_ctrl[TEMP].key[KEY_PRESS].keys, // 当前按键是否按下 + key_last = vt_ctrl[LAST].key[KEY_PRESS].keys, // 上一次按键是否按下 + key_with_ctrl = vt_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL].keys, // 当前ctrl组合键是否按下 + key_with_shift = vt_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT].keys, // 当前shift组合键是否按下 + key_last_with_ctrl = vt_ctrl[LAST].key[KEY_PRESS_WITH_CTRL].keys, // 上一次ctrl组合键是否按下 + key_last_with_shift = vt_ctrl[LAST].key[KEY_PRESS_WITH_SHIFT].keys; // 上一次shift组合键是否按下 + + for (uint16_t i = 0, j = 0x1; i < 16; j <<= 1, i++) + { + if (i == 4 || i == 5) // 4,5位为ctrl和shift,直接跳过 + continue; +// 如果当前按键按下,上一次按键没有按下,且ctrl和shift组合键没有按下,则按键按下计数加1(检测到上升沿) + if ((key_now & j) && !(key_last & j) && !(key_with_ctrl & j) && !(key_with_shift & j)) + vt_ctrl[TEMP].key_count[KEY_PRESS][i]++; +// 当前ctrl组合键按下,上一次ctrl组合键没有按下,则ctrl组合键按下计数加1(检测到上升沿) + if ((key_with_ctrl & j) && !(key_last_with_ctrl & j)) + vt_ctrl[TEMP].key_count[KEY_PRESS_WITH_CTRL][i]++; +// 当前shift组合键按下,上一次shift组合键没有按下,则shift组合键按下计数加1(检测到上升沿) + if ((key_with_shift & j) && !(key_last_with_shift & j)) + vt_ctrl[TEMP].key_count[KEY_PRESS_WITH_SHIFT][i]++; + } + + memcpy(&vt_ctrl[LAST], &vt_ctrl[TEMP], sizeof(VT_ctrl_t)); // 保存上一次的数据,用于按键持续按下和切换的判断 +} + +/*裁判系统串口接收回调函数,解析数据 */ +static void VTRefereeRxCallback() +{ + DaemonReload(vision_transfer_daemon); + JudgeReadVTData(vt_usart_instance->recv_buff); + vt_to_rc(); +} +// 裁判系统丢失回调函数,重新初始化裁判系统串口 +static void VTRefereeLostCallback(void *arg) +{ + USARTServiceInit(vt_usart_instance); + LOGWARNING("[rm_ref] lost referee vision data "); +} + +/* 裁判系统通信初始化 */ +VT_ctrl_t *VTRefereeInit(UART_HandleTypeDef *vt_usart_handle) +{ + USART_Init_Config_s conf; + conf.module_callback = VTRefereeRxCallback; + conf.usart_handle = vt_usart_handle; + conf.recv_buff_size = RE_RX_BUFFER_SIZE; // mx 255(u8) + vt_usart_instance = USARTRegister(&conf); + + Daemon_Init_Config_s daemon_conf = { + .callback = VTRefereeLostCallback, + .owner_id = vt_usart_instance, + .reload_count = 30, // 0.3s没有收到数据,则认为丢失,重启串口接收 + }; + vision_transfer_daemon = DaemonRegister(&daemon_conf); + + return &vt_ctrl; +} + +/** + * @brief 裁判系统数据发送函数 + * @param + */ +void VTRefereeSend(uint8_t *send, uint16_t tx_len) +{ + USARTSend(vt_usart_instance, send, tx_len, USART_TRANSFER_DMA); + osDelay(115); +} diff --git a/modules/referee/vision_transfer.h b/modules/referee/vision_transfer.h new file mode 100644 index 0000000..5a514e3 --- /dev/null +++ b/modules/referee/vision_transfer.h @@ -0,0 +1,46 @@ +#ifndef VISION_TRANSFER_H +#define VISION_TRANSFER_H + +#include "usart.h" +#include "referee_protocol.h" +#include "robot_def.h" +#include "bsp_usart.h" +#include "FreeRTOS.h" +#include "remote_control.h" + +#pragma pack(1) + +// 此结构体包含裁判系统接收数据以及UI绘制与机器人车间通信的相关信息 +typedef struct +{ + + xFrameHeader FrameHeader; // 接收到的帧头信息 + uint16_t CmdID; + + vision_transfer_t VisionTransfer; + Communicate_ReceiveData_t ReceiveData; + + uint8_t init_flag; + +} referee_info_vt_t; + +#pragma pack() + +/** + * @brief 裁判系统通信初始化,该函数会初始化裁判系统串口,开启中断 + * + * @param vt_usart_handle 串口handle,C板一般用串口6 + * @return referee_info_t* 返回裁判系统反馈的数据,包括热量/血量/状态等 + */ +VT_ctrl_t *VTRefereeInit(UART_HandleTypeDef *referee_usart_handle); + +/** + * @brief UI绘制和交互数的发送接口,由UI绘制任务和多机通信函数调用 + * @note 内部包含了一个实时系统的延时函数,这是因为裁判系统接收CMD数据至高位10Hz + * + * @param send 发送数据首地址 + * @param tx_len 发送长度 + */ +void VTRefereeSend(uint8_t *send, uint16_t tx_len); + +#endif // !REFEREE_H diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index 4035304..b03d19e 100644 --- a/modules/remote/remote_control.c +++ b/modules/remote/remote_control.c @@ -16,6 +16,10 @@ static uint8_t rc_init_flag = 0; // 遥控器初始化标志位 static USARTInstance *rc_usart_instance; static DaemonInstance *rc_daemon_instance; +// 图传拥有的串口实例 +static USARTInstance *vt_usart_instance; +static DaemonInstance *vt_daemon_instance; + /** * @brief 矫正遥控器摇杆的值,超过660或者小于-660的值都认为是无效值,置0 * @@ -97,6 +101,12 @@ static void RemoteControlRxCallback() sbus_to_rc(rc_usart_instance->recv_buff); // 进行协议解析 } +//static void RemoteControlvtCallback() +//{ +// DaemonReload(vt_daemon_instance); // 先喂狗 +// sbus_to_rc(vt_usart_instance->recv_buff); // 进行协议解析 +//} + /** * @brief 遥控器离线的回调函数,注册到守护进程中,串口掉线时调用 * diff --git a/modules/remote/remote_control.h b/modules/remote/remote_control.h index c12f810..de51173 100644 --- a/modules/remote/remote_control.h +++ b/modules/remote/remote_control.h @@ -113,6 +113,20 @@ typedef struct uint8_t key_count[3][16]; } RC_ctrl_t; +typedef struct +{ + struct + { + int16_t x; + int16_t y; + uint8_t press_l; + uint8_t press_r; + } mouse; + + Key_t key[3]; + uint8_t key_count[3][16]; +}VT_ctrl_t; //图传链路下发的遥控数据 + /* ------------------------- Internal Data ----------------------------------- */ /** @@ -123,6 +137,8 @@ typedef struct */ RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle); +//VT_ctrl_t *RemoteControlInit(UART_HandleTypeDef *vt_usart_handle); + /** * @brief 检查遥控器是否在线,若尚未初始化也视为离线 * diff --git a/modules/robotics/matrix.cpp b/modules/robotics/matrix.cpp new file mode 100644 index 0000000..23ea786 --- /dev/null +++ b/modules/robotics/matrix.cpp @@ -0,0 +1,24 @@ +/** + ****************************************************************************** + * @file matrix.cpp/h + * @brief Matrix/vector calculation. 矩阵/向量运算 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#include "matrix.h" + +// hat of vector +Matrixf<3, 3> vector3f::hat(Matrixf<3, 1> vec) { + float hat[9] = {0, -vec[2][0], vec[1][0], vec[2][0], 0, + -vec[0][0], -vec[1][0], vec[0][0], 0}; + return Matrixf<3, 3>(hat); +} + +// cross product +Matrixf<3, 1> vector3f::cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2) { + return vector3f::hat(vec1) * vec2; +} diff --git a/modules/robotics/matrix.h b/modules/robotics/matrix.h new file mode 100644 index 0000000..81295b5 --- /dev/null +++ b/modules/robotics/matrix.h @@ -0,0 +1,259 @@ +/** + ****************************************************************************** + * @file matrix.cpp/h + * @brief Matrix/vector calculation. 矩阵/向量运算 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#ifndef MATRIX_H +#define MATRIX_H + +//#include "arm_math.h" +#include "user_lib.h" +// Matrix class +template +class Matrixf { + public: + // Constructor without input data + Matrixf(void) : rows_(_rows), cols_(_cols) { + arm_mat_init_f32(&arm_mat_, _rows, _cols, this->data_); + } + // Constructor with input data + Matrixf(float data[_rows * _cols]) : Matrixf() { + memcpy(this->data_, data, _rows * _cols * sizeof(float)); + } + // Copy constructor + Matrixf(const Matrixf<_rows, _cols>& mat) : Matrixf() { + memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float)); + } + // Destructor + ~Matrixf(void) {} + + // Row size + int rows(void) { return _rows; } + // Column size + int cols(void) { return _cols; } + + // Element + float* operator[](const int& row) { return &this->data_[row * _cols]; } + + // Operators + Matrixf<_rows, _cols>& operator=(const Matrixf<_rows, _cols> mat) { + memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float)); + return *this; + } + Matrixf<_rows, _cols>& operator+=(const Matrixf<_rows, _cols> mat) { + arm_status s; + s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_); + return *this; + } + Matrixf<_rows, _cols>& operator-=(const Matrixf<_rows, _cols> mat) { + arm_status s; + s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_); + return *this; + } + Matrixf<_rows, _cols>& operator*=(const float& val) { + arm_status s; + s = arm_mat_scale_f32(&this->arm_mat_, val, &this->arm_mat_); + return *this; + } + Matrixf<_rows, _cols>& operator/=(const float& val) { + arm_status s; + s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &this->arm_mat_); + return *this; + } + Matrixf<_rows, _cols> operator+(const Matrixf<_rows, _cols>& mat) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_); + return res; + } + Matrixf<_rows, _cols> operator-(const Matrixf<_rows, _cols>& mat) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_); + return res; + } + Matrixf<_rows, _cols> operator*(const float& val) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_scale_f32(&this->arm_mat_, val, &res.arm_mat_); + return res; + } + friend Matrixf<_rows, _cols> operator*(const float& val, + const Matrixf<_rows, _cols>& mat) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_scale_f32(&mat.arm_mat_, val, &res.arm_mat_); + return res; + } + Matrixf<_rows, _cols> operator/(const float& val) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &res.arm_mat_); + return res; + } + // Matrix multiplication + template + friend Matrixf<_rows, cols> operator*(const Matrixf<_rows, _cols>& mat1, + const Matrixf<_cols, cols>& mat2) { + arm_status s; + Matrixf<_rows, cols> res; + s = arm_mat_mult_f32(&mat1.arm_mat_, &mat2.arm_mat_, &res.arm_mat_); + return res; + } + + // Submatrix + template + Matrixf block(const int& start_row, const int& start_col) { + Matrixf res; + for (int row = start_row; row < start_row + rows; row++) { + memcpy((float*)res[0] + (row - start_row) * cols, + (float*)this->data_ + row * _cols + start_col, + cols * sizeof(float)); + } + return res; + } + // Specific row + Matrixf<1, _cols> row(const int& row) { return block<1, _cols>(row, 0); } + // Specific column + Matrixf<_rows, 1> col(const int& col) { return block<_rows, 1>(0, col); } + + // Transpose + Matrixf<_cols, _rows> trans(void) { + Matrixf<_cols, _rows> res; + arm_mat_trans_f32(&arm_mat_, &res.arm_mat_); + return res; + } + // Trace + float trace(void) { + float res = 0; + for (int i = 0; i < fmin(_rows, _cols); i++) { + res += (*this)[i][i]; + } + return res; + } + // Norm + float norm(void) { return sqrtf((this->trans() * *this)[0][0]); } + + public: + // arm matrix instance + arm_matrix_instance_f32 arm_mat_; + + protected: + // size + int rows_, cols_; + // data + float data_[_rows * _cols]; +}; + +// Matrix funtions +namespace matrixf { + +// Special Matrices +// Zero matrix +template +Matrixf<_rows, _cols> zeros(void) { + float data[_rows * _cols] = {0}; + return Matrixf<_rows, _cols>(data); +} +// Ones matrix +template +Matrixf<_rows, _cols> ones(void) { + float data[_rows * _cols] = {0}; + for (int i = 0; i < _rows * _cols; i++) { + data[i] = 1; + } + return Matrixf<_rows, _cols>(data); +} +// Identity matrix +template +Matrixf<_rows, _cols> eye(void) { + float data[_rows * _cols] = {0}; + for (int i = 0; i < fmin(_rows, _cols); i++) { + data[i * _cols + i] = 1; + } + return Matrixf<_rows, _cols>(data); +} +// Diagonal matrix +template +Matrixf<_rows, _cols> diag(Matrixf<_rows, 1> vec) { + Matrixf<_rows, _cols> res = matrixf::zeros<_rows, _cols>(); + for (int i = 0; i < fmin(_rows, _cols); i++) { + res[i][i] = vec[i][0]; + } + return res; +} + +// Inverse +template +Matrixf<_dim, _dim> inv(Matrixf<_dim, _dim> mat) { + arm_status s; + // extended matrix [A|I] + Matrixf<_dim, 2 * _dim> ext_mat = matrixf::zeros<_dim, 2 * _dim>(); + for (int i = 0; i < _dim; i++) { + memcpy(ext_mat[i], mat[i], _dim * sizeof(float)); + ext_mat[i][_dim + i] = 1; + } + // elimination + for (int i = 0; i < _dim; i++) { + // find maximum absolute value in the first column in lower right block + float abs_max = fabs(ext_mat[i][i]); + int abs_max_row = i; + for (int row = i; row < _dim; row++) { + if (abs_max < fabs(ext_mat[row][i])) { + abs_max = fabs(ext_mat[row][i]); + abs_max_row = row; + } + } + if (abs_max < 1e-12f) { // singular + return matrixf::zeros<_dim, _dim>(); + s = ARM_MATH_SINGULAR; + } + if (abs_max_row != i) { // row exchange + float tmp; + Matrixf<1, 2 * _dim> row_i = ext_mat.row(i); + Matrixf<1, 2 * _dim> row_abs_max = ext_mat.row(abs_max_row); + memcpy(ext_mat[i], row_abs_max[0], 2 * _dim * sizeof(float)); + memcpy(ext_mat[abs_max_row], row_i[0], 2 * _dim * sizeof(float)); + } + float k = 1.f / ext_mat[i][i]; + for (int col = i; col < 2 * _dim; col++) { + ext_mat[i][col] *= k; + } + for (int row = 0; row < _dim; row++) { + if (row == i) { + continue; + } + k = ext_mat[row][i]; + for (int j = i; j < 2 * _dim; j++) { + ext_mat[row][j] -= k * ext_mat[i][j]; + } + } + } + // inv = ext_mat(:,n+1:2n) + s = ARM_MATH_SUCCESS; + Matrixf<_dim, _dim> res; + for (int i = 0; i < _dim; i++) { + memcpy(res[i], &ext_mat[i][_dim], _dim * sizeof(float)); + } + return res; +} + +} // namespace matrixf + +namespace vector3f { + +// hat of vector +Matrixf<3, 3> hat(Matrixf<3, 1> vec); + +// cross product +Matrixf<3, 1> cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2); + +} // namespace vector3f + +#endif // MATRIX_H diff --git a/modules/robotics/robotics.cpp b/modules/robotics/robotics.cpp new file mode 100644 index 0000000..4ff2c6e --- /dev/null +++ b/modules/robotics/robotics.cpp @@ -0,0 +1,340 @@ +/** + ****************************************************************************** + * @file robotics.cpp/h + * @brief Robotic toolbox on STM32. STM32机器人学库 + * @author Spoon Guan + * @ref [1] SJTU ME385-2, Robotics, Y.Ding + * [2] Bruno Siciliano, et al., Robotics: Modelling, Planning and + * Control, Springer, 2010. + * [3] R.Murry, Z.X.Li, and S.Sastry, A Mathematical Introduction + * to Robotic Manipulation, CRC Press, 1994. + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#include "robotics.h" + +Matrixf<3, 1> robotics::r2rpy(Matrixf<3, 3> R) { + float rpy[3] = { + atan2f(R[1][0], R[0][0]), // yaw + atan2f(-R[2][0], sqrtf(R[2][1] * R[2][1] + R[2][2] * R[2][2])), // pitch + atan2f(R[2][1], R[2][2]) // roll + }; + return Matrixf<3, 1>(rpy); +} + +Matrixf<3, 3> robotics::rpy2r(Matrixf<3, 1> rpy) { + float c[3] = {cosf(rpy[0][0]), cosf(rpy[1][0]), cosf(rpy[2][0])}; + float s[3] = {sinf(rpy[0][0]), sinf(rpy[1][0]), sinf(rpy[2][0])}; + float R[9] = { + c[0] * c[1], // R11 + c[0] * s[1] * s[2] - s[0] * c[2], // R12 + c[0] * s[1] * c[2] + s[0] * s[2], // R13 + s[0] * c[1], // R21 + s[0] * s[1] * s[2] + c[0] * c[2], // R22 + s[0] * s[1] * c[2] - c[0] * s[2], // R23 + -s[1], // R31 + c[1] * s[2], // R32 + c[1] * c[2] // R33 + }; + return Matrixf<3, 3>(R); +} + +Matrixf<4, 1> robotics::r2angvec(Matrixf<3, 3> R) { + float theta = acosf(math::limit(0.5f * (R.trace() - 1), -1, 1)); + if (theta == 0 || theta == PI) { + float angvec[4] = { + (1 + R[0][0] - R[1][1] - R[2][2]) / 4.0f, // rx=(1+R11-R22-R33)/4 + (1 - R[0][0] + R[1][1] - R[2][2]) / 4.0f, // ry=(1-R11+R22-R33)/4 + (1 - R[0][0] - R[1][1] + R[2][2]) / 4.0f, // rz=(1-R11-R22+R33)/4 + theta // theta + }; + return Matrixf<4, 1>(angvec); + } + float angvec[4] = { + (R[2][1] - R[1][2]) / (2.0f * sinf(theta)), // rx=(R32-R23)/2sinθ + (R[0][2] - R[2][0]) / (2.0f * sinf(theta)), // ry=(R13-R31)/2sinθ + (R[1][0] - R[0][1]) / (2.0f * sinf(theta)), // rz=(R21-R12)/2sinθ + theta // theta + }; + return Matrixf<4, 1>(angvec); +} + +Matrixf<3, 3> robotics::angvec2r(Matrixf<4, 1> angvec) { + float theta = angvec[3][0]; + Matrixf<3, 1> r = angvec.block<3, 1>(0, 0); + Matrixf<3, 3> R; + // Rodrigues formula: R=I+ω^sinθ+ω^^2(1-cosθ) + R = matrixf::eye<3, 3>() + vector3f::hat(r) * sinf(theta) + + vector3f::hat(r) * vector3f::hat(r) * (1 - cosf(theta)); + return R; +} + +Matrixf<4, 1> robotics::r2quat(Matrixf<3, 3> R) { + float q[4] = { + 0.5f * sqrtf(math::limit(R.trace(), -1, 1) + 1), // q0=cos(θ/2) + math::sign(R[2][1] - R[1][2]) * 0.5f * + sqrtf(math::limit(R[0][0] - R[1][1] - R[2][2], -1, 1) + + 1), // q1=rx*sin(θ/2) + math::sign(R[0][2] - R[2][0]) * 0.5f * + sqrtf(math::limit(-R[0][0] + R[1][1] - R[2][2], -1, 1) + + 1), // q2=ry*sin(θ/2) + math::sign(R[1][0] - R[0][1]) * 0.5f * + sqrtf(math::limit(-R[0][0] - R[1][1] + R[2][2], -1, 1) + + 1), // q3=rz*sin(θ/2) + }; + return (Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm()); +} + +Matrixf<3, 3> robotics::quat2r(Matrixf<4, 1> q) { + float R[9] = { + 1 - 2.0f * (q[2][0] * q[2][0] + q[3][0] * q[3][0]), // R11 + 2.0f * (q[1][0] * q[2][0] - q[0][0] * q[3][0]), // R12 + 2.0f * (q[1][0] * q[3][0] + q[0][0] * q[2][0]), // R13 + 2.0f * (q[1][0] * q[2][0] + q[0][0] * q[3][0]), // R21 + 1 - 2.0f * (q[1][0] * q[1][0] + q[3][0] * q[3][0]), // R22 + 2.0f * (q[2][0] * q[3][0] - q[0][0] * q[1][0]), // R23 + 2.0f * (q[1][0] * q[3][0] - q[0][0] * q[2][0]), // R31 + 2.0f * (q[2][0] * q[3][0] + q[0][0] * q[1][0]), // R32 + 1 - 2.0f * (q[1][0] * q[1][0] + q[2][0] * q[2][0]) // R33 + }; + return Matrixf<3, 3>(R); +} + +Matrixf<3, 1> robotics::quat2rpy(Matrixf<4, 1> q) { + float rpy[3] = { + atan2f(2.0f * (q[1][0] * q[2][0] + q[0][0] * q[3][0]), + 1 - 2.0f * (q[2][0] * q[2][0] + q[3][0] * q[3][0])), // yaw + asinf(2.0f * (q[0][0] * q[2][0] - q[1][0] * q[3][0])), // pitch + atan2f(2.0f * (q[2][0] * q[3][0] + q[0][0] * q[1][0]), + 1 - 2.0f * (q[1][0] * q[1][0] + q[2][0] * q[2][0])) // rol + }; + return Matrixf<3, 1>(rpy); +} + +Matrixf<4, 1> robotics::rpy2quat(Matrixf<3, 1> rpy) { + float c[3] = {cosf(0.5f * rpy[0][0]), cosf(0.5f * rpy[1][0]), + cosf(0.5f * rpy[2][0])}; // cos(*/2) + float s[3] = {sinf(0.5f * rpy[0][0]), sinf(0.5f * rpy[1][0]), + sinf(0.5f * rpy[2][0])}; // sin(*/2) + float q[4] = { + c[0] * c[1] * c[2] + s[0] * s[1] * s[2], // q0=cos(θ/2) + c[0] * c[1] * s[2] - s[0] * s[1] * c[2], // q1=rx*sin(θ/2) + c[0] * s[1] * c[2] + s[0] * c[1] * s[2], // q2=ry*sin(θ/2) + s[0] * c[1] * c[2] - c[0] * s[1] * s[2] // q3=rz*sin(θ/2) + }; + return (Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm()); +} + +Matrixf<4, 1> robotics::quat2angvec(Matrixf<4, 1> q) { + float cosq0; + float theta = 2.0f * acosf(math::limit(q[0][0], -1, 1)); + if (theta == 0 || theta == PI) { + float angvec[4] = {0, 0, 0, theta}; // θ=0||PI, return[0;θ] + return Matrixf<4, 1>(angvec); + } + Matrixf<3, 1> vec = q.block<3, 1>(1, 0); + float angvec[4] = { + vec[0][0] / vec.norm(), // rx + vec[1][0] / vec.norm(), // ry + vec[2][0] / vec.norm(), // rz + theta // theta + }; + return Matrixf<4, 1>(angvec); +} + +Matrixf<4, 1> robotics::angvec2quat(Matrixf<4, 1> angvec) { + float c = cosf(0.5f * angvec[3][0]), s = sinf(0.5f * angvec[3][0]); + float q[4] = { + c, // q0=cos(θ/2) + s * angvec[0][0], // q1=rx*sin(θ/2) + s * angvec[1][0], // q2=ry*sin(θ/2) + s * angvec[2][0] // q3=rz*sin(θ/2) + }; + return Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm(); +} + +Matrixf<3, 3> robotics::t2r(Matrixf<4, 4> T) { + return T.block<3, 3>(0, 0); // R=T(1:3,1:3) +} + +Matrixf<4, 4> robotics::r2t(Matrixf<3, 3> R) { + // T=[R,0;0,1] + float T[16] = {R[0][0], R[0][1], R[0][2], 0, R[1][0], R[1][1], R[1][2], 0, + R[2][0], R[2][1], R[2][2], 0, 0, 0, 0, 1}; + return Matrixf<4, 4>(T); +} + +Matrixf<3, 1> robotics::t2p(Matrixf<4, 4> T) { + return T.block<3, 1>(0, 3); // p=T(1:3,4) +} + +Matrixf<4, 4> robotics::p2t(Matrixf<3, 1> p) { + // T=[I,P;0,1] + float T[16] = {1, 0, 0, p[0][0], 0, 1, 0, p[1][0], + 0, 0, 1, p[2][0], 0, 0, 0, 1}; + return Matrixf<4, 4>(T); +} + +Matrixf<4, 4> robotics::rp2t(Matrixf<3, 3> R, Matrixf<3, 1> p) { + // T=[R,P;0,1] + float T[16] = {R[0][0], R[0][1], R[0][2], p[0][0], R[1][0], R[1][1], + R[1][2], p[1][0], R[2][0], R[2][1], R[2][2], p[2][0], + 0, 0, 0, 1}; + return Matrixf<4, 4>(T); +} + +Matrixf<4, 4> robotics::invT(Matrixf<4, 4> T) { + Matrixf<3, 3> RT = t2r(T).trans(); + Matrixf<3, 1> p_ = -1.0f * RT * t2p(T); + float invT[16] = {RT[0][0], RT[0][1], RT[0][2], p_[0][0], RT[1][0], RT[1][1], + RT[1][2], p_[1][0], RT[2][0], RT[2][1], RT[2][2], p_[2][0], + 0, 0, 0, 1}; + return Matrixf<4, 4>(invT); +} + +Matrixf<3, 1> robotics::t2rpy(Matrixf<4, 4> T) { + return r2rpy(t2r(T)); +} + +Matrixf<4, 4> robotics::rpy2t(Matrixf<3, 1> rpy) { + return r2t(rpy2r(rpy)); +} + +Matrixf<4, 1> robotics::t2angvec(Matrixf<4, 4> T) { + return r2angvec(t2r(T)); +} + +Matrixf<4, 4> robotics::angvec2t(Matrixf<4, 1> angvec) { + return r2t(angvec2r(angvec)); +} + +Matrixf<4, 1> robotics::t2quat(Matrixf<4, 4> T) { + return r2quat(t2r(T)); +} + +Matrixf<4, 4> robotics::quat2t(Matrixf<4, 1> quat) { + return r2t(quat2r(quat)); +} + +Matrixf<6, 1> robotics::t2twist(Matrixf<4, 4> T) { + Matrixf<3, 1> p = t2p(T); + Matrixf<4, 1> angvec = t2angvec(T); + Matrixf<3, 1> phi = angvec.block<3, 1>(0, 0) * angvec[3][0]; + float twist[6] = {p[0][0], p[1][0], p[2][0], phi[0][0], phi[1][0], phi[2][0]}; + return Matrixf<6, 1>(twist); +} + +Matrixf<4, 4> robotics::twist2t(Matrixf<6, 1> twist) { + Matrixf<3, 1> p = twist.block<3, 1>(0, 0); + float theta = twist.block<3, 1>(3, 0).norm(); + float angvec[4] = {0, 0, 0, theta}; + if (theta != 0) { + angvec[0] = twist[3][0] / theta; + angvec[1] = twist[4][0] / theta; + angvec[2] = twist[5][0] / theta; + } + return rp2t(angvec2r(angvec), p); +} + +Matrixf<4, 4> robotics::DH_t::fkine() { + float ct = cosf(theta), st = sinf(theta); // cosθ, sinθ + float ca = cosf(alpha), sa = sinf(alpha); // cosα, sinα + + // T = + // | cθ -sθcα sθsα acθ | + // | sθ cθcα -cθsα asθ | + // | 0 sα cα d | + // | 0 0 0 1 | + T[0][0] = ct; + T[0][1] = -st * ca; + T[0][2] = st * sa; + T[0][3] = a * ct; + + T[1][0] = st; + T[1][1] = ct * ca; + T[1][2] = -ct * sa; + T[1][3] = a * st; + + T[2][0] = 0; + T[2][1] = sa; + T[2][2] = ca; + T[2][3] = d; + + T[3][0] = 0; + T[3][1] = 0; + T[3][2] = 0; + T[3][3] = 1; + + return T; +} + +robotics::Link::Link(float theta, + float d, + float a, + float alpha, + robotics::Joint_Type_e type, + float offset, + float qmin, + float qmax, + float m, + Matrixf<3, 1> rc, + Matrixf<3, 3> I) { + dh_.theta = theta; + dh_.d = d; + dh_.alpha = alpha; + dh_.a = a; + type_ = type; + offset_ = offset; + qmin_ = qmin; + qmax_ = qmax; + m_ = m; + rc_ = rc; + I_ = I; +} + +robotics::Link::Link(const Link& link) { + dh_.theta = link.dh_.theta; + dh_.d = link.dh_.d; + dh_.alpha = link.dh_.alpha; + dh_.a = link.dh_.a; + type_ = link.type_; + offset_ = link.offset_; + qmin_ = link.qmin_; + qmax_ = link.qmax_; + m_ = link.m_; + rc_ = link.rc_; + I_ = link.I_; +} + +robotics::Link& robotics::Link::operator=(Link link) { + dh_.theta = link.dh_.theta; + dh_.d = link.dh_.d; + dh_.alpha = link.dh_.alpha; + dh_.a = link.dh_.a; + type_ = link.type_; + offset_ = link.offset_; + qmin_ = link.qmin_; + qmax_ = link.qmax_; + m_ = link.m_; + rc_ = link.rc_; + I_ = link.I_; + return *this; +} + +Matrixf<4, 4> robotics::Link::T(float q) { + if (type_ == R) { + if (qmin_ >= qmax_) + dh_.theta = q + offset_; + else + dh_.theta = math::limit(q + offset_, qmin_, qmax_); + } else { + if (qmin_ >= qmax_) + dh_.d = q + offset_; + else + dh_.d = math::limit(q + offset_, qmin_, qmax_); + } + return dh_.fkine(); +} diff --git a/modules/robotics/robotics.h b/modules/robotics/robotics.h new file mode 100644 index 0000000..373a437 --- /dev/null +++ b/modules/robotics/robotics.h @@ -0,0 +1,407 @@ +/** + ****************************************************************************** + * @file robotics.cpp/h + * @brief Robotic toolbox on STM32. STM32机器人学库 + * @author Spoon Guan + * @ref [1] SJTU ME385-2, Robotics, Y.Ding + * [2] Bruno Siciliano, et al., Robotics: Modelling, Planning and + * Control, Springer, 2010. + * [3] R.Murry, Z.X.Li, and S.Sastry, A Mathematical Introduction + * to Robotic Manipulation, CRC Press, 1994. + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#ifndef ROBOTICS_H +#define ROBOTICS_H + +#include "utils.h" +#include "matrix.h" + +namespace robotics { +// rotation matrix(R) -> RPY([yaw;pitch;roll]) +Matrixf<3, 1> r2rpy(Matrixf<3, 3> R); +// RPY([yaw;pitch;roll]) -> rotation matrix(R) +Matrixf<3, 3> rpy2r(Matrixf<3, 1> rpy); +// rotation matrix(R) -> angle vector([r;θ]) +Matrixf<4, 1> r2angvec(Matrixf<3, 3> R); +// angle vector([r;θ]) -> rotation matrix(R) +Matrixf<3, 3> angvec2r(Matrixf<4, 1> angvec); +// rotation matrix(R) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] +Matrixf<4, 1> r2quat(Matrixf<3, 3> R); +// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> rotation matrix(R) +Matrixf<3, 3> quat2r(Matrixf<4, 1> quat); +// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> RPY([yaw;pitch;roll]) +Matrixf<3, 1> quat2rpy(Matrixf<4, 1> q); +// RPY([yaw;pitch;roll]) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] +Matrixf<4, 1> rpy2quat(Matrixf<3, 1> rpy); +// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> angle vector([r;θ]) +Matrixf<4, 1> quat2angvec(Matrixf<4, 1> q); +// angle vector([r;θ]) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] +Matrixf<4, 1> angvec2quat(Matrixf<4, 1> angvec); +// homogeneous transformation matrix(T) -> rotation matrix(R) +Matrixf<3, 3> t2r(Matrixf<4, 4> T); +// rotation matrix(R) -> homogeneous transformation matrix(T) +Matrixf<4, 4> r2t(Matrixf<3, 3> R); +// homogeneous transformation matrix(T) -> translation vector(p) +Matrixf<3, 1> t2p(Matrixf<4, 4> T); +// translation vector(p) -> homogeneous transformation matrix(T) +Matrixf<4, 4> p2t(Matrixf<3, 1> p); +// rotation matrix(R) & translation vector(p) -> homogeneous transformation +// matrix(T) +Matrixf<4, 4> rp2t(Matrixf<3, 3> R, Matrixf<3, 1> p); +// homogeneous transformation matrix(T) -> RPY([yaw;pitch;roll]) +Matrixf<3, 1> t2rpy(Matrixf<4, 4> T); +// inverse of homogeneous transformation matrix(T^-1=[R',-R'P;0,1]) +Matrixf<4, 4> invT(Matrixf<4, 4> T); +// RPY([yaw;pitch;roll]) -> homogeneous transformation matrix(T) +Matrixf<4, 4> rpy2t(Matrixf<3, 1> rpy); +// homogeneous transformation matrix(T) -> angle vector([r;θ]) +Matrixf<4, 1> t2angvec(Matrixf<4, 4> T); +// angle vector([r;θ]) -> homogeneous transformation matrix(T) +Matrixf<4, 4> angvec2t(Matrixf<4, 1> angvec); +// homogeneous transformation matrix(T) -> quaternion, +// [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] +Matrixf<4, 1> t2quat(Matrixf<4, 4> T); +// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> homogeneous transformation +// matrix(T) +Matrixf<4, 4> quat2t(Matrixf<4, 1> quat); +// homogeneous transformation matrix(T) -> twist coordinates vector ([p;rθ]) +Matrixf<6, 1> t2twist(Matrixf<4, 4> T); +// twist coordinates vector ([p;rθ]) -> homogeneous transformation matrix(T) +Matrixf<4, 4> twist2t(Matrixf<6, 1> twist); + +// joint type: R-revolute joint, P-prismatic joint +typedef enum joint_type { + R = 0, + P = 1, +} Joint_Type_e; + +// Denavit–Hartenberg(DH) method +struct DH_t { + // forward kinematic + Matrixf<4, 4> fkine(); + // DH parameter + float theta; + float d; + float a; + float alpha; + Matrixf<4, 4> T; +}; + +class Link { + public: + Link(){}; + Link(float theta, float d, float a, float alpha, Joint_Type_e type = R, + float offset = 0, float qmin = 0, float qmax = 0, float m = 1, + Matrixf<3, 1> rc = matrixf::zeros<3, 1>(), + Matrixf<3, 3> I = matrixf::zeros<3, 3>()); + Link(const Link& link); + + Link& operator=(Link link); + + float qmin() { return qmin_; } + float qmax() { return qmax_; } + Joint_Type_e type() { return type_; } + float m() { return m_; } + Matrixf<3, 1> rc() { return rc_; } + Matrixf<3, 3> I() { return I_; } + + Matrixf<4, 4> T(float q); // forward kinematic + + public: + // kinematic parameter + DH_t dh_; + float offset_; + // limit(qmin,qmax), no limit if qmin<=qmax + float qmin_; + float qmax_; + // joint type + Joint_Type_e type_; + // dynamic parameter + float m_; // mass + Matrixf<3, 1> rc_; // centroid(link coordinate) + Matrixf<3, 3> I_; // inertia tensor(3*3) +}; + +template +class Serial_Link { + public: + Serial_Link(Link links[_n]) { + for (int i = 0; i < _n; i++) + links_[i] = links[i]; + gravity_ = matrixf::zeros<3, 1>(); + gravity_[2][0] = -9.81f; + } + + Serial_Link(Link links[_n], Matrixf<3, 1> gravity) { + for (int i = 0; i < _n; i++) + links_[i] = links[i]; + gravity_ = gravity; + } + + // forward kinematic: T_n^0 + // param[in] q: joint variable vector + // param[out] T_n^0 + Matrixf<4, 4> fkine(Matrixf<_n, 1> q) { + T_ = matrixf::eye<4, 4>(); + for (int iminus1 = 0; iminus1 < _n; iminus1++) + T_ = T_ * links_[iminus1].T(q[iminus1][0]); + return T_; + } + + // forward kinematic: T_k^0 + // param[in] q: joint variable vector + // param[in] k: joint number + // param[out] T_k^0 + Matrixf<4, 4> fkine(Matrixf<_n, 1> q, uint16_t k) { + if (k > _n) + k = _n; + Matrixf<4, 4> T = matrixf::eye<4, 4>(); + for (int iminus1 = 0; iminus1 < k; iminus1++) + T = T * links_[iminus1].T(q[iminus1][0]); + return T; + } + + // T_k^k-1: homogeneous transformation matrix of link k + // param[in] q: joint variable vector + // param[in] kminus: joint number k, input k-1 + // param[out] T_k^k-1 + Matrixf<4, 4> T(Matrixf<_n, 1> q, uint16_t kminus1) { + if (kminus1 >= _n) + kminus1 = _n - 1; + return links_[kminus1].T(q[kminus1][0]); + } + + // jacobian matrix, J_i = [J_pi;j_oi] + // param[in] q: joint variable vector + // param[out] jacobian matix J_6*n + Matrixf<6, _n> jacob(Matrixf<_n, 1> q) { + Matrixf<3, 1> p_e = t2p(fkine(q)); // p_e + Matrixf<4, 4> T_iminus1 = matrixf::eye<4, 4>(); // T_i-1^0 + Matrixf<3, 1> z_iminus1; // z_i-1^0 + Matrixf<3, 1> p_iminus1; // p_i-1^0 + Matrixf<3, 1> J_pi; + Matrixf<3, 1> J_oi; + for (int iminus1 = 0; iminus1 < _n; iminus1++) { + // revolute joint: J_pi = z_i-1x(p_e-p_i-1), J_oi = z_i-1 + if (links_[iminus1].type() == R) { + z_iminus1 = T_iminus1.block<3, 1>(0, 2); + p_iminus1 = t2p(T_iminus1); + T_iminus1 = T_iminus1 * links_[iminus1].T(q[iminus1][0]); + J_pi = vector3f::cross(z_iminus1, p_e - p_iminus1); + J_oi = z_iminus1; + } + // prismatic joint: J_pi = z_i-1, J_oi = 0 + else { + z_iminus1 = T_iminus1.block<3, 1>(0, 2); + T_iminus1 = T_iminus1 * links_[iminus1].T(q[iminus1][0]); + J_pi = z_iminus1; + J_oi = matrixf::zeros<3, 1>(); + } + J_[0][iminus1] = J_pi[0][0]; + J_[1][iminus1] = J_pi[1][0]; + J_[2][iminus1] = J_pi[2][0]; + J_[3][iminus1] = J_oi[0][0]; + J_[4][iminus1] = J_oi[1][0]; + J_[5][iminus1] = J_oi[2][0]; + } + return J_; + } + + // inverse kinematic, numerical solution(Newton method) + // param[in] T: homogeneous transformation matrix of end effector + // param[in] q: initial joint variable vector(q0) for Newton method's + // iteration + // param[in] tol: tolerance of error (norm(error of twist vector)) + // param[in] max_iter: maximum iterations, default 30 + // param[out] q: joint variable vector + Matrixf<_n, 1> ikine(Matrixf<4, 4> Td, + Matrixf<_n, 1> q = matrixf::zeros<_n, 1>(), + float tol = 1e-4f, uint16_t max_iter = 50) { + Matrixf<4, 4> T; + Matrixf<3, 1> pe, we; + Matrixf<6, 1> err, new_err; + Matrixf<_n, 1> dq; + float step = 1; + for (int i = 0; i < max_iter; i++) { + T = fkine(q); + pe = t2p(Td) - t2p(T); + // angvec(Td*T^-1), transform angular vector(T->Td) in world coordinate + we = t2twist(Td * invT(T)).block<3, 1>(3, 0); + for (int i = 0; i < 3; i++) { + err[i][0] = pe[i][0]; + err[i + 3][0] = we[i][0]; + } + if (err.norm() < tol) + return q; + // adjust iteration step + Matrixf<6, _n> J = jacob(q); + for (int j = 0; j < 5; j++) { + dq = matrixf::inv(J.trans() * J) * (J.trans() * err) * step; + if (dq[0][0] == INFINITY) // J'*J singular + { + dq = matrixf::inv(J.trans() * J + 0.1f * matrixf::eye<_n, _n>()) * + J.trans() * err * step; + // SVD<6, _n> JTJ_svd(J.trans() * J); + // dq = JTJ_svd.solve(err) * step * 5e-2f; + q += dq; + for (int i = 0; i < _n; i++) { + if (links_[i].type() == R) + q[i][0] = math::loopLimit(q[i][0], -PI, PI); + } + break; + } + T = fkine(q + dq); + pe = t2p(Td) - t2p(T); + we = t2twist(Td * invT(T)).block<3, 1>(3, 0); + for (int i = 0; i < 3; i++) { + new_err[i][0] = pe[i][0]; + new_err[i + 3][0] = we[i][0]; + } + if (new_err.norm() < err.norm()) { + q += dq; + for (int i = 0; i < _n; i++) { + if (links_[i].type() == robotics::Joint_Type_e::R) { + q[i][0] = math::loopLimit(q[i][0], -PI, PI); + } + } + break; + } else { + step /= 2.0f; + } + } + if (step < 1e-3f) + return q; + } + return q; + } + + // (Reserved function) inverse kinematic, analytic solution(geometric method) + Matrixf<_n, 1> (*ikine_analytic)(Matrixf<4, 4> T); + + // inverse dynamic, Newton-Euler method + // param[in] q: joint variable vector + // param[in] qv: dq/dt + // param[in] qa: d^2q/dt^2 + // param[in] he: load on end effector [f;μ], default 0 + Matrixf<_n, 1> rne(Matrixf<_n, 1> q, + Matrixf<_n, 1> qv = matrixf::zeros<_n, 1>(), + Matrixf<_n, 1> qa = matrixf::zeros<_n, 1>(), + Matrixf<6, 1> he = matrixf::zeros<6, 1>()) { + // forward propagation + // record each links' motion state in matrices + // [ωi] angular velocity + Matrixf<3, _n + 1> w = matrixf::zeros<3, _n + 1>(); + // [βi] angular acceleration + Matrixf<3, _n + 1> b = matrixf::zeros<3, _n + 1>(); + // [pi] position of joint + Matrixf<3, _n + 1> p = matrixf::zeros<3, _n + 1>(); + // [vi] velocity of joint + Matrixf<3, _n + 1> v = matrixf::zeros<3, _n + 1>(); + // [ai] acceleration of joint + Matrixf<3, _n + 1> a = matrixf::zeros<3, _n + 1>(); + // [aci] acceleration of mass center + Matrixf<3, _n + 1> ac = matrixf::zeros<3, _n + 1>(); + // temperary vectors + Matrixf<3, 1> w_i, b_i, p_i, v_i, ai, ac_i; + // i & i-1 coordinate convert to 0 coordinate + Matrixf<4, 4> T_0i = matrixf::eye<4, 4>(); + Matrixf<4, 4> T_0iminus1 = matrixf::eye<4, 4>(); + Matrixf<3, 3> R_0i = matrixf::eye<3, 3>(); + Matrixf<3, 3> R_0iminus1 = matrixf::eye<3, 3>(); + // unit vector of z-axis + Matrixf<3, 1> ez = matrixf::zeros<3, 1>(); + ez[2][0] = 1; + + for (int i = 1; i <= _n; i++) { + T_0i = T_0i * T(q, i - 1); // T_i^0 + R_0i = t2r(T_0i); // R_i^0 + R_0iminus1 = t2r(T_0iminus1); // R_i-1^0 + // ω_i = ω_i-1+qv_i*R_i-1^0*ez + w_i = w.col(i - 1) + qv[i - 1][0] * R_0iminus1 * ez; + // β_i = β_i-1+ω_i-1x(qv_i*R_i-1^0*ez)+qa_i*R_i-1^0*ez + b_i = b.col(i - 1) + + vector3f::cross(w.col(i - 1), qv[i - 1][0] * R_0iminus1 * ez) + + qa[i - 1][0] * R_0iminus1 * ez; + p_i = t2p(T_0i); // p_i = T_i^0(1:3,4) + // v_i = v_i-1+ω_ix(p_i-p_i-1) + v_i = v.col(i - 1) + vector3f::cross(w_i, p_i - p.col(i - 1)); + // a_i = a_i-1+β_ix(p_i-p_i-1)+ω_ix(ω_ix(p_i-p_i-1)) + ai = a.col(i - 1) + vector3f::cross(b_i, p_i - p.col(i - 1)) + + vector3f::cross(w_i, vector3f::cross(w_i, p_i - p.col(i - 1))); + // ac_i = a_i+β_ix(R_0^i*rc_i^i)+ω_ix(ω_ix(R_0^i*rc_i^i)) + ac_i = + ai + vector3f::cross(b_i, R_0i * links_[i - 1].rc()) + + vector3f::cross(w_i, vector3f::cross(w_i, R_0i * links_[i - 1].rc())); + for (int row = 0; row < 3; row++) { + w[row][i] = w_i[row][0]; + b[row][i] = b_i[row][0]; + p[row][i] = p_i[row][0]; + v[row][i] = v_i[row][0]; + a[row][i] = ai[row][0]; + ac[row][i] = ac_i[row][0]; + } + T_0iminus1 = T_0i; // T_i-1^0 + } + + // backward propagation + // record each links' force + Matrixf<3, _n + 1> f = matrixf::zeros<3, _n + 1>(); // joint force + Matrixf<3, _n + 1> mu = matrixf::zeros<3, _n + 1>(); // joint moment + // temperary vector + Matrixf<3, 1> f_iminus1, mu_iminus1; + // {T,R',P}_i^i-1 + Matrixf<4, 4> T_iminus1i; + Matrixf<3, 3> RT_iminus1i; + Matrixf<3, 1> P_iminus1i; + // I_i-1(in 0 coordinate) + Matrixf<3, 3> I_i; + // joint torque + Matrixf<_n, 1> torq; + + // load on end effector + for (int row = 0; row < 3; row++) { + f[row][_n] = he.block<3, 1>(0, 0)[row][0]; + mu[row][_n] = he.block<3, 1>(3, 0)[row][0]; + } + for (int i = _n; i > 0; i--) { + T_iminus1i = T(q, i - 1); // T_i^i-1 + P_iminus1i = t2p(T_iminus1i); // P_i^i-1 + RT_iminus1i = t2r(T_iminus1i).trans(); // R_i^i-1' + R_0iminus1 = R_0i * RT_iminus1i; // R_i-1^0 + // I_i^0 = R_i^0*I_i^i*(R_i^0)' + I_i = R_0i * links_[i - 1].I() * R_0i.trans(); + // f_i-1 = f_i+m_i*ac_i-m_i*g + f_iminus1 = f.col(i) + links_[i - 1].m() * ac.col(i) - + links_[i - 1].m() * gravity_; + // μ_i-1 = μ_i+f_ixrc_i-f_i-1xrc_i-1->ci+I_i*b_i+ω_ix(I_i*ω_i) + mu_iminus1 = mu.col(i) + + vector3f::cross(f.col(i), R_0i * links_[i - 1].rc()) - + vector3f::cross(f_iminus1, R_0i * (RT_iminus1i * P_iminus1i + + links_[i - 1].rc())) + + I_i * b.col(i) + vector3f::cross(w.col(i), I_i * w.col(i)); + // τ_i = μ_i-1'*(R_i-1^0*ez) + torq[i - 1][0] = (mu_iminus1.trans() * R_0iminus1 * ez)[0][0]; + for (int row = 0; row < 3; row++) { + f[row][i - 1] = f_iminus1[row][0]; + mu[row][i - 1] = mu_iminus1[row][0]; + } + R_0i = R_0iminus1; + } + + return torq; + } + + private: + Link links_[_n]; + Matrixf<3, 1> gravity_; + + Matrixf<4, 4> T_; + Matrixf<6, _n> J_; +}; +}; // namespace robotics + +#endif // ROBOTICS_H diff --git a/modules/robotics/utils.cpp b/modules/robotics/utils.cpp new file mode 100644 index 0000000..d1b6fd4 --- /dev/null +++ b/modules/robotics/utils.cpp @@ -0,0 +1,56 @@ +/** + ****************************************************************************** + * @file utils.cpp/h + * @brief General math utils. 常用数学工具函数 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#include "utils.h" + +float math::limit(float val, const float& min, const float& max) { + if (min > max) + return val; + else if (val < min) + val = min; + else if (val > max) + val = max; + return val; +} + +float math::limitMin(float val, const float& min) { + if (val < min) + val = min; + return val; +} + +float math::limitMax(float val, const float& max) { + if (val > max) + val = max; + return val; +} + +float math::loopLimit(float val, const float& min, const float& max) { + if (min >= max) + return val; + if (val > max) { + while (val > max) + val -= (max - min); + } else if (val < min) { + while (val < min) + val += (max - min); + } + return val; +} + +float math::sign(const float& val) { + if (val > 0) + return 1; + else if (val < 0) + return -1; + return 0; +} + diff --git a/modules/robotics/utils.h b/modules/robotics/utils.h new file mode 100644 index 0000000..0a08cf3 --- /dev/null +++ b/modules/robotics/utils.h @@ -0,0 +1,27 @@ +/** + ****************************************************************************** + * @file utils.cpp/h + * @brief General math utils. 常用数学工具函数 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#ifndef UTILS_H +#define UTILS_H + +#include + +namespace math { + +float limit(float val, const float& min, const float& max); +float limitMin(float val, const float& min); +float limitMax(float val, const float& max); +float loopLimit(float val, const float& min, const float& max); +float sign(const float& val); + +} // namespace math + +#endif // UTILS_H diff --git a/modules/super_cap/super_cap.c b/modules/super_cap/super_cap.c index 15b2c87..ed07413 100644 --- a/modules/super_cap/super_cap.c +++ b/modules/super_cap/super_cap.c @@ -1,14 +1,8 @@ -/* - * @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) @@ -17,9 +11,10 @@ static void SuperCapRxCallback(CANInstance *_instance) SuperCap_Msg_s *Msg; rxbuff = _instance->rx_buff; Msg = &super_cap_instance->cap_msg; - 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]); + 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]); } SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config) @@ -29,6 +24,8 @@ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config) supercap_config->can_config.can_module_callback = SuperCapRxCallback; super_cap_instance->can_ins = CANRegister(&supercap_config->can_config); + + PIDInit(&super_cap_instance->buffer_pid, &supercap_config->buffer_config_pid); return super_cap_instance; } @@ -38,6 +35,15 @@ 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; diff --git a/modules/super_cap/super_cap.h b/modules/super_cap/super_cap.h index 78157c0..4f460cf 100644 --- a/modules/super_cap/super_cap.h +++ b/modules/super_cap/super_cap.h @@ -1,35 +1,32 @@ -/* - * @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 #include "bsp_can.h" +#include "controller.h" #pragma pack(1) typedef struct { - uint16_t vol; // 电压 - uint16_t current; // 电流 - uint16_t power; // 功率 + int16_t input_vol; // 输入电压 + int16_t cap_vol; // 电容电压 + int16_t input_cur; // 输入电流 + int16_t power_set; // 设定功率 } SuperCap_Msg_s; #pragma pack() /* 超级电容实例 */ typedef struct { - CANInstance *can_ins; // CAN实例 + CANInstance *can_ins; // CAN实例 SuperCap_Msg_s cap_msg; // 超级电容信息 + PIDInstance buffer_pid; // 对缓冲功率进行闭环 } SuperCapInstance; /* 超级电容初始化配置 */ typedef struct { CAN_Init_Config_s can_config; + PID_Init_Config_s buffer_config_pid; } SuperCap_Init_Config_s; /** @@ -48,4 +45,12 @@ 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 diff --git a/modules/vofa/vofa.c b/modules/vofa/vofa.c index 298bbcf..6e30ee0 100644 --- a/modules/vofa/vofa.c +++ b/modules/vofa/vofa.c @@ -6,6 +6,13 @@ * @LastEditTime: 2022-12-05 14:15:53 */ #include "vofa.h" +#include "usbd_cdc_if.h" + +typedef union +{ + float float_t; + uint8_t uint8_t[4]; +} send_float; /*VOFA浮点协议*/ void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart ) @@ -29,5 +36,57 @@ void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart send_data[4 * num + 2] = 0x80; send_data[4 * num + 3] = 0x7f; //加上协议要求的4个尾巴 - HAL_UART_Transmit(huart, (uint8_t *)send_data, 4 * num + 4, 100); + //HAL_UART_Transmit(huart, (uint8_t *)send_data, 4 * num + 4, 100); + CDC_Transmit_FS((uint8_t *)send_data,4 * num + 4); +} + +#define BYTE0(dwTemp) (*(char*)(&dwTemp)) +#define BYTE1(dwTemp) (*((char*)(&dwTemp)+1)) +#define BYTE2(dwTemp) (*((char*)(&dwTemp)+2)) +#define BYTE3(dwTemp) (*((char*)(&dwTemp)+3)) + +uint8_t DataSendBuf[100]; +//匿名上位机 +void ANODT_SendF1(int32_t Angle,int32_t speed_rpm,int32_t Angle_target,int32_t speed_target)//F1灵活格式帧 +{ + uint8_t cnt=0; + DataSendBuf[cnt++]=0xAA; //帧头 + DataSendBuf[cnt++]=0xFF; //目标地址 + DataSendBuf[cnt++]=0xF1; //功能码 + DataSendBuf[cnt++]=16; //数据长度 + + DataSendBuf[cnt++]=BYTE0(Angle); + DataSendBuf[cnt++]=BYTE1(Angle); + DataSendBuf[cnt++]=BYTE2(Angle); + DataSendBuf[cnt++]=BYTE3(Angle); + + DataSendBuf[cnt++]=BYTE0(speed_rpm); + DataSendBuf[cnt++]=BYTE1(speed_rpm); + DataSendBuf[cnt++]=BYTE2(speed_rpm); + DataSendBuf[cnt++]=BYTE3(speed_rpm); + + DataSendBuf[cnt++]=BYTE0(Angle_target); + DataSendBuf[cnt++]=BYTE1(Angle_target); + DataSendBuf[cnt++]=BYTE2(Angle_target); + DataSendBuf[cnt++]=BYTE3(Angle_target); + + DataSendBuf[cnt++]=BYTE0(speed_target); + DataSendBuf[cnt++]=BYTE1(speed_target); + DataSendBuf[cnt++]=BYTE2(speed_target); + DataSendBuf[cnt++]=BYTE3(speed_target); + + uint8_t sc=0; //和校验 + uint8_t ac=0; //附加校验 + for(uint8_t i=0;i