HFUT_Firmament_EC
/
basic_framework
12
0
Fork 0
Code Issues 1 Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: HFUT_Firmament_EC:master
Branches Tags
HFUT_Firmament_EC:master
HFUT_Firmament_EC:standard3_sjq
HFUT_Firmament_EC:Hero_zcj_test
HFUT_Firmament_EC:Omnidirectional-infantry
HFUT_Firmament_EC:Hero_zcj
HFUT_Firmament_EC:SPEED_FEEDFORWARD__LTS
..
compare: HFUT_Firmament_EC:Hero_zcj
Branches Tags
HFUT_Firmament_EC:master
HFUT_Firmament_EC:standard3_sjq
HFUT_Firmament_EC:Hero_zcj_test
HFUT_Firmament_EC:Omnidirectional-infantry
HFUT_Firmament_EC:Hero_zcj
HFUT_Firmament_EC:SPEED_FEEDFORWARD__LTS

5 Commits
master ... Hero_zcj

Author SHA1 Message Date
周楚杰 7fc13a67e6 底盘跟随，5V激光，拨弹轮 2024-01-26 17:25:52 +08:00
周楚杰 c2388e74e5 PITCH云台，小陀螺 2024-01-16 19:51:18 +08:00
周楚杰 0bf1ba7f3e 底盘代码逻辑调整完成，未调参数 2023-11-19 18:19:37 +08:00
周楚杰 29c6b56489 完成PID参数的调试 2023-10-19 21:08:43 +08:00
周楚杰 0154d1164a 完成英雄YAW和PITCH轴的简单调试，PID参数需要重新调试 2023-10-19 20:33:23 +08:00
56 changed files with 993 additions and 4548 deletions
Show Stats Expand all files Collapse all files

4
.idea/runConfigurations/OCD_basic_framework.xml
View File

@ -1,6 +1,6 @@
<component name="ProjectRunConfigurationManager"> <component name="ProjectRunConfigurationManager">
<configuration default="false" name="OCD basic_framework" type="com.jetbrains.cidr.embedded.openocd.conf.type" factoryName="com.jetbrains.cidr.embedded.openocd.conf.factory" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="basic_framework" TARGET_NAME="basic_framework.elf" CONFIG_NAME="Debug" version="1" RUN_TARGET_PROJECT_NAME="basic_framework" RUN_TARGET_NAME="basic_framework.elf"> <configuration default="false" name="OCD basic_framework" type="com.jetbrains.cidr.embedded.openocd.conf.type" factoryName="com.jetbrains.cidr.embedded.openocd.conf.factory" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="basic_framework" TARGET_NAME="basic_framework.elf" CONFIG_NAME="Debug" version="1" RUN_TARGET_PROJECT_NAME="basic_framework" RUN_TARGET_NAME="basic_framework.elf">
<openocd version="1" gdb-port="3333" telnet-port="4444" board-config="D:\CLion\Project\basic_framework\daplink.cfg" reset-type="INIT" download-type="UPDATED_ONLY"> <openocd version="1" gdb-port="3333" telnet-port="4444" board-config="C:\Users\24871\Desktop\RM\RM2024\basic_framework\daplink.cfg" reset-type="INIT" download-type="UPDATED_ONLY">
<debugger kind="GDB" isBundled="true" /> <debugger kind="GDB" isBundled="true" />
</openocd> </openocd>
<method v="2"> <method v="2">

6
CMakeLists.txt
View File

@ -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 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 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 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/RGB modules/standard_cmd 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/auto_aim 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/motor/ECmotor modules/motor/DMmotor modules/motor/DJImotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor
application application/chassis application/cmd application/gimbal application/shoot application application/chassis application/cmd application/gimbal application/shoot
Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config) Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config)

3
Inc/main.h
View File

@ -31,8 +31,7 @@ extern "C" {
/* Private includes ----------------------------------------------------------*/ /* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */ /* USER CODE BEGIN Includes */
#include "robot.h"
#include "bsp_log.h"
/* USER CODE END Includes */ /* USER CODE END Includes */
/* Exported types ------------------------------------------------------------*/ /* Exported types ------------------------------------------------------------*/

2
Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c vendored
View File

@ -26,7 +26,7 @@
* *
*---------------------------------------------------------------------------- *----------------------------------------------------------------------------
* *
* Portions Copyright © 2016 STMicroelectronics International N.V. All rights reserved. * Portions Copyright <EFBFBD> 2016 STMicroelectronics International N.V. All rights reserved.
* Portions Copyright (c) 2013 ARM LIMITED * Portions Copyright (c) 2013 ARM LIMITED
* All rights reserved. * All rights reserved.
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without

7
Src/gpio.c
View File

@ -103,6 +103,13 @@ void MX_GPIO_Init(void)
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(CS1_GYRO_GPIO_Port, &GPIO_InitStruct); HAL_GPIO_Init(CS1_GYRO_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = GPIO_PIN_8;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
HAL_GPIO_WritePin(GPIOC,GPIO_PIN_8,GPIO_PIN_SET);
/* EXTI interrupt init*/ /* EXTI interrupt init*/
HAL_NVIC_SetPriority(EXTI3_IRQn, 5, 0); HAL_NVIC_SetPriority(EXTI3_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(EXTI3_IRQn); HAL_NVIC_EnableIRQ(EXTI3_IRQn);

3
Src/main.c
View File

@ -35,7 +35,8 @@
/* Private includes ----------------------------------------------------------*/ /* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */ /* USER CODE BEGIN Includes */
#include "robot.h"
#include "bsp_log.h"
/* USER CODE END Includes */ /* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/

229
application/chassis/chassis.c
View File

@ -22,17 +22,12 @@
#include "bsp_dwt.h" #include "bsp_dwt.h"
#include "referee_UI.h" #include "referee_UI.h"
#include "arm_math.h" #include "arm_math.h"
#include "user_lib.h"
#include "vofa.h"
/* 根据robot_def.h中的macro自动计算的参数 */ /* 根据robot_def.h中的macro自动计算的参数 */
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距 #define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f) // 半轮距 #define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f) // 半轮距
#define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长 #define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长
#define P_MAX 50.0f//功率控制 单位：W
/* 底盘应用包含的模块和信息存储,底盘是单例模式,因此不需要为底盘建立单独的结构体 */ /* 底盘应用包含的模块和信息存储,底盘是单例模式,因此不需要为底盘建立单独的结构体 */
#ifdef CHASSIS_BOARD // 如果是底盘板,使用板载IMU获取底盘转动角速度 #ifdef CHASSIS_BOARD // 如果是底盘板,使用板载IMU获取底盘转动角速度
#include "can_comm.h" #include "can_comm.h"
@ -46,13 +41,11 @@ static Subscriber_t *chassis_sub; // 用于订阅底盘的控
#endif // !ONE_BOARD #endif // !ONE_BOARD
static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令 static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据 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; // 用于获取裁判系统的数据
SuperCapInstance *cap; // 超级电容全局 static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
static uint16_t last_chassis_power_limit=0;//超级电容更新
static SuperCapInstance *cap; // 超级电容
static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
/* 用于自旋变速策略的时间变量 */ /* 用于自旋变速策略的时间变量 */
@ -61,28 +54,25 @@ static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left righ
/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */ /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘 static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘
static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅 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和反转标志位即可 // 四个轮子的参数一样,改tx_id和反转标志位即可
Motor_Init_Config_s chassis_motor_config = { Motor_Init_Config_s chassis_motor_config = {
.can_init_config.can_handle = &hcan1, .can_init_config.can_handle = &hcan1,
.controller_param_init_config = { .controller_param_init_config = {
.speed_PID = { .speed_PID = {
.Kp = 5.0f, .Kp = 10, // 4.5
.Ki = 0.01f, .Ki = 0, // 0
.Kd = 0.002f, .Kd = 0, // 0
.IntegralLimit = 3000, .IntegralLimit = 3000,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.MaxOut = 30000, .MaxOut = 12000,
}, },
.current_PID = { .current_PID = {
.Kp = 0.0f, .Kp = 0.4, // 0.4
.Ki = 0.0f, .Ki = 0, // 0
.Kd = 0.0f, .Kd = 0,
.IntegralLimit = 3000, .IntegralLimit = 3000,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.MaxOut = 15000, .MaxOut = 15000,
@ -92,52 +82,37 @@ void ChassisInit() {
.angle_feedback_source = MOTOR_FEED, .angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
.outer_loop_type = SPEED_LOOP, .outer_loop_type = SPEED_LOOP,
.close_loop_type = SPEED_LOOP, // CURRENT_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
.power_limit_flag = POWER_LIMIT_ON,
}, },
.motor_type = M3508, .motor_type = M3508,
}; };
// @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改. // @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
// 四个轮子pid分开 chassis_motor_config.can_init_config.tx_id = 2;
//右前 chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
chassis_motor_config.can_init_config.tx_id = 3;
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
motor_lf = DJIMotorInit(&chassis_motor_config); motor_lf = DJIMotorInit(&chassis_motor_config);
chassis_motor_config.can_init_config.tx_id = 2; chassis_motor_config.can_init_config.tx_id = 1;
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
motor_rf = DJIMotorInit(&chassis_motor_config); motor_rf = DJIMotorInit(&chassis_motor_config);
chassis_motor_config.can_init_config.tx_id = 4; chassis_motor_config.can_init_config.tx_id = 4;
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
motor_lb = DJIMotorInit(&chassis_motor_config); motor_lb = DJIMotorInit(&chassis_motor_config);
chassis_motor_config.can_init_config.tx_id = 1; chassis_motor_config.can_init_config.tx_id = 3;
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
motor_rb = DJIMotorInit(&chassis_motor_config); motor_rb = DJIMotorInit(&chassis_motor_config);
//超级电容 referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI
SuperCap_Init_Config_s cap_conf = { SuperCap_Init_Config_s cap_conf = {
.can_config = { .can_config = {
.can_handle = &hcan1, .can_handle = &hcan2,
.tx_id = 0x210, .tx_id = 0x302, // 超级电容默认接收id
.rx_id = 0x211, .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
}};
},
.buffer_config_pid = {
.Kp = 1.0f,
.Ki = 0,
.Kd = 0,
.MaxOut = 300,
},
};
cap = SuperCapInit(&cap_conf); // 超级电容初始化 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来传递消息 // 发布订阅初始化,如果为双板,则需要can comm来传递消息
#ifdef CHASSIS_BOARD #ifdef CHASSIS_BOARD
Chassis_IMU_data = INS_Init(); // 底盘IMU初始化 Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
@ -159,7 +134,7 @@ void ChassisInit() {
chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s)); chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
#endif // ONE_BOARD #endif // ONE_BOARD
} }
//225+0+215-0
#define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD) #define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD) #define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD) #define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
@ -168,106 +143,46 @@ void ChassisInit() {
* @brief 计算每个轮毂电机的输出,正运动学解算 * @brief 计算每个轮毂电机的输出,正运动学解算
* 用宏进行预替换减小开销,运动解算具体过程参考教程 * 用宏进行预替换减小开销,运动解算具体过程参考教程
*/ */
//全向轮解算 static void MecanumCalculate()
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)
{ {
vt_lf = chassis_vx + chassis_vy - chassis_cmd_recv.wz * (LF_CENTER-1);
float I_cmd = chassis_motor->motor_controller.current_PID.Output; vt_rf = -chassis_vx + chassis_vy + chassis_cmd_recv.wz * (RF_CENTER-1);
float w = chassis_motor->measure.speed_aps /6 ;//aps to rpm vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * (-LB_CENTER-1);
vt_rb = -chassis_vx - chassis_vy + chassis_cmd_recv.wz * (-RB_CENTER-1);
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 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值 * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
* *
*/ */
static void LimitChassisOutput() static void LimitChassisOutput()
{ {
// float Plimit = 1.0f; // 功率限制待添加
P_cmd = motor_rf->motor_controller.motor_power_predict + // referee_data->PowerHeatData.chassis_power;
motor_rb->motor_controller.motor_power_predict + // referee_data->PowerHeatData.chassis_power_buffer;
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_lf, vt_lf);
DJIMotorSetRef(motor_rf, vt_rf); DJIMotorSetRef(motor_rf, vt_rf);
DJIMotorSetRef(motor_lb, vt_lb); DJIMotorSetRef(motor_lb, vt_lb);
DJIMotorSetRef(motor_rb, vt_rb); 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 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算 * @brief 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算
* 对于双板的情况,考虑增加来自底盘板IMU的数据 * 对于双板的情况,考虑增加来自底盘板IMU的数据
* *
*/ */
static void EstimateSpeed() { static void EstimateSpeed()
{
// 根据电机速度和陀螺仪的角速度进行解算,还可以利用加速度计判断是否打滑(如果有) // 根据电机速度和陀螺仪的角速度进行解算,还可以利用加速度计判断是否打滑(如果有)
// chassis_feedback_data.vx vy wz = // 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 #ifdef ONE_BOARD
@ -277,12 +192,15 @@ void ChassisTask() {
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm); chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
#endif // CHASSIS_BOARD #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_lf);
DJIMotorStop(motor_rf); DJIMotorStop(motor_rf);
DJIMotorStop(motor_lb); DJIMotorStop(motor_lb);
DJIMotorStop(motor_rb); DJIMotorStop(motor_rb);
} else { // 正常工作 }
else
{ // 正常工作
DJIMotorEnable(motor_lf); DJIMotorEnable(motor_lf);
DJIMotorEnable(motor_rf); DJIMotorEnable(motor_rf);
DJIMotorEnable(motor_lb); DJIMotorEnable(motor_lb);
@ -290,77 +208,48 @@ void ChassisTask() {
} }
// 根据控制模式设定旋转速度 // 根据控制模式设定旋转速度
switch (chassis_cmd_recv.chassis_mode) { switch (chassis_cmd_recv.chassis_mode)
{
case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态 case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
chassis_cmd_recv.wz = 0; chassis_cmd_recv.wz = 0;
break; break;
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
chassis_cmd_recv.wz = 40.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle); chassis_cmd_recv.wz = 2.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; break;
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
if(last_chassis_mode != CHASSIS_ROTATE){ chassis_cmd_recv.wz = 1600;
rotate_speed = -rotate_speed;
}
chassis_cmd_recv.wz = rotate_speed;
break; break;
default: default:
break; break;
} }
last_chassis_mode = chassis_cmd_recv.chassis_mode;
// 根据云台和底盘的角度offset将控制量映射到底盘坐标系上 // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
// 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东) // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
static float sin_theta, cos_theta; static float sin_theta, cos_theta;
cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD); 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); 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_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; chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
// 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(); MecanumCalculate();
OmniCalculate();
////对缓冲功率进行闭环
SuperCapSetUpdate();
// 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值 // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
LimitChassisOutput(); 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(如果有)计算真实速度 // 根据电机的反馈速度和IMU(如果有)计算真实速度
EstimateSpeed(); EstimateSpeed();
//todo: 裁判系统信息移植到消息中心发送 // // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
// 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送 // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
// 发送敌方方颜色id // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0;
//chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color; // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
// 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况 // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
//chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit; // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;
chassis_feedback_data.cap_vol = cap->cap_msg.cap_vol;
// 推送反馈消息 // 推送反馈消息
#ifdef ONE_BOARD #ifdef ONE_BOARD
PubPushMessage(chassis_pub, (void *) &chassis_feedback_data); PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);
#endif #endif
#ifdef CHASSIS_BOARD #ifdef CHASSIS_BOARD
CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data); CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data);

589
application/cmd/robot_cmd.c
View File

@ -8,15 +8,9 @@
#include "message_center.h" #include "message_center.h"
#include "general_def.h" #include "general_def.h"
#include "dji_motor.h" #include "dji_motor.h"
#include "auto_aim.h"
#include "super_cap.h"
#include "user_lib.h"
// bsp // bsp
#include "bsp_dwt.h" #include "bsp_dwt.h"
#include "bsp_log.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 #define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360
@ -25,8 +19,6 @@
/* cmd应用包含的模块实例指针和交互信息存储*/ /* cmd应用包含的模块实例指针和交互信息存储*/
#ifdef GIMBAL_BOARD // 对双板的兼容,条件编译 #ifdef GIMBAL_BOARD // 对双板的兼容,条件编译
#include "can_comm.h" #include "can_comm.h"
#include "user_lib.h"
static CANCommInstance *cmd_can_comm; // 双板通信 static CANCommInstance *cmd_can_comm; // 双板通信
#endif #endif
#ifdef ONE_BOARD #ifdef ONE_BOARD
@ -36,22 +28,10 @@ static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者
static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关 static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关
static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等 static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
extern SuperCapInstance *cap; // 超级电容
static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回 static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回
static VT_ctrl_t *vt_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 Publisher_t *gimbal_cmd_pub; // 云台控制消息发布者
static Subscriber_t *gimbal_feed_sub; // 云台反馈信息订阅者 static Subscriber_t *gimbal_feed_sub; // 云台反馈信息订阅者
@ -65,14 +45,10 @@ static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
static Robot_Status_e robot_state; // 机器人整体工作状态 static Robot_Status_e robot_state; // 机器人整体工作状态
static referee_info_t *referee_data; // 用于获取裁判系统的数据 void RobotCMDInit()
static uint8_t loader_flag = 0; //拨弹模式选择标志位 {
void RobotCMDInit() {
rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个 rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
vision_recv_data = VisionInit(); // 视觉通信串口 vision_recv_data = VisionInit(&huart1); // 视觉通信串口
vt_data = VTRefereeInit(&huart1); // 图传通信串口
referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI
gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
@ -105,7 +81,8 @@ void RobotCMDInit() {
* 单圈绝对角度的范围是0~360,说明文档中有图示 * 单圈绝对角度的范围是0~360,说明文档中有图示
* *
*/ */
static void CalcOffsetAngle() { static void CalcOffsetAngle()
{
// 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数 // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
static float angle; static float angle;
angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度 angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
@ -124,487 +101,158 @@ static void CalcOffsetAngle() {
else else
chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE + 360.0f; chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE + 360.0f;
#endif #endif
// if(chassis_cmd_send.offset_angle > 340.0f)
// chassis_cmd_send.offset_angle = chassis_cmd_send.offset_angle - 360.0f;
// else if(chassis_cmd_send.offset_angle < -340.0f)
// chassis_cmd_send.offset_angle = chassis_cmd_send.offset_angle + 360.0f;
// else
// chassis_cmd_send.offset_angle = chassis_cmd_send.offset_angle;
} }
//功能：死亡后清除小陀螺的状态
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 控制输入为遥控器(调试时)的模式和控制量设置 * @brief 控制输入为遥控器(调试时)的模式和控制量设置
* *
*/ */
static void RemoteControlSet() { static void RemoteControlSet()
{
// 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据? // 控制底盘和云台运行模式,云台待添加,云台是否始终使用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; chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; 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_FOLLOW_GIMBAL_YAW; chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_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的增量进行控制 // 待添加,视觉会发来和目标的误差,同样将其转化为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)) { if (switch_is_down(rc_data[TEMP].rc.switch_left) || vision_recv_data->target_state == NO_TARGET)
auto_aim_mode(); { // 按照摇杆的输出大小进行角度增量,增益系数需调整
gimbal_cmd_send.yaw += 0.003f * (float)rc_data[TEMP].rc.rocker_l_;
gimbal_cmd_send.pitch += 0.0007f * (float)rc_data[TEMP].rc.rocker_l1;
if(gimbal_cmd_send.pitch>=18.0) gimbal_cmd_send.pitch=18.0f;
if(gimbal_cmd_send.pitch<=-40.0) gimbal_cmd_send.pitch=-40.0f;
} }
// 云台软件限位 // 云台软件限位
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整，遥控器输入灵敏度 // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
chassis_cmd_send.vx = 0.001f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向 chassis_cmd_send.vx = 13.1f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向
chassis_cmd_send.vy = 0.001f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向 chassis_cmd_send.vy = 13.1f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向
// 发射参数 // 发射参数
if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
; // 弹舱舵机控制,待添加servo_motor模块,开启 ; // 弹舱舵机控制,待添加servo_motor模块,开启
else; // 弹舱舵机控制,待添加servo_motor模块,关闭 else
; // 弹舱舵机控制,待添加servo_motor模块,关闭
// 摩擦轮控制,拨轮向上打为负,向下为正 // 摩擦轮控制,拨轮向上打为负,向下为正
if (rc_data[TEMP].rc.dial < -100) // 向上超过100,打开摩擦轮 if (rc_data[TEMP].rc.dial < -100) // 向上超过100,打开摩擦轮
shoot_cmd_send.friction_mode = FRICTION_ON; shoot_cmd_send.friction_mode = FRICTION_ON;
else else
shoot_cmd_send.friction_mode = FRICTION_OFF; shoot_cmd_send.friction_mode = FRICTION_OFF;
// 拨弹控制,遥控器固定为一种拨弹模式,可自行选择 // 拨弹控制,遥控器固定为一种拨弹模式,可自行选择
if (rc_data[TEMP].rc.dial < -500)// if (rc_data[TEMP].rc.dial < -500)
shoot_cmd_send.loader_mode = LOAD_BURSTFIRE; shoot_cmd_send.load_mode = LOAD_1_BULLET;
else else
shoot_cmd_send.loader_mode = LOAD_STOP; shoot_cmd_send.load_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发,后续可以根据左侧拨轮的值大小切换射频, // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
shoot_cmd_send.shoot_rate = 18; shoot_cmd_send.shoot_rate = 8;
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 输入为键鼠时模式和控制量设置 * @brief 输入为键鼠时模式和控制量设置
* *
*/ */
static void MouseKeySet() { 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; chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].w * 300 - rc_data[TEMP].key[KEY_PRESS].s * 300; // 系数待测
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].s * 300 - rc_data[TEMP].key[KEY_PRESS].d * 300;
gimbal_cmd_send.yaw -= (float) rc_data[TEMP].mouse.x / 660 * 4; // 系数待测 gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测
gimbal_cmd_send.pitch += (float) rc_data[TEMP].mouse.y / 660 * 6; gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10;
float delta_yaw_total = gimbal_cmd_send.yaw - gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速
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)) // 左键发射模式
{
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;
}
if (rc_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 && 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键手动刷新UI
{
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: case 0:
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_8, GPIO_PIN_SET); shoot_cmd_send.bullet_speed = 15;
break;
case 1:
shoot_cmd_send.bullet_speed = 18;
break; break;
default: default:
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_8, GPIO_PIN_RESET); shoot_cmd_send.bullet_speed = 30;
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式
{
case 0:
shoot_cmd_send.load_mode = LOAD_STOP;
break;
case 1:
shoot_cmd_send.load_mode = LOAD_1_BULLET;
break;
case 2:
shoot_cmd_send.load_mode = LOAD_3_BULLET;
break;
default:
shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱
{
case 0:
shoot_cmd_send.lid_mode = LID_OPEN;
break;
default:
shoot_cmd_send.lid_mode = LID_CLOSE;
break; break;
} }
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮 switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮
{ {
case 0: case 0:
shoot_cmd_send.shoot_mode = SHOOT_OFF;
shoot_cmd_send.friction_mode = FRICTION_OFF; shoot_cmd_send.friction_mode = FRICTION_OFF;
break; break;
default: default:
shoot_cmd_send.shoot_mode = SHOOT_ON;
shoot_cmd_send.friction_mode = FRICTION_ON; shoot_cmd_send.friction_mode = FRICTION_ON;
break; break;
} }
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_B] % 2) // B键开关弹舱盖 switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度
{ {
case 0: case 0:
shoot_cmd_send.lid_mode = LID_CLOSE; chassis_cmd_send.chassis_speed_buff = 40;
break;
default:
shoot_cmd_send.lid_mode = LID_OPEN;
break;
}
// 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 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; break;
case 1: case 1:
chassis_cmd_send.chassis_mode = CHASSIS_SIDEWAYS; 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; break;
} }
// switch (rc_data[TEMP].key_count[KEY_PRESS][Key_G] % 3) // G键切换发射模式 switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量
// {
// 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: 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;
default:
break; 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 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等 * @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等
* 停止的阈值'300'待修改成合适的值,或改为开关控制. * 停止的阈值'300'待修改成合适的值,或改为开关控制.
@ -612,7 +260,8 @@ static void VTMouseKeySet() {
* @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现 * @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现
* *
*/ */
static void EmergencyHandler() { static void EmergencyHandler()
{
// 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正 // 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正
if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断 if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
{ {
@ -621,79 +270,53 @@ static void EmergencyHandler() {
chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
shoot_cmd_send.shoot_mode = SHOOT_OFF; shoot_cmd_send.shoot_mode = SHOOT_OFF;
shoot_cmd_send.friction_mode = FRICTION_OFF; shoot_cmd_send.friction_mode = FRICTION_OFF;
shoot_cmd_send.loader_mode = LOAD_STOP; shoot_cmd_send.load_mode = LOAD_STOP;
LOGERROR("[CMD] emergency 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; robot_state = ROBOT_READY;
shoot_cmd_send.shoot_mode = SHOOT_ON; shoot_cmd_send.shoot_mode = SHOOT_ON;
gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle;
LOGINFO("[CMD] reinstate, robot ready"); LOGINFO("[CMD] reinstate, robot ready");
} }
} }
/* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */ /* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */
void RobotCMDTask() { void RobotCMDTask()
{
// 从其他应用获取回传数据 // 从其他应用获取回传数据
#ifdef ONE_BOARD #ifdef ONE_BOARD
SubGetMessage(chassis_feed_sub, (void *) &chassis_fetch_data); SubGetMessage(chassis_feed_sub, (void *)&chassis_fetch_data);
#endif // ONE_BOARD #endif // ONE_BOARD
#ifdef GIMBAL_BOARD #ifdef GIMBAL_BOARD
chassis_fetch_data = *(Chassis_Upload_Data_s *)CANCommGet(cmd_can_comm); chassis_fetch_data = *(Chassis_Upload_Data_s *)CANCommGet(cmd_can_comm);
#endif // GIMBAL_BOARD #endif // GIMBAL_BOARD
SubGetMessage(shoot_feed_sub, &shoot_fetch_data); SubGetMessage(shoot_feed_sub, &shoot_fetch_data);
SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data); SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
update_ui_data();
// 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成 // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
CalcOffsetAngle(); CalcOffsetAngle();
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
if (robot_state != ROBOT_STOP) {
// 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠 // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
if (switch_is_down(rc_data[TEMP].rc.switch_left) || if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],[中],遥控器控制
RemoteControlSet(); RemoteControlSet();
else if (switch_is_up(rc_data[TEMP].rc.switch_left) && else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
switch_is_mid(rc_data[TEMP].rc.switch_right)) // 遥控器左侧开关状态为[上],键盘控制,遥控器右侧开关状态为[中],遥控器通道
MouseKeySet(); MouseKeySet();
else if (switch_is_up(rc_data[TEMP].rc.switch_left) &&
switch_is_up(rc_data[TEMP].rc.switch_right)) // 遥控器左侧开关状态为[上],键盘控制，遥控器右侧开关状态为[上],图传链路通道 EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
VTMouseKeySet();
}
// 设置视觉发送数据,还需增加加速度和角速度数据 // 设置视觉发送数据,还需增加加速度和角速度数据
VisionSetFlag(!referee_data->referee_id.Robot_Color); // VisionSetFlag(chassis_fetch_data.enemy_color,,chassis_fetch_data.bullet_speed)
//根据裁判系统反馈确定底盘功率上限
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中完成设置 // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置
#ifdef ONE_BOARD #ifdef ONE_BOARD
PubPushMessage(chassis_cmd_pub, (void *) &chassis_cmd_send); PubPushMessage(chassis_cmd_pub, (void *)&chassis_cmd_send);
#endif // ONE_BOARD #endif // ONE_BOARD
#ifdef GIMBAL_BOARD #ifdef GIMBAL_BOARD
CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send); CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send);
#endif // GIMBAL_BOARD #endif // GIMBAL_BOARD
PubPushMessage(shoot_cmd_pub, (void *) &shoot_cmd_send); PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
PubPushMessage(gimbal_cmd_pub, (void *) &gimbal_cmd_send); PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
VisionSend(&vision_send_data);
} }

119
application/gimbal/gimbal.c
View File

@ -4,8 +4,8 @@
#include "ins_task.h" #include "ins_task.h"
#include "message_center.h" #include "message_center.h"
#include "general_def.h" #include "general_def.h"
#include "bmi088.h" #include "bmi088.h"
#include "vofa.h"
static attitude_t *gimba_IMU_data; // 云台IMU数据 static attitude_t *gimba_IMU_data; // 云台IMU数据
static DJIMotorInstance *yaw_motor, *pitch_motor; static DJIMotorInstance *yaw_motor, *pitch_motor;
@ -15,56 +15,33 @@ static Subscriber_t *gimbal_sub; // cmd控制消息订阅者
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息
static float gravity_current = 0; void GimbalInit()
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电机的其他数据来源 gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
// YAW // YAW
Motor_Init_Config_s yaw_config = { Motor_Init_Config_s yaw_config = {
.can_init_config = { .can_init_config = {
.can_handle = &hcan1, .can_handle = &hcan2,
.tx_id = 1, .tx_id = 1,
}, },
.controller_param_init_config = { .controller_param_init_config = {
.angle_PID = { .angle_PID = {
.Kp = 0.8f,//1.2 .Kp = 30, // 8
.Ki = 0.0f,//0 .Ki = 0,
.Kd = 0.04f,//0.05 .Kd = 1,
.DeadBand = 0.0f, .DeadBand = 0.1,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100, .IntegralLimit = 100,
.MaxOut = 500, .MaxOut = 500,
}, },
.speed_PID = { .speed_PID = {
.Kp = 5000, //4000 .Kp = 70, // 50
.Ki = 100.0f, //100 .Ki = 200, // 200
.Kd = 0.03f, //0 .Kd = 10,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 10000, .IntegralLimit = 3000,
.MaxOut = 15000, .MaxOut = 20000,
}, },
.other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle, .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
@ -74,39 +51,36 @@ void GimbalInit() {
.angle_feedback_source = OTHER_FEED, .angle_feedback_source = OTHER_FEED,
.speed_feedback_source = OTHER_FEED, .speed_feedback_source = OTHER_FEED,
.outer_loop_type = ANGLE_LOOP, .outer_loop_type = ANGLE_LOOP,
.close_loop_type = SPEED_LOOP | ANGLE_LOOP, .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, .motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
}, },
.motor_type = GM6020, .motor_type = GM6020};
.motor_control_type = CURRENT_CONTROL
};
// PITCH // PITCH
Motor_Init_Config_s pitch_config = { Motor_Init_Config_s pitch_config = {
.can_init_config = { .can_init_config = {
.can_handle = &hcan2, .can_handle = &hcan2,
.tx_id = 1, .tx_id = 2,
}, },
.controller_param_init_config = { .controller_param_init_config = {
.angle_PID = { .angle_PID = {
.Kp = 1.0f, .Kp = 15, // 10
.Ki = 0.0f, .Ki = 0.1f,
.Kd = 0.02f, .Kd = 1,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100, .IntegralLimit = 100,//100
.MaxOut = 500, .MaxOut = 500,//500
}, },
.speed_PID = { .speed_PID = {
.Kp = 6000.0f, .Kp = 30, // 50
.Ki = 900.0f, .Ki = 0, // 350
.Kd = 0.0f, // 0 .Kd = 1, // 0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 10000, .IntegralLimit = 150,//2500
.MaxOut = 15000, .MaxOut = 2000,//20000
}, },
.other_angle_feedback_ptr = &gimba_IMU_data->Roll, .other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
.other_speed_feedback_ptr = &gimba_IMU_data->Gyro[1], .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]),
.current_feedforward_ptr = &gravity_current,
}, },
.controller_setting_init_config = { .controller_setting_init_config = {
.angle_feedback_source = OTHER_FEED, .angle_feedback_source = OTHER_FEED,
@ -114,11 +88,9 @@ void GimbalInit() {
.outer_loop_type = ANGLE_LOOP, .outer_loop_type = ANGLE_LOOP,
.close_loop_type = SPEED_LOOP | ANGLE_LOOP, .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_REVERSE, .motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
.feedforward_flag = CURRENT_FEEDFORWARD, .feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE,
.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE
}, },
.motor_type = GM6020, .motor_type = M3508,
.motor_control_type = CURRENT_CONTROL
}; };
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
yaw_motor = DJIMotorInit(&yaw_config); yaw_motor = DJIMotorInit(&yaw_config);
@ -129,16 +101,16 @@ void GimbalInit() {
} }
/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
void GimbalTask() { void GimbalTask()
//红点激光 {
//HAL_GPIO_WritePin(GPIOC,GPIO_PIN_8,GPIO_PIN_SET);
// 获取云台控制数据 // 获取云台控制数据
// 后续增加未收到数据的处理 // 后续增加未收到数据的处理
SubGetMessage(gimbal_sub, &gimbal_cmd_recv); SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
// @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘 // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘
// 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref
switch (gimbal_cmd_recv.gimbal_mode) { switch (gimbal_cmd_recv.gimbal_mode)
{
// 停止 // 停止
case GIMBAL_ZERO_FORCE: case GIMBAL_ZERO_FORCE:
DJIMotorStop(yaw_motor); DJIMotorStop(yaw_motor);
@ -154,7 +126,6 @@ void GimbalTask() {
DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
//LimitPitchAngleAuto();
break; break;
// 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关 // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)
@ -174,27 +145,11 @@ void GimbalTask() {
// 在合适的地方添加pitch重力补偿前馈力矩 // 在合适的地方添加pitch重力补偿前馈力矩
// 根据IMU姿态/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 // 设置反馈数据,主要是imu和yaw的ecd
gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round; 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);
} }

2
application/robot.c
View File

@ -31,7 +31,6 @@ void RobotInit()
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD) #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
RobotCMDInit(); RobotCMDInit();
GimbalInit(); GimbalInit();
ShootInit(); ShootInit();
#endif #endif
@ -50,7 +49,6 @@ void RobotTask()
{ {
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD) #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
RobotCMDTask(); RobotCMDTask();
//暂时注释云台和发射任务 调试底盘
GimbalTask(); GimbalTask();
ShootTask(); ShootTask();
#endif #endif

7
application/robot.h
View File

@ -1,8 +1,5 @@
#ifndef ROBOT_H #ifndef ROBOT_H
#define ROBOT_H #define ROBOT_H
#ifdef __cplusplus
extern "C"{
#endif
/* Robot利用robot_def.h中的宏对不同的机器人进行了大量的兼容,同时兼容了两个开发板(云台板和底盘板)的配置 */ /* Robot利用robot_def.h中的宏对不同的机器人进行了大量的兼容,同时兼容了两个开发板(云台板和底盘板)的配置 */
@ -18,8 +15,4 @@ void RobotInit();
*/ */
void RobotTask(); void RobotTask();
#ifdef __cplusplus
}
#endif
#endif #endif

67
application/robot_def.h
View File

@ -26,25 +26,21 @@
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
// 云台参数 // 云台参数
#define YAW_CHASSIS_ALIGN_ECD 2053 // 小 1443 大2053 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 #define YAW_CHASSIS_ALIGN_ECD 891 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 #define YAW_ECD_GREATER_THAN_4096 1 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
#define PITCH_HORIZON_ECD 4422 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 #define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
#define PITCH_MAX_ANGLE 25 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) #define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define PITCH_MIN_ANGLE -24 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) #define PITCH_MIN_ANGLE 0 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define PITCH_MAX_RELATIVE_ANGLE 123 // 云台相对底盘最大角度
#define PITCH_MIN_RELATIVE_ANGLE 80 // 云台相对底盘最小角度
// 发射参数 // 发射参数
#define ONE_BULLET_DELTA_ANGLE 1620 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 #define ONE_BULLET_DELTA_ANGLE 1368 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
#define REDUCTION_RATIO_LOADER 36.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f 2006-36.0f #define REDUCTION_RATIO_LOADER 19.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f//初始是49
#define NUM_PER_CIRCLE 8 // 拨盘一圈的装载量 #define NUM_PER_CIRCLE 5 // 拨盘一圈的装载量
// 机器人底盘修改的参数,单位为m(米) // 机器人底盘修改的参数,单位为mm(毫米)
#define WHEEL_BASE 0.1f // 纵向轴距(前进后退方向) #define WHEEL_BASE 420 // 纵向轴距(前进后退方向)
#define TRACK_WIDTH 0.1f // 横向轮距(左右平移方向) #define TRACK_WIDTH 450 // 横向轮距(左右平移方向)
#define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0 #define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0
#define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0 #define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0
#define RADIUS_WHEEL 0.08f // 轮子半径 #define RADIUS_WHEEL 75 // 轮子半径
#define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换 #define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
#define GYRO2GIMBAL_DIR_YAW 1 // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反 #define GYRO2GIMBAL_DIR_YAW 1 // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
@ -90,7 +86,6 @@ typedef enum
CHASSIS_ROTATE, // 小陀螺模式 CHASSIS_ROTATE, // 小陀螺模式
CHASSIS_NO_FOLLOW, // 不跟随，允许全向平移 CHASSIS_NO_FOLLOW, // 不跟随，允许全向平移
CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式，底盘叠加角度环控制 CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式，底盘叠加角度环控制
CHASSIS_SIDEWAYS, // 侧向
} chassis_mode_e; } chassis_mode_e;
// 云台模式设置 // 云台模式设置
@ -115,7 +110,7 @@ typedef enum
typedef enum typedef enum
{ {
LID_OPEN , // 弹舱盖打开 LID_OPEN = 0, // 弹舱盖打开
LID_CLOSE, // 弹舱盖关闭 LID_CLOSE, // 弹舱盖关闭
} lid_mode_e; } lid_mode_e;
@ -127,22 +122,13 @@ typedef enum
LOAD_3_BULLET, // 三发 LOAD_3_BULLET, // 三发
LOAD_BURSTFIRE, // 连发 LOAD_BURSTFIRE, // 连发
} loader_mode_e; } loader_mode_e;
typedef enum
{
HEAT_OPEN , // 热控打开
HEAT_CLOSE, // 热控关闭
}heat_mode_e;
// 功率限制,从裁判系统获取,是否有必要保留? // 功率限制,从裁判系统获取,是否有必要保留?
typedef struct typedef struct
{ { // 功率控制
uint16_t chassis_power_mx; // 功率限制 float chassis_power_mx;
} Chassis_Power_Data_s; } Chassis_Power_Data_s;
// 电容信息
typedef struct
{
int16_t cap_vol;
} Cap_Data_s;
/* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */ /* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */
/** /**
* @brief 对于双板情况,遥控器和pc在云台,裁判系统在底盘 * @brief 对于双板情况,遥控器和pc在云台,裁判系统在底盘
@ -158,11 +144,6 @@ typedef struct
float offset_angle; // 底盘和归中位置的夹角 float offset_angle; // 底盘和归中位置的夹角
chassis_mode_e chassis_mode; chassis_mode_e chassis_mode;
int chassis_speed_buff; int chassis_speed_buff;
uint16_t chassis_power_limit;
uint16_t buffer_energy;
uint16_t buffer_supercap;
uint8_t chassic_flag;
// UI部分 // UI部分
// ... // ...
@ -182,16 +163,12 @@ typedef struct
typedef struct typedef struct
{ {
shoot_mode_e shoot_mode; shoot_mode_e shoot_mode;
loader_mode_e loader_mode; loader_mode_e load_mode;
lid_mode_e lid_mode; lid_mode_e lid_mode;
friction_mode_e friction_mode; friction_mode_e friction_mode;
heat_mode_e heat_mode;
Bullet_Speed_e bullet_speed; // 弹速枚举 Bullet_Speed_e bullet_speed; // 弹速枚举
uint8_t rest_heat;
float shoot_rate; // 连续发射的射频,unit per s,发/秒 float shoot_rate; // 连续发射的射频,unit per s,发/秒
uint16_t heat; // 实时热量
uint16_t heat_limit; // 热量上限
uint16_t heat_cool; // 热量每秒冷却值
} Shoot_Ctrl_Cmd_s; } Shoot_Ctrl_Cmd_s;
/* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/ /* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/
@ -210,17 +187,17 @@ typedef struct
// float real_vy; // float real_vy;
// float real_wz; // float real_wz;
uint8_t rest_heat; // 剩余枪口热量
Bullet_Speed_e bullet_speed; // 弹速限制 Bullet_Speed_e bullet_speed; // 弹速限制
Enemy_Color_e enemy_color; // 0 for red, 1 for blue Enemy_Color_e enemy_color; // 0 for blue, 1 for red
uint16_t chassis_power_mx;
int16_t cap_vol;
} Chassis_Upload_Data_s; } Chassis_Upload_Data_s;
typedef struct typedef struct
{ {
attitude_t gimbal_imu_data; attitude_t gimbal_imu_data;
float yaw_motor_single_round_angle; uint16_t yaw_motor_single_round_angle;
} Gimbal_Upload_Data_s; } Gimbal_Upload_Data_s;
typedef struct typedef struct

6
application/robot_task.h
View File

@ -17,8 +17,6 @@
#include "bsp_log.h" #include "bsp_log.h"
osThreadId insTaskHandle; osThreadId insTaskHandle;
osThreadId robotTaskHandle; osThreadId robotTaskHandle;
osThreadId motorTaskHandle; osThreadId motorTaskHandle;
@ -121,9 +119,9 @@ __attribute__((noreturn)) void StartROBOTTASK(void const *argument)
robot_start = DWT_GetTimeline_ms(); robot_start = DWT_GetTimeline_ms();
RobotTask(); RobotTask();
robot_dt = DWT_GetTimeline_ms() - robot_start; robot_dt = DWT_GetTimeline_ms() - robot_start;
if (robot_dt > 1) if (robot_dt > 5)
LOGERROR("[freeRTOS] ROBOT core Task is being DELAY! dt = [%f]", &robot_dt); LOGERROR("[freeRTOS] ROBOT core Task is being DELAY! dt = [%f]", &robot_dt);
osDelay(1); osDelay(5);
} }
} }

200
application/shoot/shoot.c
View File

@ -5,11 +5,11 @@
#include "message_center.h" #include "message_center.h"
#include "bsp_dwt.h" #include "bsp_dwt.h"
#include "general_def.h" #include "general_def.h"
#include "servo_motor.h"
/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */ /* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
static DJIMotorInstance *friction_l, *friction_r, *loader; // 拨盘电机 static DJIMotorInstance *friction_l, *friction_r, *loader; // 拨盘电机
static ServoInstance *lid; // static servo_instance *lid; 需要增加弹舱盖
static Publisher_t *shoot_pub; static Publisher_t *shoot_pub;
static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自cmd的发射控制信息 static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自cmd的发射控制信息
@ -17,7 +17,9 @@ static Subscriber_t *shoot_sub;
static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息 static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息
// dwt定时,计算冷却用 // dwt定时,计算冷却用
static float hibernate_time = 0,last_hibernate_time = 0, dead_time = 0; static float hibernate_time = 0, dead_time = 0;
static uint16_t locked_time;
void ShootInit() void ShootInit()
{ {
@ -28,16 +30,16 @@ void ShootInit()
}, },
.controller_param_init_config = { .controller_param_init_config = {
.speed_PID = { .speed_PID = {
.Kp = 1.5f, .Kp = 20, // 20
.Ki = 0.2f, .Ki = 1, // 1
.Kd = 0, .Kd = 0,
.Improve = PID_Integral_Limit, .Improve = PID_Integral_Limit,
.IntegralLimit = 10000, .IntegralLimit = 10000,
.MaxOut = 15000, .MaxOut = 15000,
}, },
.current_PID = { .current_PID = {
.Kp = 0, .Kp = 0.7f, // 0.7
.Ki = 0, .Ki = 0.1f, // 0.1
.Kd = 0, .Kd = 0,
.Improve = PID_Integral_Limit, .Improve = PID_Integral_Limit,
.IntegralLimit = 10000, .IntegralLimit = 10000,
@ -47,136 +49,76 @@ void ShootInit()
.controller_setting_init_config = { .controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED, .angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
.outer_loop_type = SPEED_LOOP, .outer_loop_type = SPEED_LOOP,
.close_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
}, },
.motor_type = M3508}; .motor_type = M3508};
friction_config.can_init_config.tx_id = 2, friction_config.can_init_config.tx_id = 3,
friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
friction_l = DJIMotorInit(&friction_config); friction_l = DJIMotorInit(&friction_config);
friction_config.can_init_config.tx_id = 3; // 右摩擦轮,改txid和方向就行 friction_config.can_init_config.tx_id = 4; // 右摩擦轮,改txid和方向就行
friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
friction_r = DJIMotorInit(&friction_config); friction_r = DJIMotorInit(&friction_config);
// 拨盘电机 // 拨盘电机
Motor_Init_Config_s loader_config = { Motor_Init_Config_s loader_config = {
.can_init_config = { .can_init_config = {
.can_handle = &hcan2, .can_handle = &hcan1,
.tx_id = 1 .tx_id = 5,
}, },
.controller_param_init_config = { .controller_param_init_config = {
.angle_PID = { .angle_PID = {
// 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降 // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
.Kp = 500, .Kp = 130, // 10
.Ki = 100, .Ki = 0,
.Kd = 0, .Kd = 0,
.MaxOut = 5000, .MaxOut = 15000,
}, },
.speed_PID = { .speed_PID = {
.Kp = 2, // 3 .Kp = 10, // 10
.Ki = 0, // 1 .Ki = 1, // 1
.Kd = 0, .Kd = 0,
.Improve = PID_Integral_Limit, .Improve = PID_Integral_Limit,
.IntegralLimit = 5000, .IntegralLimit = 5000,
.MaxOut = 10000, .MaxOut = 10000,
}, },
.current_PID = { .current_PID = {
.Kp = 0, // 0.7 .Kp = 0.7f, // 0.7
.Ki = 0, // 0.1 .Ki = 0, // 0.1
.Kd = 0, .Kd = 0,
.Improve = PID_Integral_Limit, .Improve = PID_Integral_Limit,
.IntegralLimit = 5000, .IntegralLimit = 5000,
.MaxOut = 5000, .MaxOut = 15000,
}, },
}, },
.controller_setting_init_config = { .controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED, .angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
.outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转 .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
.close_loop_type = SPEED_LOOP | ANGLE_LOOP, .close_loop_type = CURRENT_LOOP | SPEED_LOOP | ANGLE_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向 .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
//.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE,
}, },
.motor_type = M2006 // 英雄使用m3508 .motor_type = M3508 // 英雄使用m3508
}; };
loader = DJIMotorInit(&loader_config); loader = DJIMotorInit(&loader_config);
shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s)); shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_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() void ShootTask()
{ {
//从cmd获取控制数据 // 从cmd获取控制数据
SubGetMessage(shoot_sub, &shoot_cmd_recv); SubGetMessage(shoot_sub, &shoot_cmd_recv);
// 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止) // 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止)
if (shoot_cmd_recv.shoot_mode == SHOOT_OFF) if (shoot_cmd_recv.shoot_mode == SHOOT_OFF)
{ {
DJIMotorSetRef(friction_l, 0); DJIMotorStop(friction_l);
DJIMotorSetRef(friction_r, 0); DJIMotorStop(friction_r);
DJIMotorStop(loader); DJIMotorStop(loader);
} }
else // 恢复运行 else // 恢复运行
@ -185,14 +127,13 @@ void ShootTask()
DJIMotorEnable(friction_r); DJIMotorEnable(friction_r);
DJIMotorEnable(loader); DJIMotorEnable(loader);
} }
// 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可 // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
// 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发) // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
if (hibernate_time + dead_time > DWT_GetTimeline_ms()) if (hibernate_time + dead_time > DWT_GetTimeline_ms())
return; return;
// 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换 // 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换
switch (shoot_cmd_recv.loader_mode) switch (shoot_cmd_recv.load_mode)
{ {
// 停止拨盘 // 停止拨盘
case LOAD_STOP: case LOAD_STOP:
@ -202,62 +143,56 @@ void ShootTask()
// 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射) // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用. case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环 DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
DJIMotorSetRef(loader, loader->measure.total_angle - ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度 DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 150; // 完成1发弹丸发射的时间 dead_time = 2000; // 完成1发弹丸发射的时间
break; break;
// 三连发,如果不需要后续可能删除 // 三连发,如果不需要后续可能删除
case LOAD_3_BULLET: case LOAD_3_BULLET:
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环 DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
DJIMotorSetRef(loader, loader->measure.total_angle - 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发 DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 1000; // 完成3发弹丸发射的时间 dead_time = 300; // 完成3发弹丸发射的时间
break; break;
// 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz // 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
case LOAD_BURSTFIRE: case LOAD_BURSTFIRE:
DJIMotorOuterLoop(loader, SPEED_LOOP); DJIMotorOuterLoop(loader, SPEED_LOOP);
DJIMotorSetRef(loader, -(shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8)); DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8);
// x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是)
// x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
break; break;
// 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流) // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
// 也有可能需要从switch-case中独立出来 // 也有可能需要从switch-case中独立出来
case LOAD_REVERSE: 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; break;
default: default:
while (1) while (1)
; // 未知模式,停止运行,检查指针越界,内存溢出等问题 ; // 未知模式,停止运行,检查指针越界,内存溢出等问题
} }
// 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启) // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
if (shoot_cmd_recv.friction_mode == FRICTION_ON) if (shoot_cmd_recv.friction_mode == FRICTION_ON)
{ {
// 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入 // 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
// switch (shoot_cmd_recv.bullet_speed) switch (shoot_cmd_recv.bullet_speed)
// { {
// case SMALL_AMU_15: case BIG_AMU_10:
// DJIMotorSetRef(friction_l, 0); DJIMotorSetRef(friction_l, 10);
// DJIMotorSetRef(friction_r, 0); DJIMotorSetRef(friction_r, 10);
// break; break;
// case SMALL_AMU_18: case BIG_AMU_16:
// DJIMotorSetRef(friction_l, 0); DJIMotorSetRef(friction_l, 16);
// DJIMotorSetRef(friction_r, 0); DJIMotorSetRef(friction_r, 16);
// break; break;
// case SMALL_AMU_30: case BULLET_SPEED_NONE:
// DJIMotorSetRef(friction_l, 0);
// DJIMotorSetRef(friction_r, 0);
// break;
//default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
DJIMotorSetRef(friction_l, 39000); DJIMotorSetRef(friction_l, 39000);
DJIMotorSetRef(friction_r, 39000); DJIMotorSetRef(friction_r, 39000);
// break; break;
// } default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
break;
}
} }
else // 关闭摩擦轮 else // 关闭摩擦轮
{ {
@ -268,33 +203,16 @@ void ShootTask()
// 开关弹舱盖 // 开关弹舱盖
if (shoot_cmd_recv.lid_mode == LID_CLOSE) 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) 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); PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);
} }

1
application/shoot/shoot.h
View File

@ -1,6 +1,7 @@
#ifndef SHOOT_H #ifndef SHOOT_H
#define SHOOT_H #define SHOOT_H
/** /**
* @brief 发射初始化,会被RobotInit()调用 * @brief 发射初始化,会被RobotInit()调用
* *

2
bsp/usb/bsp_usb.c
View File

@ -27,5 +27,5 @@ uint8_t *USBInit(USB_Init_Config_s usb_conf)
void USBTransmit(uint8_t *buffer, uint16_t len) void USBTransmit(uint8_t *buffer, uint16_t len)
{ {
//CDC_Transmit_FS(buffer, len); // 发送 CDC_Transmit_FS(buffer, len); // 发送
} }

26
daplink.cfg
View File

@ -1,18 +1,34 @@
# choose st-link/j-link/dap-link etc. # choose st-link/j-link/dap-link etc.
# choose CMSIS-DAP Debugger # choose CMSIS-DAP Debugger
adapter driver cmsis-dap ##adapter driver dap-link
# select SWD port # select SWD port
transport select swd ##transport select swd
# 0x10000 = 64K Flash Size # 0x10000 = 64K Flash Size
# 1MB on robomaster-C # 1MB on robomaster-C
set FLASH_SIZE 0x100000 ##set FLASH_SIZE 0x100000
source [find target/stm32f4x.cfg] ##source [find target/stm32f4x.cfg]
# download speed = 10MHz # download speed = 10MHz
# 10MHz on FireDebugger # 10MHz on FireDebugger
adapter speed 10000 ##adapter speed 10000
# connect under reset # connect under reset
# reset_config srst_only # reset_config srst_only
source [find interface/cmsis-dap.cfg]
transport select swd
# increase working area to 64KB
set WORKAREASIZE 0x10000
source [find target/stm32f4x.cfg]
reset_config none

91
modules/algorithm/crc16.c
View File

@ -1,29 +1,7 @@
#include "crc16.h" #include "crc16.h"
static uint8_t crc_tab16_init = 0; 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 ); * uint16_t crc_16( const unsigned char *input_str, size_t num_bytes );
@ -33,29 +11,21 @@ static uint16_t crc_tab16[256] = {
*要检查的字节也是一个参数。字符串中的字节数为 *要检查的字节也是一个参数。字符串中的字节数为
*受恒定大小最大值的限制。 *受恒定大小最大值的限制。
*/ */
uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes) { uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes)
// uint16_t crc; {
// const uint8_t *ptr; uint16_t crc;
// uint16_t a; const uint8_t *ptr;
//// if (!crc_tab16_init) uint16_t a;
//// init_crc16_tab(); if (!crc_tab16_init)
// crc = CRC_START_16; init_crc16_tab();
// ptr = input_str; crc = CRC_START_16;
// if (ptr != NULL) ptr = input_str;
// for (a = 0; a < num_bytes; a++) { if (ptr != NULL)
// crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t) *ptr++) & 0x00FF]; for (a = 0; a < num_bytes; a++)
// } {
// return crc; crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)*ptr++) & 0x00FF];
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; return crc;
} }
/* /*
@ -66,7 +36,8 @@ uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes) {
*要检查的字节数也是一个参数。 *要检查的字节数也是一个参数。
*/ */
uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes) { uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes)
{
uint16_t crc; uint16_t crc;
const uint8_t *ptr; const uint8_t *ptr;
uint16_t a; uint16_t a;
@ -77,9 +48,10 @@ uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes) {
crc = CRC_START_MODBUS; crc = CRC_START_MODBUS;
ptr = input_str; ptr = input_str;
if (ptr != NULL) 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; return crc;
} }
@ -90,10 +62,11 @@ uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes) {
*函数update_crc_16()根据 *函数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) if (!crc_tab16_init)
init_crc16_tab(); init_crc16_tab();
return (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t) c) & 0x00FF]; return (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)c) & 0x00FF];
} }
/* /*
@ -104,15 +77,18 @@ uint16_t update_crc_16(uint16_t crc, uint8_t c) {
*查找表首次由init_crc16_tab()例程计算 *查找表首次由init_crc16_tab()例程计算
*调用循环冗余校验函数。 *调用循环冗余校验函数。
*/ */
void init_crc16_tab(void) { void init_crc16_tab(void)
{
uint16_t i; uint16_t i;
uint16_t j; uint16_t j;
uint16_t crc; uint16_t crc;
uint16_t c; uint16_t c;
for (i = 0; i < 256; i++) { for (i = 0; i < 256; i++)
{
crc = 0; crc = 0;
c = i; c = i;
for (j = 0; j < 8; j++) { for (j = 0; j < 8; j++)
{
if ((crc ^ c) & 0x0001) if ((crc ^ c) & 0x0001)
crc = (crc >> 1) ^ CRC_POLY_16; crc = (crc >> 1) ^ CRC_POLY_16;
else else
@ -123,12 +99,3 @@ void init_crc16_tab(void) {
} }
crc_tab16_init = 1; 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]);
}

2
modules/algorithm/crc16.h
View File

@ -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 crc_modbus(const uint8_t *input_str, uint16_t num_bytes);
uint16_t update_crc_16(uint16_t crc, uint8_t c); uint16_t update_crc_16(uint16_t crc, uint8_t c);
void init_crc16_tab(void); void init_crc16_tab(void);
uint32_t VerifyCRC16CheckSum(uint8_t *pchMessage, uint32_t dwLength);
#endif #endif

44
modules/algorithm/user_lib.c
View File

@ -212,47 +212,3 @@ void MatInit(mat *m, uint8_t row, uint8_t col)
m->numRows = row; m->numRows = row;
m->pData = (float *)zmalloc(row * col * sizeof(float)); m->pData = (float *)zmalloc(row * col * sizeof(float));
} }
/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
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);
}
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;
}
/**
* @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;
}

22
modules/algorithm/user_lib.h
View File

@ -53,22 +53,6 @@ void MatInit(mat *m, uint8_t row, uint8_t col);
#ifndef PI #ifndef PI
#define PI 3.14159265354f #define PI 3.14159265354f
#endif #endif
typedef struct
{
float input; //输入数据
float out; //滤波输出的数据
float num[1]; //滤波参数
float frame_period; //滤波的时间间隔 单位 s
} first_order_filter_type_t;
typedef struct
{
float input; //输入数据
float out; //输出数据
float min_value; //限幅最小值
float max_value; //限幅最大值
float frame_period; //时间间隔
} ramp_function_source_t;
#define VAL_LIMIT(val, min, max) \ #define VAL_LIMIT(val, min, max) \
do \ do \
@ -136,12 +120,6 @@ void Cross3d(float *v1, float *v2, float *res);
float Dot3d(float *v1, float *v2); float Dot3d(float *v1, float *v2);
float AverageFilter(float new_data, float *buf, uint8_t len); float AverageFilter(float new_data, float *buf, uint8_t len);
float uint_to_float(int x_int, float x_min, float x_max, int bits);
uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits);
//一阶低通滤波初始化
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) #define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)

302
modules/auto_aim/auto_aim.c
View File

@ -1,302 +0,0 @@
//
// Created by sph on 2024/1/21.
//
#include "auto_aim.h"
#include "arm_math.h"
/**
* @brief 选择目标装甲板
* @author SJQ
* @param[in] trajectory_cal:弹道解算结构体
* @retval 返回空
*/
int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) {
aim_sel->delay_time = trajectory_cal->fly_time + trajectory_cal->extra_delay_time;
aim_sel->target_state.yaw += aim_sel->target_state.v_yaw * aim_sel->delay_time;
//计算四块装甲板的位置
int use_1 = 1;
int i = 0;
int idx = 0; // 选择的装甲板
// 为平衡步兵
if (aim_sel->target_state.armor_num == 2) {
for (i = 0; i < 2; i++) {
float tmp_yaw = aim_sel->target_state.yaw + i * PI;
float r = aim_sel->target_state.r1;
aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw);
aim_sel->armor_pose[i].z = aim_sel->target_state.z;
aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * PI;
}
// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
//
// //因为是平衡步兵 只需判断两块装甲板即可
// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[1].yaw);
// if (temp_yaw_diff < yaw_diff_min) {
// yaw_diff_min = temp_yaw_diff;
// idx = 1;
// }
// 平衡步兵选择两块装甲板中较近的装甲板
float distance_min = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2);
float distance_temp = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2);
if (distance_temp < distance_min) {
distance_min = distance_temp;
idx = 1;
}
} else if (aim_sel->target_state.armor_num == 3)//前哨站
{
for (i = 0; i < 3; i++) {
float tmp_yaw;
if (aim_sel->target_state.v_yaw <= 0)
tmp_yaw = aim_sel->target_state.yaw + i * (2.0 * PI / 3.0);
else
tmp_yaw = aim_sel->target_state.yaw - i * (2.0 * PI / 3.0);
float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2;
aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * cos(tmp_yaw);
aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * sin(tmp_yaw);
aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z +
aim_sel->target_state.dz;
aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * (2.0 * PI / 3.0);
use_1 = !use_1;
}
// //计算枪管到目标装甲板yaw最小的那个装甲板
// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
// for (i = 1; i < 3; i++) {
// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
// if (temp_yaw_diff < yaw_diff_min) {
// yaw_diff_min = temp_yaw_diff;
// idx = i;
// }
// }
// 选择两块较近的装甲板，再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
// float distance_temp0 = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2);
// float distance_temp1 = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2);
// float distance_temp2 = powf(aim_sel->armor_pose[2].x, 2) + powf(aim_sel->armor_pose[2].y, 2);
// int label_first = 0;
// int label_second = 1;
// if (distance_temp0 > distance_temp1) {
// label_first = 1;
// if (distance_temp0 > distance_temp2) {
// label_second = 2;
// } else label_second = 0;
// } else {
// label_first = 0;
// if (distance_temp1 > distance_temp2) {
// label_second = 2;
// } else label_second = 1;
// }
// 选择两块较近的装甲板
float distance[3];
for (i = 0; i < 3; i++) {
distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
}
int label_first = 0;
int label_second = 0;
for (i = 1; i < 3; i++) {
if (distance[i] < distance[label_first]) {
label_second = label_first;
label_first = i;
} else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) {
label_second = i;
}else if (distance[i] < distance[label_second]) {
label_second = i;
}
}
//再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 2));
float cos_theta_first = (aim_sel->armor_pose[label_first].x * aim_sel->target_state.x +
aim_sel->armor_pose[label_first].y * aim_sel->target_state.y) /
(sqrtf(powf(aim_sel->armor_pose[label_first].x, 2) +
powf(aim_sel->armor_pose[label_first].y, 2)) * center_length);
float cos_theta_second = (aim_sel->armor_pose[label_second].x * aim_sel->target_state.x +
aim_sel->armor_pose[label_second].y * aim_sel->target_state.y) /
(sqrtf(powf(aim_sel->armor_pose[label_second].x, 2) +
powf(aim_sel->armor_pose[label_second].y, 2)) * center_length);
if (cos_theta_first > cos_theta_second)
idx = label_first;
else
idx = label_second;
} else {
for (i = 0; i < 4; i++) {
float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0;
float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2;
aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw);
aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z +
aim_sel->target_state.dz;
aim_sel->armor_pose[i].yaw = tmp_yaw;
use_1 = !use_1;
}
// //计算枪管到目标装甲板yaw最小的那个装甲板
// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
// for (i = 1; i < 4; i++) {
// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
// if (temp_yaw_diff < yaw_diff_min) {
// yaw_diff_min = temp_yaw_diff;
// idx = i;
// }
// }
// 选择两块较近的装甲板
float distance[4];
for (i = 0; i < 4; i++) {
distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
}
int label_first = 0;
int label_second = 1;
if(distance[label_first] > distance[label_second])
{
label_first = 1;
label_second = 0;
}
if(distance[2]<distance[label_first]){
label_second = label_first;
label_first = 2;
}
else if(distance[2]<distance[label_second])
label_second = 2;
if(distance[3]<distance[label_first]){
label_second = label_first;
label_first = 3;
}
else if(distance[3]<distance[label_second])
label_second = 3;
//再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 2));
float cos_theta_first = (aim_sel->armor_pose[label_first].x * aim_sel->target_state.x +
aim_sel->armor_pose[label_first].y * aim_sel->target_state.y) /
(sqrtf(powf(aim_sel->armor_pose[label_first].x, 2) +
powf(aim_sel->armor_pose[label_first].y, 2)) * center_length);
float cos_theta_second = (aim_sel->armor_pose[label_second].x * aim_sel->target_state.x +
aim_sel->armor_pose[label_second].y * aim_sel->target_state.y) /
(sqrtf(powf(aim_sel->armor_pose[label_second].x, 2) +
powf(aim_sel->armor_pose[label_second].y, 2)) * center_length);
if (cos_theta_first > cos_theta_second)
idx = label_first;
else
idx = label_second;
}
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;
}

77
modules/auto_aim/auto_aim.h
View File

@ -1,77 +0,0 @@
//
// Created by sph on 2024/1/21.
//
#ifndef BASIC_FRAMEWORK_AUTO_AIM_H
#define BASIC_FRAMEWORK_AUTO_AIM_H
#include "master_process.h"
//弹道解算
typedef struct
{
float v0; //子弹射速
float velocity[3];//目标xyz速度
float vx; //目标相对枪口方向的速度
float vy;
float alpha; //目标初始航向角
float position_xy[2];//目标xy坐标
float z; //目标z坐标
float fly_time; //子弹飞行时间
float fly_time2; //子弹飞行时间平方
float extra_delay_time ;
float theta_0; //初始目标角度
float theta_k; //迭代目标角度
float dis; //目标距离
float dis2; //目标距离平方
float err_k; //迭代误差
uint8_t k; //迭代次数
float h_k; //迭代高度
float h_r; //目标真实高度
float cmd_yaw;
float cmd_pitch;
} Trajectory_Type_t;
//整车状态
typedef struct
{
float x;
float y;
float z;
float yaw;
float vx;
float vy;
float vz;
float v_yaw;
float r1;
float r2;
float dz;
uint8_t armor_type;
uint8_t armor_num;
}Target_State_Type_t;
//预瞄点
typedef struct
{
float x;
float y;
float z;
float yaw;
}Armor_Pose_Type_t;
typedef struct
{
Target_State_Type_t target_state; //整车状态
Armor_Pose_Type_t armor_pose[4]; //四个装甲板状态
float aim_point[3]; //预瞄点
float delay_time; //预瞄时间差
uint8_t suggest_fire;
}Aim_Select_Type_t;
int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal);
float get_fly_time(float x, float vx, float v_x0);
void get_cmd_angle(Trajectory_Type_t *trajectory_cal);
int auto_aim(Aim_Select_Type_t *aim_sel,Trajectory_Type_t *trajectory_cal,RecievePacket_t *receive_packet);
#endif //BASIC_FRAMEWORK_AUTO_AIM_H

57
modules/imu/ins_task.c
View File

@ -20,19 +20,13 @@
#include "general_def.h" #include "general_def.h"
#include "master_process.h" #include "master_process.h"
#include "vofa.h"
static INS_t INS; static INS_t INS;
static IMU_Param_t IMU_Param; static IMU_Param_t IMU_Param;
static PIDInstance TempCtrl = {0}; 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 yb[3] = {0, 1, 0};
const float zb[3] = {-1, 0, 0}; const float zb[3] = {0, 0, 1};
// 用于获取两次采样之间的时间间隔 // 用于获取两次采样之间的时间间隔
static uint32_t INS_DWT_Count = 0; static uint32_t INS_DWT_Count = 0;
@ -66,9 +60,9 @@ static void InitQuaternion(float *init_q4)
for (uint8_t i = 0; i < 100; ++i) for (uint8_t i = 0; i < 100; ++i)
{ {
BMI088_Read(&BMI088); BMI088_Read(&BMI088);
acc_init[X] += -BMI088.Accel[2]; acc_init[X] += BMI088.Accel[X];
acc_init[Y] += BMI088.Accel[Y]; acc_init[Y] += BMI088.Accel[Y];
acc_init[Z] += BMI088.Accel[0]; acc_init[Z] += BMI088.Accel[Z];
DWT_Delay(0.001); DWT_Delay(0.001);
} }
for (uint8_t i = 0; i < 3; ++i) for (uint8_t i = 0; i < 3; ++i)
@ -78,9 +72,6 @@ static void InitQuaternion(float *init_q4)
float angle = acosf(Dot3d(acc_init, gravity_norm)); float angle = acosf(Dot3d(acc_init, gravity_norm));
Cross3d(acc_init, gravity_norm, axis_rot); Cross3d(acc_init, gravity_norm, axis_rot);
Norm3d(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); init_q4[0] = cosf(angle / 2.0f);
for (uint8_t i = 0; i < 2; ++i) for (uint8_t i = 0; i < 2; ++i)
init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量) init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
@ -106,7 +97,7 @@ attitude_t *INS_Init(void)
IMU_Param.flag = 1; IMU_Param.flag = 1;
float init_quaternion[4] = {0}; float init_quaternion[4] = {0};
InitQuaternion(init_quaternion);//计算由实际重力加速度方向(0,0,1)到当前加速度方向的旋转 InitQuaternion(init_quaternion);
IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0); IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0);
// imu heat init // imu heat init
PID_Init_Config_s config = {.MaxOut = 2000, PID_Init_Config_s config = {.MaxOut = 2000,
@ -138,12 +129,12 @@ void INS_Task(void)
{ {
BMI088_Read(&BMI088); BMI088_Read(&BMI088);
INS.Accel[X] = -BMI088.Accel[2]; INS.Accel[X] = BMI088.Accel[X];
INS.Accel[Y] = BMI088.Accel[1]; INS.Accel[Y] = BMI088.Accel[Y];
INS.Accel[Z] = BMI088.Accel[0]; INS.Accel[Z] = BMI088.Accel[Z];
INS.Gyro[X] = -BMI088.Gyro[2]; INS.Gyro[X] = BMI088.Gyro[X];
INS.Gyro[Y] = BMI088.Gyro[1]; INS.Gyro[Y] = BMI088.Gyro[Y];
INS.Gyro[Z] = BMI088.Gyro[0]; INS.Gyro[Z] = BMI088.Gyro[Z];
// demo function,用于修正安装误差,可以不管,本demo暂时没用 // demo function,用于修正安装误差,可以不管,本demo暂时没用
IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel); IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
@ -171,36 +162,12 @@ void INS_Task(void)
} }
BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
INS.Yaw = QEKF_INS.Yaw; INS.Yaw = QEKF_INS.Yaw;
INS.Pitch = QEKF_INS.Pitch; INS.Pitch = QEKF_INS.Pitch;
INS.Roll = QEKF_INS.Roll; 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; INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
// float vofa_send_data[9]; VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
// 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 // temperature control

8
modules/imu/ins_task.h
View File

@ -23,10 +23,6 @@
#define Y 1 #define Y 1
#define Z 2 #define Z 2
//#define X 2
//#define Y 1
//#define Z 0
#define INS_TASK_PERIOD 1 #define INS_TASK_PERIOD 1
typedef struct typedef struct
@ -38,7 +34,6 @@ typedef struct
float Pitch; float Pitch;
float Yaw; float Yaw;
float YawTotalAngle; float YawTotalAngle;
} attitude_t; // 最终解算得到的角度,以及yaw转动的总角度(方便多圈控制) } attitude_t; // 最终解算得到的角度,以及yaw转动的总角度(方便多圈控制)
typedef struct typedef struct
@ -68,9 +63,6 @@ typedef struct
float Yaw; float Yaw;
float YawTotalAngle; float YawTotalAngle;
float YawAngleLast;
float YawRoundCount;
uint8_t init; uint8_t init;
} INS_t; } INS_t;

77
modules/master_machine/master_process.c
View File

@ -13,42 +13,25 @@
#include "daemon.h" #include "daemon.h"
#include "bsp_log.h" #include "bsp_log.h"
#include "robot_def.h" #include "robot_def.h"
#include "crc16.h"
static RecievePacket_t recv_data; static Vision_Recv_s recv_data;
static SendPacket_t send_data; static Vision_Send_s send_data;
static DaemonInstance *vision_daemon_instance; 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.detect_color = enemy_color; send_data.enemy_color = enemy_color;
send_data.reserved = 0; send_data.work_mode = work_mode;
send_data.bullet_speed = bullet_speed;
} }
//void VisionSetAltitude(float yaw, float pitch) void VisionSetAltitude(float yaw, float pitch, float roll)
//{
// send_data.yaw = yaw;
// send_data.pitch = pitch;
//}
void VisionSetAltitude(float yaw, float pitch)
{ {
send_data.yaw = yaw; send_data.yaw = yaw;
send_data.pitch = pitch; 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调用 * @brief 离线回调函数,将在daemon.c中被daemon task调用
* @attention 由于HAL库的设计问题,串口开启DMA接收之后同时发送有概率出现__HAL_LOCK()导致的死锁,使得无法 * @attention 由于HAL库的设计问题,串口开启DMA接收之后同时发送有概率出现__HAL_LOCK()导致的死锁,使得无法
@ -130,26 +113,19 @@ void VisionSend()
#ifdef VISION_USE_VCP #ifdef VISION_USE_VCP
#include "bsp_usb.h" #include "bsp_usb.h"
static uint8_t *vis_recv_buff; //接收到的原始数据 static uint8_t *vis_recv_buff;
static void DecodeVision(uint16_t recv_len) static void DecodeVision(uint16_t recv_len)
{ {
// uint16_t flag_register; uint16_t flag_register;
// get_protocol_info(vis_recv_buff, &flag_register, (uint8_t *)&recv_data.pitch); get_protocol_info(vis_recv_buff, &flag_register, (uint8_t *)&recv_data.pitch);
// // TODO: code to resolve flag_register; // 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));
}
}
} }
/* 视觉通信初始化 */ /* 视觉通信初始化 */
RecievePacket_t *VisionInit(void) Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
{ {
// UNUSED(_handle); // 仅为了消除警告 UNUSED(_handle); // 仅为了消除警告
USB_Init_Config_s conf = {.rx_cbk = DecodeVision}; USB_Init_Config_s conf = {.rx_cbk = DecodeVision};
vis_recv_buff = USBInit(conf); vis_recv_buff = USBInit(conf);
@ -166,23 +142,14 @@ RecievePacket_t *VisionInit(void)
void VisionSend() void VisionSend()
{ {
// static uint16_t flag_register; static uint16_t flag_register;
// static uint8_t send_buff[VISION_SEND_SIZE]; static uint8_t send_buff[VISION_SEND_SIZE];
// static uint16_t tx_len; static uint16_t tx_len;
// // TODO: code to set flag_register // TODO: code to set flag_register
// flag_register = 30 << 8 | 0b00000001; flag_register = 30 << 8 | 0b00000001;
// // 将数据转化为seasky协议的数据包 // 将数据转化为seasky协议的数据包
// get_protocol_send_data(0x02, flag_register, &send_data.yaw, 3, send_buff, &tx_len); get_protocol_send_data(0x02, flag_register, &send_data.yaw, 3, send_buff, &tx_len);
// USBTransmit(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 #endif // VISION_USE_VCP

74
modules/master_machine/master_process.h
View File

@ -8,19 +8,22 @@
#define VISION_SEND_SIZE 36u #define VISION_SEND_SIZE 36u
#pragma pack(1) #pragma pack(1)
typedef enum { typedef enum
{
NO_FIRE = 0, NO_FIRE = 0,
AUTO_FIRE = 1, AUTO_FIRE = 1,
AUTO_AIM = 2 AUTO_AIM = 2
} Fire_Mode_e; } Fire_Mode_e;
typedef enum { typedef enum
{
NO_TARGET = 0, NO_TARGET = 0,
TARGET_CONVERGING = 1, TARGET_CONVERGING = 1,
READY_TO_FIRE = 2 READY_TO_FIRE = 2
} Target_State_e; } Target_State_e;
typedef enum { typedef enum
{
NO_TARGET_NUM = 0, NO_TARGET_NUM = 0,
HERO1 = 1, HERO1 = 1,
ENGINEER2 = 2, ENGINEER2 = 2,
@ -32,7 +35,8 @@ typedef enum {
BASE = 8 BASE = 8
} Target_Type_e; } Target_Type_e;
typedef struct { typedef struct
{
Fire_Mode_e fire_mode; Fire_Mode_e fire_mode;
Target_State_e target_state; Target_State_e target_state;
Target_Type_e target_type; Target_Type_e target_type;
@ -41,21 +45,22 @@ typedef struct {
float yaw; float yaw;
} Vision_Recv_s; } Vision_Recv_s;
typedef enum { typedef enum
// COLOR_NONE = 0, {
// COLOR_BLUE = 1, COLOR_NONE = 0,
// COLOR_RED = 2, COLOR_BLUE = 1,
ENEMY_COLOR_RED = 0, COLOR_RED = 2,
ENEMY_COLOR_BLUE = 1,
} Enemy_Color_e; } Enemy_Color_e;
typedef enum { typedef enum
{
VISION_MODE_AIM = 0, VISION_MODE_AIM = 0,
VISION_MODE_SMALL_BUFF = 1, VISION_MODE_SMALL_BUFF = 1,
VISION_MODE_BIG_BUFF = 2 VISION_MODE_BIG_BUFF = 2
} Work_Mode_e; } Work_Mode_e;
typedef enum { typedef enum
{
BULLET_SPEED_NONE = 0, BULLET_SPEED_NONE = 0,
BIG_AMU_10 = 10, BIG_AMU_10 = 10,
SMALL_AMU_15 = 15, SMALL_AMU_15 = 15,
@ -64,7 +69,8 @@ typedef enum {
SMALL_AMU_30 = 30, SMALL_AMU_30 = 30,
} Bullet_Speed_e; } Bullet_Speed_e;
typedef struct { typedef struct
{
Enemy_Color_e enemy_color; Enemy_Color_e enemy_color;
Work_Mode_e work_mode; Work_Mode_e work_mode;
Bullet_Speed_e bullet_speed; Bullet_Speed_e bullet_speed;
@ -73,38 +79,6 @@ typedef struct {
float pitch; float pitch;
float roll; float roll;
} Vision_Send_s; } 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() #pragma pack()
/** /**
@ -112,8 +86,7 @@ typedef struct {
* *
* @param handle 用于和视觉通信的串口handle(C板上一般为USART1,丝印为USART2,4pin) * @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 发送视觉数据 * @brief 发送视觉数据
@ -128,8 +101,7 @@ void VisionSend();
* @param work_mode * @param work_mode
* @param bullet_speed * @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 设置发送数据的姿态部分 * @brief 设置发送数据的姿态部分
@ -137,8 +109,6 @@ void VisionSetFlag(Enemy_Color_e enemy_color);
* @param yaw * @param yaw
* @param pitch * @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 #endif // !MASTER_PROCESS_H

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

@ -2,7 +2,6 @@
#include "general_def.h" #include "general_def.h"
#include "bsp_dwt.h" #include "bsp_dwt.h"
#include "bsp_log.h" #include "bsp_log.h"
#include "user_lib.h"
static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用 static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */ /* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
@ -20,52 +19,42 @@ static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在co
* can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF * can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF
* can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF * can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF
*/ */
static CANInstance sender_assignment[10] = { 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] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
0}}, [1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .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 = { [2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
0}}, [3] = {.can_handle = &hcan2, .txconf.StdId = 0x1ff, .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 = { [4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
0}}, [5] = {.can_handle = &hcan2, .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}}
}; };
/** /**
* @brief 10个用于确认是否有电机注册到sender_assignment中的标志位,防止发送空帧,此变量将在DJIMotorControl()使用 * @brief 6个用于确认是否有电机注册到sender_assignment中的标志位,防止发送空帧,此变量将在DJIMotorControl()使用
* flag的初始化在 MotorSenderGrouping()中进行 * flag的初始化在 MotorSenderGrouping()中进行
*/ */
static uint8_t sender_enable_flag[10] = {0}; static uint8_t sender_enable_flag[6] = {0};
/** /**
* @brief 根据电调/拨码开关上的ID,根据说明书的默认id分配方式计算发送ID和接收ID, * @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_id = config->tx_id - 1; // 下标从零开始,先减一方便赋值
uint8_t motor_send_num; uint8_t motor_send_num;
uint8_t motor_grouping; uint8_t motor_grouping;
switch (motor->motor_type) { switch (motor->motor_type)
{
case M2006: case M2006:
case M3508: case M3508:
if (motor_id < 4) // 根据ID分组 if (motor_id < 4) // 根据ID分组
{ {
motor_send_num = motor_id; motor_send_num = motor_id;
motor_grouping = config->can_handle == &hcan1 ? 1 : 4; motor_grouping = config->can_handle == &hcan1 ? 1 : 4;
} else { }
else
{
motor_send_num = motor_id - 4; motor_send_num = motor_id - 4;
motor_grouping = config->can_handle == &hcan1 ? 0 : 3; motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
} }
@ -77,11 +66,11 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
motor->sender_group = motor_grouping; motor->sender_group = motor_grouping;
// 检查是否发生id冲突 // 检查是否发生id冲突
for (size_t i = 0; i < idx; ++i) { for (size_t i = 0; i < idx; ++i)
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && {
dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) { if (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."); 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; 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! (((不是) 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); LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
@ -90,19 +79,6 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
break; break;
case GM6020: case GM6020:
if(motor->motor_control_type == CURRENT_CONTROL) //6020电流控制帧id 0x1fe 0x2fe
{
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) if (motor_id < 4)
{ {
motor_send_num = motor_id; motor_send_num = motor_id;
@ -113,18 +89,17 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
motor_send_num = motor_id - 4; motor_send_num = motor_id - 4;
motor_grouping = config->can_handle == &hcan1 ? 2 : 5; motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
} }
}
config->rx_id = 0x204 + motor_id + 1; // 把ID+1,进行分组设置 config->rx_id = 0x204 + motor_id + 1; // 把ID+1,进行分组设置
sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册 sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
motor->message_num = motor_send_num; motor->message_num = motor_send_num;
motor->sender_group = motor_grouping; motor->sender_group = motor_grouping;
for (size_t i = 0; i < idx; ++i) { for (size_t i = 0; i < idx; ++i)
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && {
dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) { if (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."); 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; 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! (((不是) 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); LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
@ -144,11 +119,12 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
* *
* @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例 * @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例
*/ */
static void DecodeDJIMotor(CANInstance *_instance) { static void DecodeDJIMotor(CANInstance *_instance)
{
// 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址 // 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
// _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针 // _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针
uint8_t *rxbuff = _instance->rx_buff; uint8_t *rxbuff = _instance->rx_buff;
DJIMotorInstance *motor = (DJIMotorInstance *) _instance->id; DJIMotorInstance *motor = (DJIMotorInstance *)_instance->id;
DJI_Motor_Measure_s *measure = &motor->measure; // measure要多次使用,保存指针减小访存开销 DJI_Motor_Measure_s *measure = &motor->measure; // measure要多次使用,保存指针减小访存开销
DaemonReload(motor->daemon); DaemonReload(motor->daemon);
@ -156,12 +132,12 @@ static void DecodeDJIMotor(CANInstance *_instance) {
// 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册 // 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册
measure->last_ecd = measure->ecd; measure->last_ecd = measure->ecd;
measure->ecd = ((uint16_t) rxbuff[0]) << 8 | rxbuff[1]; measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1];
measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float) measure->ecd; measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float)measure->ecd;
measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps + 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 + 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]; measure->temperature = rxbuff[6];
// 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了 // 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了
@ -172,21 +148,22 @@ static void DecodeDJIMotor(CANInstance *_instance) {
measure->total_angle = measure->total_round * 360 + measure->angle_single_round; measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
} }
static void DJIMotorLostCallback(void *motor_ptr) { static void DJIMotorLostCallback(void *motor_ptr)
DJIMotorInstance *motor = (DJIMotorInstance *) motor_ptr; {
DJIMotorInstance *motor = (DJIMotorInstance *)motor_ptr;
uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2; 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); 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 *DJIMotorInit(Motor_Init_Config_s *config)
DJIMotorInstance *instance = (DJIMotorInstance *) malloc(sizeof(DJIMotorInstance)); {
DJIMotorInstance *instance = (DJIMotorInstance *)malloc(sizeof(DJIMotorInstance));
memset(instance, 0, sizeof(DJIMotorInstance)); memset(instance, 0, sizeof(DJIMotorInstance));
// motor basic setting 电机基本设置 // motor basic setting 电机基本设置
instance->motor_type = config->motor_type; // 6020 or 2006 or 3508 instance->motor_type = config->motor_type; // 6020 or 2006 or 3508
instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等 instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等
instance->motor_control_type = config->motor_control_type; //电流控制or电压控制
// motor controller init 电机控制器初始化 // motor controller init 电机控制器初始化
PIDInit(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID); PIDInit(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
@ -210,7 +187,7 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) {
Daemon_Init_Config_s daemon_config = { Daemon_Init_Config_s daemon_config = {
.callback = DJIMotorLostCallback, .callback = DJIMotorLostCallback,
.owner_id = instance, .owner_id = instance,
.reload_count = 100, // 20ms未收到数据则丢失 .reload_count = 2, // 20ms未收到数据则丢失
}; };
instance->daemon = DaemonRegister(&daemon_config); instance->daemon = DaemonRegister(&daemon_config);
@ -220,7 +197,8 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) {
} }
/* 电流只能通过电机自带传感器监测,后续考虑加入力矩传感器应变片等 */ /* 电流只能通过电机自带传感器监测,后续考虑加入力矩传感器应变片等 */
void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type) { void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type)
{
if (loop == ANGLE_LOOP) if (loop == ANGLE_LOOP)
motor->motor_settings.angle_feedback_source = type; motor->motor_settings.angle_feedback_source = type;
else if (loop == SPEED_LOOP) else if (loop == SPEED_LOOP)
@ -229,38 +207,31 @@ void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback
LOGERROR("[dji_motor] loop type error, check memory access and func param"); // 检查是否传入了正确的LOOP类型,或发生了指针越界 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; motor->stop_flag = MOTOR_STOP;
} }
void DJIMotorEnable(DJIMotorInstance *motor) { void DJIMotorEnable(DJIMotorInstance *motor)
{
motor->stop_flag = MOTOR_ENALBED; 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; 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; motor->motor_controller.pid_ref = ref;
} }
static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f};
//依据3508电机功率模型，预测电机输出功率
static float EstimatePower(DJIMotorInstance* chassis_motor,float output)
{
float I_cmd = chassis_motor->motor_controller.current_PID.Output;
float w = chassis_motor->measure.speed_aps /6 ;//aps to rpm
float power = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w + motor_power_K[2]*I_cmd*I_cmd;
return power;
}
// 为所有电机实例计算三环PID,发送控制报文 // 为所有电机实例计算三环PID,发送控制报文
void DJIMotorControl() { void DJIMotorControl()
{
// 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性 // 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性
uint8_t group, num; // 电机组号和组内编号 uint8_t group, num; // 电机组号和组内编号
int16_t set; // 电机控制CAN发送设定值 int16_t set; // 电机控制CAN发送设定值
@ -271,7 +242,8 @@ void DJIMotorControl() {
float pid_measure, pid_ref; // 电机PID测量值和设定值 float pid_measure, pid_ref; // 电机PID测量值和设定值
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值 // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
for (size_t i = 0; i < idx; ++i) { // 减小访存开销,先保存指针引用 for (size_t i = 0; i < idx; ++i)
{ // 减小访存开销,先保存指针引用
motor = dji_motor_instance[i]; motor = dji_motor_instance[i];
motor_setting = &motor->motor_settings; motor_setting = &motor->motor_settings;
motor_controller = &motor->motor_controller; motor_controller = &motor->motor_controller;
@ -282,7 +254,8 @@ void DJIMotorControl() {
// pid_ref会顺次通过被启用的闭环充当数据的载体 // 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) if (motor_setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_angle_feedback_ptr; pid_measure = *motor_controller->other_angle_feedback_ptr;
else else
@ -292,21 +265,16 @@ 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) if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD)
pid_ref += *motor_controller->speed_feedforward_ptr; pid_ref += *motor_controller->speed_feedforward_ptr;
if (motor_setting->speed_feedback_source == OTHER_FEED) if (motor_setting->speed_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_speed_feedback_ptr; pid_measure = *motor_controller->other_speed_feedback_ptr;
else // MOTOR_FEED else // MOTOR_FEED
{
if(motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
pid_measure = - measure->speed_aps;
else //NORMAL
pid_measure = measure->speed_aps; pid_measure = measure->speed_aps;
}
// 更新pid_ref进入下一个环 // 更新pid_ref进入下一个环
pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref); pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
} }
@ -314,54 +282,22 @@ void DJIMotorControl() {
// 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈 // 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD) if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor_controller->current_feedforward_ptr; pid_ref += *motor_controller->current_feedforward_ptr;
if (motor_setting->close_loop_type & CURRENT_LOOP) { if (motor_setting->close_loop_type & CURRENT_LOOP)
//现在电调都有内置电流环，无需pid计算 {
//pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref); pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
} }
if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE) if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
pid_ref *= -1; 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; group = motor->sender_group;
num = motor->message_num; num = motor->message_num;
sender_assignment[group].tx_buff[2 * num] = (uint8_t) (set >> 8); // 低八位 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 + 1] = (uint8_t)(set & 0x00ff); // 高八位
// 若该电机处于停止状态,直接将buff置零 // 若该电机处于停止状态,直接将buff置零
if (motor->stop_flag == MOTOR_STOP) if (motor->stop_flag == MOTOR_STOP)
@ -369,8 +305,10 @@ void DJIMotorControl() {
} }
// 遍历flag,检查是否要发送这一帧报文 // 遍历flag,检查是否要发送这一帧报文
for (size_t i = 0; i < 10; ++i) { for (size_t i = 0; i < 6; ++i)
if (sender_enable_flag[i]) { {
if (sender_enable_flag[i])
{
CANTransmit(&sender_assignment[i], 1); CANTransmit(&sender_assignment[i], 1);
} }
} }

1
modules/motor/DJImotor/dji_motor.h
View File

@ -58,7 +58,6 @@ typedef struct
uint8_t message_num; uint8_t message_num;
Motor_Type_e motor_type; // 电机类型 Motor_Type_e motor_type; // 电机类型
Motor_Control_Type_e motor_control_type;//电机控制方式
Motor_Working_Type_e stop_flag; // 启停标志 Motor_Working_Type_e stop_flag; // 启停标志
DaemonInstance* daemon; DaemonInstance* daemon;

310
modules/motor/DMmotor/dmmotor.c
View File

@ -1,310 +0,0 @@
#include "dmmotor.h"
#include "memory.h"
#include "general_def.h"
#include "user_lib.h"
#include "cmsis_os.h"
#include "string.h"
#include "daemon.h"
#include "stdlib.h"
#include "bsp_log.h"
static uint8_t idx;
static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT];
static osThreadId dm_task_handle[DM_MOTOR_CNT];
/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
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);
}
}

76
modules/motor/DMmotor/dmmotor.h
View File

@ -1,76 +0,0 @@
#ifndef DM4310_H
#define DM4310_H
#include <stdint.h>
#include "bsp_can.h"
#include "controller.h"
#include "motor_def.h"
#include "daemon.h"
#define DM_MOTOR_CNT 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

179
modules/motor/ECmotor/ECA8210.c
View File

@ -1,179 +0,0 @@
//
// 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);
}

130
modules/motor/ECmotor/ECA8210.h
View File

@ -1,130 +0,0 @@
//
// 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

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

@ -26,8 +26,19 @@ static void HTMotorSetMode(HTMotor_Mode_t cmd, HTMotorInstance *motor)
CANTransmit(motor->motor_can_instace, 1); CANTransmit(motor->motor_can_instace, 1);
memcpy(motor->motor_can_instace->tx_buff, zero_buff, 6); memcpy(motor->motor_can_instace->tx_buff, zero_buff, 6);
} }
/* 两个用于将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;
}
/** /**
* @brief 解析电机反馈值 * @brief 解析电机反馈值

23
modules/motor/motor_def.h
View File

@ -68,12 +68,6 @@ typedef enum
MOTOR_ENALBED = 1, MOTOR_ENALBED = 1,
} Motor_Working_Type_e; } Motor_Working_Type_e;
typedef enum
{
NO_POWER_LIMIT = 0,
POWER_LIMIT_ON = 1,
} Power_Limit_Type_e;
/* 电机控制设置,包括闭环类型,反转标志和反馈来源 */ /* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
typedef struct typedef struct
{ {
@ -84,19 +78,9 @@ typedef struct
Feedback_Source_e angle_feedback_source; // 角度反馈类型 Feedback_Source_e angle_feedback_source; // 角度反馈类型
Feedback_Source_e speed_feedback_source; // 速度反馈类型 Feedback_Source_e speed_feedback_source; // 速度反馈类型
Feedfoward_Type_e feedforward_flag; // 前馈标志 Feedfoward_Type_e feedforward_flag; // 前馈标志
Power_Limit_Type_e power_limit_flag; //功率限制标志
} Motor_Control_Setting_s; } Motor_Control_Setting_s;
/* 电机控制方式枚举 */
typedef enum
{
CONTROL_TYPE_NONE = 0,
CURRENT_CONTROL,
VOLTAGE_CONTROL,
} Motor_Control_Type_e;
/* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/ /* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/
// 后续增加前馈数据指针 // 后续增加前馈数据指针
typedef struct typedef struct
@ -111,10 +95,6 @@ typedef struct
PIDInstance angle_PID; PIDInstance angle_PID;
float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环 float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环
float motor_power_max; //每个电机分配的功率上限
float motor_power_predict; //根据模型预测的电机功率
float motor_power_scale; //电机功率缩放比例
} Motor_Controller_s; } Motor_Controller_s;
/* 电机类型枚举 */ /* 电机类型枚举 */
@ -126,8 +106,6 @@ typedef enum
M2006, M2006,
LK9025, LK9025,
HT04, HT04,
DM4310,
DM6006,
} Motor_Type_e; } Motor_Type_e;
/** /**
@ -155,7 +133,6 @@ typedef struct
Motor_Control_Setting_s controller_setting_init_config; Motor_Control_Setting_s controller_setting_init_config;
Motor_Type_e motor_type; Motor_Type_e motor_type;
CAN_Init_Config_s can_init_config; CAN_Init_Config_s can_init_config;
Motor_Control_Type_e motor_control_type;
} Motor_Init_Config_s; } Motor_Init_Config_s;
#endif // !MOTOR_DEF_H #endif // !MOTOR_DEF_H

5
modules/motor/motor_task.c
View File

@ -1,11 +1,9 @@
#include "motor_task.h" #include "motor_task.h"
#include "LK9025.h" #include "LK9025.h"
#include "ECA8210.h"
#include "HT04.h" #include "HT04.h"
#include "dji_motor.h" #include "dji_motor.h"
#include "step_motor.h" #include "step_motor.h"
#include "servo_motor.h" #include "servo_motor.h"
#include "dmmotor.h"
void MotorControlTask() void MotorControlTask()
{ {
@ -16,8 +14,7 @@ void MotorControlTask()
/* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
LKMotorControl(); LKMotorControl();
ECMotorControl();
//DMMotorControl();
// legacy support // legacy support
// 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞 // 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞
// 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换 // 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换

38
modules/power_meter/power_meter.c
View File

@ -1,38 +0,0 @@
//
// 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"

39
modules/power_meter/power_meter.h
View File

@ -1,39 +0,0 @@
//
// 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

85
modules/referee/referee_protocol.h
View File

@ -65,7 +65,7 @@ typedef struct
/****************************cmd_id命令码说明****************************/ /****************************cmd_id命令码说明****************************/
/****************************cmd_id命令码说明****************************/ /****************************cmd_id命令码说明****************************/
// V1.6.1 裁判系统串口协议
/* 命令码ID,用来判断接收的是什么数据 */ /* 命令码ID,用来判断接收的是什么数据 */
typedef enum typedef enum
{ {
@ -83,28 +83,24 @@ typedef enum
ID_robot_hurt = 0x0206, // 伤害状态数据 ID_robot_hurt = 0x0206, // 伤害状态数据
ID_shoot_data = 0x0207, // 实时射击数据 ID_shoot_data = 0x0207, // 实时射击数据
ID_student_interactive = 0x0301, // 机器人间交互数据 ID_student_interactive = 0x0301, // 机器人间交互数据
ID_custom_robot = 0x302, // 自定义控制器数据（图传链路）
ID_remote_control = 0x304, // 键鼠遥控数据（图传链路）
} CmdID_e; } CmdID_e;
/* 命令码数据段长,根据官方协议来定义长度，还有自定义数据长度 */ /* 命令码数据段长,根据官方协议来定义长度，还有自定义数据长度 */
typedef enum typedef enum
{ {
LEN_game_state = 11, // 0x0001 LEN_game_state = 3, // 0x0001
LEN_game_result = 1, // 0x0002 LEN_game_result = 1, // 0x0002
LEN_game_robot_HP = 32, // 0x0003 LEN_game_robot_HP = 2, // 0x0003
LEN_event_data = 4, // 0x0101 LEN_event_data = 4, // 0x0101
LEN_supply_projectile_action = 4, // 0x0102 LEN_supply_projectile_action = 4, // 0x0102
LEN_game_robot_state = 13, // 0x0201 LEN_game_robot_state = 27, // 0x0201
LEN_power_heat_data = 16, // 0x0202 LEN_power_heat_data = 14, // 0x0202
LEN_game_robot_pos = 16, // 0x0203 LEN_game_robot_pos = 16, // 0x0203
LEN_buff_musk = 6, // 0x0204 LEN_buff_musk = 1, // 0x0204
LEN_aerial_robot_energy = 2, // 0x0205 LEN_aerial_robot_energy = 1, // 0x0205
LEN_robot_hurt = 1, // 0x0206 LEN_robot_hurt = 1, // 0x0206
LEN_shoot_data = 7, // 0x0207 LEN_shoot_data = 7, // 0x0207
LEN_receive_data = 6 + Communicate_Data_LEN, // 0x0301 LEN_receive_data = 6 + Communicate_Data_LEN, // 0x0301
LEN__custom_robot = 30, // 0x0302
LEN_remote_control = 12, // 0x0304
} JudgeDataLength_e; } JudgeDataLength_e;
@ -144,7 +140,7 @@ typedef struct
uint16_t blue_7_robot_HP; uint16_t blue_7_robot_HP;
uint16_t blue_outpost_HP; uint16_t blue_outpost_HP;
uint16_t blue_base_HP; uint16_t blue_base_HP;
}ext_game_robot_HP_t; } ext_game_robot_HP_t;
/* ID: 0x0101 Byte: 4 场地事件数据 */ /* ID: 0x0101 Byte: 4 场地事件数据 */
typedef struct typedef struct
@ -152,7 +148,7 @@ typedef struct
uint32_t event_type; uint32_t event_type;
} ext_event_data_t; } ext_event_data_t;
/* ID: 0x0102 Byte: 4 场地补给站动作标识数据 */ /* ID: 0x0102 Byte: 3 场地补给站动作标识数据 */
typedef struct typedef struct
{ {
uint8_t supply_projectile_id; uint8_t supply_projectile_id;
@ -161,32 +157,37 @@ typedef struct
uint8_t supply_projectile_num; uint8_t supply_projectile_num;
} ext_supply_projectile_action_t; } ext_supply_projectile_action_t;
/* ID: 0X0201 Byte: 13 机器人性能体系数据 */ /* ID: 0X0201 Byte: 27 机器人状态数据 */
typedef struct typedef struct
{ {
uint8_t robot_id; uint8_t robot_id;
uint8_t robot_level; uint8_t robot_level;
uint16_t current_HP; uint16_t remain_HP;
uint16_t maximum_HP; uint16_t max_HP;
uint16_t shooter_barrel_cooling_value; uint16_t shooter_id1_17mm_cooling_rate;
uint16_t shooter_barrel_heat_limit; 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; uint16_t chassis_power_limit;
uint8_t mains_power_gimbal_output : 1;
uint8_t power_management_gimbal_output : 1; uint8_t mains_power_chassis_output : 1;
uint8_t power_management_chassis_output : 1; uint8_t mains_power_shooter_output : 1;
uint8_t power_management_shooter_output : 1;
} ext_game_robot_state_t; } ext_game_robot_state_t;
/* ID: 0X0202 Byte: 16 实时功率热量数据 */ /* ID: 0X0202 Byte: 14 实时功率热量数据 */
typedef struct typedef struct
{ {
uint16_t chassis_voltage; uint16_t chassis_volt;
uint16_t chassis_current; uint16_t chassis_current;
float chassis_power; float chassis_power; // 瞬时功率
uint16_t buffer_energy; uint16_t chassis_power_buffer; // 60焦耳缓冲能量
uint16_t shooter_17mm_1_barrel_heat; uint16_t shooter_heat0; // 17mm
uint16_t shooter_17mm_2_barrel_heat; uint16_t shooter_heat1;
uint16_t shooter_42mm_barrel_heat;
} ext_power_heat_data_t; } ext_power_heat_data_t;
/* ID: 0x0203 Byte: 16 机器人位置数据 */ /* ID: 0x0203 Byte: 16 机器人位置数据 */
@ -198,22 +199,16 @@ typedef struct
float yaw; float yaw;
} ext_game_robot_pos_t; } ext_game_robot_pos_t;
/* ID: 0x0204 Byte: 6 机器人增益数据 */ /* ID: 0x0204 Byte: 1 机器人增益数据 */
typedef struct typedef struct
{ {
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; uint8_t power_rune_buff;
} ext_buff_musk_t; } ext_buff_musk_t;
/* ID: 0x0205 Byte: 2 空中机器人能量状态数据 */ /* ID: 0x0205 Byte: 1 空中机器人能量状态数据 */
typedef struct typedef struct
{ {
uint8_t airforce_status; uint8_t attack_time;
uint8_t time_remain;
} aerial_robot_energy_t; } aerial_robot_energy_t;
/* ID: 0x0206 Byte: 1 伤害状态数据 */ /* ID: 0x0206 Byte: 1 伤害状态数据 */
@ -232,20 +227,6 @@ typedef struct
float bullet_speed; float bullet_speed;
} ext_shoot_data_t; } 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个，也不会超*/ /* 发送的内容数据段最大为 113 检测是否超出大小限制?实际上图形段不会超，数据段最多30个，也不会超*/

329
modules/referee/referee_task.c
View File

@ -14,15 +14,10 @@
#include "referee_UI.h" #include "referee_UI.h"
#include "string.h" #include "string.h"
#include "cmsis_os.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_Interactive_info_t *Interactive_data; // UI绘制需要的机器人状态数据
static referee_info_t *referee_recv_info; // 接收到的裁判系统数据 static referee_info_t *referee_recv_info; // 接收到的裁判系统数据
Referee_Interactive_info_t ui_data;
uint8_t UI_Seq; // 包序号，供整个referee文件使用 uint8_t UI_Seq; // 包序号，供整个referee文件使用
// @todo 不应该使用全局变量 // @todo 不应该使用全局变量
/** /**
@ -33,7 +28,7 @@ uint8_t UI_Seq; // 包序号，供整个ref
*/ */
static void DeterminRobotID() static void DeterminRobotID()
{ {
// id小于7是红色,大于7是蓝色 #define Robot_Red 0 #define Robot_Blue 1 // id小于7是红色,大于7是蓝色,0为红色，1为蓝色 #define Robot_Red 0 #define Robot_Blue 1
referee_recv_info->referee_id.Robot_Color = referee_recv_info->GameRobotState.robot_id > 7 ? Robot_Blue : Robot_Red; referee_recv_info->referee_id.Robot_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.Robot_ID = referee_recv_info->GameRobotState.robot_id;
referee_recv_info->referee_id.Cilent_ID = 0x0100 + referee_recv_info->referee_id.Robot_ID; // 计算客户端ID referee_recv_info->referee_id.Cilent_ID = 0x0100 + referee_recv_info->referee_id.Robot_ID; // 计算客户端ID
@ -54,17 +49,15 @@ referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Int
void UITask() void UITask()
{ {
//RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常 RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
MyUIRefresh(referee_recv_info, Interactive_data); MyUIRefresh(referee_recv_info, Interactive_data);
} }
static Graph_Data_t UI_shoot_line[10]; // 射击准线 static Graph_Data_t UI_shoot_line[10]; // 射击准线
static Graph_Data_t UI_shoot_yuan[2]; // 射击圆心 static Graph_Data_t UI_Energy[3]; // 电容能量条
static Graph_Data_t UI_Energy[6]; // 电容能量条 static String_Data_t UI_State_sta[6]; // 机器人状态,静态只需画一次
static String_Data_t UI_State_sta[7]; // 机器人状态,静态只需画一次 static String_Data_t UI_State_dyn[6]; // 机器人状态,动态先add才能change
static String_Data_t UI_State_dyn[7]; // 机器人状态,动态先add才能change
static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565}; static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565};
static String_Data_t UI_prompt_sta[3]; // 操作手提示
void MyUIInit() void MyUIInit()
{ {
@ -76,79 +69,108 @@ void MyUIInit()
DeterminRobotID(); // 确定ui要发送到的目标客户端 DeterminRobotID(); // 确定ui要发送到的目标客户端
UIDelete(&referee_recv_info->referee_id, UI_Data_Del_ALL, 0); // 清空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[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[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[2], "sl2", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[2], 1110, shoot_line_location[2]);
UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Orange, 2, 960, 200, 960, 600); UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[3], 1110, shoot_line_location[3]);
UICircleDraw(&UI_shoot_yuan[0],"sy0",UI_Graph_ADD,7,UI_Color_Yellow,2,960,540,80); 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_yuan[0]); 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[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, 20, 2, 10, 750, "E.spin:"); UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 150, 750, "chassis:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[0]); 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:"); 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]); 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:"); 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]); 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:"); UICharDraw(&UI_State_sta[3], "ss3", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 600, "frict:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[3]); UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[3]);
UICharDraw(&UI_State_sta[4], "ss4", UI_Graph_ADD, 8, UI_Color_Main, 20, 2, 10, 650, "B.lid:"); UICharDraw(&UI_State_sta[4], "ss4", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 550, "lid:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[4]); 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无法刷新 // 由于初始化时xxx_last_mode默认为0，所以此处对应UI也应该设为0时对应的UI，防止模式不变的情况下无法置位flag，导致UI无法刷新
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 20, 2, 170, 750, "off"); UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]); 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"); 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]); 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"); 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]); 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"); UICharDraw(&UI_State_dyn[3], "sd3", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 600, "off");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[3]); UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[3]);
UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_ADD, 8, UI_Color_Pink, 20, 2, 170, 650, "off"); UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 550, "open ");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]); 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); 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]); UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[0]);
// 底盘功率显示,动态 // 底盘功率显示,动态
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); UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 2, 750, 230, 24000);
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[1]); // 能量条初始状态
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[2], "sd7", UI_Graph_ADD, 8, UI_Color_Pink, 18, 2,2, 850, 280,Interactive_data->Cap_Data.cap_vol); static uint8_t count = 0;
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[2]); static uint16_t count1 = 0;
// 电容能量条 static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测试用函数，实现模式自动变化
// 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]); count++;
if (count >= 50)
// 发弹量条框 {
UILineDraw(&UI_Energy[4], "ss7", UI_Graph_ADD, 7, UI_Color_Pink, 2, 1325, 500, 1480, 500); count = 0;
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[4]); 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;
}
} }
static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data) static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data)
@ -158,126 +180,77 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
if (_Interactive_data->Referee_Interactive_Flag.chassis_flag == 1) if (_Interactive_data->Referee_Interactive_Flag.chassis_flag == 1)
{ {
switch (_Interactive_data->chassis_mode) switch (_Interactive_data->chassis_mode)
// 此处注意字数对齐问题，字数相同才能覆盖掉
{ {
case CHASSIS_ZERO_FORCE: 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[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
UICharDraw(&UI_State_dyn[6], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 550, "off");
break; break;
case CHASSIS_ROTATE: 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[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "rotate ");
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; break;
case CHASSIS_NO_FOLLOW: 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[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "nofollow ");
UICharDraw(&UI_State_dyn[6], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 550, "off");
break; break;
case CHASSIS_FOLLOW_GIMBAL_YAW: 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[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow ");
UICharDraw(&UI_State_dyn[6], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 20, 2, 170, 550, "off");
break; 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; _Interactive_data->Referee_Interactive_Flag.chassis_flag = 0;
} }
// gimbal // gimbal
// if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1) if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1)
// { {
// switch (_Interactive_data->gimbal_mode) switch (_Interactive_data->gimbal_mode)
// { {
// case GIMBAL_ZERO_FORCE: case GIMBAL_ZERO_FORCE:
// { {
// UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce"); UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce");
// break; break;
// } }
// case GIMBAL_FREE_MODE: case GIMBAL_FREE_MODE:
// { {
// UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "free "); UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "free ");
// break; break;
// } }
// case GIMBAL_GYRO_MODE: case GIMBAL_GYRO_MODE:
// { {
// UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "gyro "); UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "gyro ");
// break; break;
// } }
// } }
// UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]); UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
// _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0; _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0;
// } }
// shoot // shoot
// if (_Interactive_data->Referee_Interactive_Flag.shoot_flag == 1) 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"); 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]); UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]);
// _Interactive_data->Referee_Interactive_Flag.shoot_flag = 0; _Interactive_data->Referee_Interactive_Flag.shoot_flag = 0;
// } }
// friction // friction
if (_Interactive_data->Referee_Interactive_Flag.friction_flag == 1) if (_Interactive_data->Referee_Interactive_Flag.friction_flag == 1)
{ {
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"); UICharDraw(&UI_State_dyn[3], "sd3", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 600, _Interactive_data->friction_mode == FRICTION_ON ? "on " : "off");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[3]); UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[3]);
_Interactive_data->Referee_Interactive_Flag.friction_flag = 0; _Interactive_data->Referee_Interactive_Flag.friction_flag = 0;
} }
// lid // lid
if (_Interactive_data->Referee_Interactive_Flag.lid_flag == 1) if (_Interactive_data->Referee_Interactive_Flag.lid_flag == 1)
{ {
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"); UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 550, _Interactive_data->lid_mode == LID_OPEN ? "open " : "close");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]); UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
_Interactive_data->Referee_Interactive_Flag.lid_flag = 0; _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 // power
if (_Interactive_data->Referee_Interactive_Flag.Power_flag == 1) if (_Interactive_data->Referee_Interactive_Flag.Power_flag == 1)
{ {
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); 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);
UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1]); UILineDraw(&UI_Energy[2], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)750 + _Interactive_data->Chassis_Power_Data.chassis_power_mx * 30, 160);
UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
_Interactive_data->Referee_Interactive_Flag.Power_flag = 0; _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;
}
} }
/** /**
@ -294,17 +267,17 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
_Interactive_data->chassis_last_mode = _Interactive_data->chassis_mode; _Interactive_data->chassis_last_mode = _Interactive_data->chassis_mode;
} }
// if (_Interactive_data->gimbal_mode != _Interactive_data->gimbal_last_mode) if (_Interactive_data->gimbal_mode != _Interactive_data->gimbal_last_mode)
// { {
// _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 1; _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 1;
// _Interactive_data->gimbal_last_mode = _Interactive_data->gimbal_mode; _Interactive_data->gimbal_last_mode = _Interactive_data->gimbal_mode;
// } }
// if (_Interactive_data->shoot_mode != _Interactive_data->shoot_last_mode) if (_Interactive_data->shoot_mode != _Interactive_data->shoot_last_mode)
// { {
// _Interactive_data->Referee_Interactive_Flag.shoot_flag = 1; _Interactive_data->Referee_Interactive_Flag.shoot_flag = 1;
// _Interactive_data->shoot_last_mode = _Interactive_data->shoot_mode; _Interactive_data->shoot_last_mode = _Interactive_data->shoot_mode;
// } }
if (_Interactive_data->friction_mode != _Interactive_data->friction_last_mode) if (_Interactive_data->friction_mode != _Interactive_data->friction_last_mode)
{ {
@ -318,33 +291,9 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
_Interactive_data->lid_last_mode = _Interactive_data->lid_mode; _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) 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->Referee_Interactive_Flag.Power_flag = 1;
_Interactive_data->Chassis_last_Power_Data.chassis_power_mx = _Interactive_data->Chassis_Power_Data.chassis_power_mx; _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;
// }
} }

1
modules/referee/referee_task.h
View File

@ -8,7 +8,6 @@
* @brief 初始化裁判系统交互任务(UI和多机通信) * @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); referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data);
/** /**

16
modules/referee/rm_referee.h
View File

@ -7,8 +7,6 @@
#include "bsp_usart.h" #include "bsp_usart.h"
#include "FreeRTOS.h" #include "FreeRTOS.h"
extern uint8_t UI_Seq; extern uint8_t UI_Seq;
#pragma pack(1) #pragma pack(1)
@ -56,10 +54,6 @@ typedef struct
uint32_t lid_flag : 1; uint32_t lid_flag : 1;
uint32_t friction_flag : 1; uint32_t friction_flag : 1;
uint32_t Power_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; } Referee_Interactive_Flag_t;
// 此结构体包含UI绘制与机器人车间通信的需要的其他非裁判系统数据 // 此结构体包含UI绘制与机器人车间通信的需要的其他非裁判系统数据
@ -69,14 +63,10 @@ typedef struct
// 为UI绘制以及交互数据所用 // 为UI绘制以及交互数据所用
chassis_mode_e chassis_mode; // 底盘模式 chassis_mode_e chassis_mode; // 底盘模式
gimbal_mode_e gimbal_mode; // 云台模式 gimbal_mode_e gimbal_mode; // 云台模式
shoot_mode_e shoot_mode; // 发射开关设置 shoot_mode_e shoot_mode; // 发射模式设置
friction_mode_e friction_mode; // 摩擦轮关闭 friction_mode_e friction_mode; // 摩擦轮关闭
lid_mode_e lid_mode; // 弹舱盖打开 lid_mode_e lid_mode; // 弹舱盖打开
Chassis_Power_Data_s Chassis_Power_Data; // 功率控制 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判断 // 上一次的模式，用于flag判断
chassis_mode_e chassis_last_mode; chassis_mode_e chassis_last_mode;
@ -85,10 +75,6 @@ typedef struct
friction_mode_e friction_last_mode; friction_mode_e friction_last_mode;
lid_mode_e lid_last_mode; lid_mode_e lid_last_mode;
Chassis_Power_Data_s Chassis_last_Power_Data; 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; } Referee_Interactive_info_t;

162
modules/referee/vision_transfer.c
View File

@ -1,162 +0,0 @@
/**
* @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);
}

46
modules/referee/vision_transfer.h
View File

@ -1,46 +0,0 @@
#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

10
modules/remote/remote_control.c
View File

@ -16,10 +16,6 @@ static uint8_t rc_init_flag = 0; // 遥控器初始化标志位
static USARTInstance *rc_usart_instance; static USARTInstance *rc_usart_instance;
static DaemonInstance *rc_daemon_instance; static DaemonInstance *rc_daemon_instance;
// 图传拥有的串口实例
static USARTInstance *vt_usart_instance;
static DaemonInstance *vt_daemon_instance;
/** /**
* @brief 矫正遥控器摇杆的值,超过660或者小于-660的值都认为是无效值,置0 * @brief 矫正遥控器摇杆的值,超过660或者小于-660的值都认为是无效值,置0
* *
@ -101,12 +97,6 @@ static void RemoteControlRxCallback()
sbus_to_rc(rc_usart_instance->recv_buff); // 进行协议解析 sbus_to_rc(rc_usart_instance->recv_buff); // 进行协议解析
} }
//static void RemoteControlvtCallback()
//{
// DaemonReload(vt_daemon_instance); // 先喂狗
// sbus_to_rc(vt_usart_instance->recv_buff); // 进行协议解析
//}
/** /**
* @brief 遥控器离线的回调函数,注册到守护进程中,串口掉线时调用 * @brief 遥控器离线的回调函数,注册到守护进程中,串口掉线时调用
* *

16
modules/remote/remote_control.h
View File

@ -113,20 +113,6 @@ typedef struct
uint8_t key_count[3][16]; uint8_t key_count[3][16];
} RC_ctrl_t; } 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 ----------------------------------- */ /* ------------------------- Internal Data ----------------------------------- */
/** /**
@ -137,8 +123,6 @@ typedef struct
*/ */
RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle); RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle);
//VT_ctrl_t *RemoteControlInit(UART_HandleTypeDef *vt_usart_handle);
/** /**
* @brief 检查遥控器是否在线,若尚未初始化也视为离线 * @brief 检查遥控器是否在线,若尚未初始化也视为离线
* *

24
modules/robotics/matrix.cpp
View File

@ -1,24 +0,0 @@
/**
******************************************************************************
* @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;
}

259
modules/robotics/matrix.h
View File

@ -1,259 +0,0 @@
/**
******************************************************************************
* @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 <int _rows, int _cols>
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 <int cols>
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 <int rows, int cols>
Matrixf<rows, cols> block(const int& start_row, const int& start_col) {
Matrixf<rows, cols> 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 <int _rows, int _cols>
Matrixf<_rows, _cols> zeros(void) {
float data[_rows * _cols] = {0};
return Matrixf<_rows, _cols>(data);
}
// Ones matrix
template <int _rows, int _cols>
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 <int _rows, int _cols>
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 <int _rows, int _cols>
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 <int _dim>
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

340
modules/robotics/robotics.cpp
View File

@ -1,340 +0,0 @@
/**
******************************************************************************
* @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();
}

407
modules/robotics/robotics.h
View File

@ -1,407 +0,0 @@
/**
******************************************************************************
* @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 <uint16_t _n = 1>
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

56
modules/robotics/utils.cpp
View File

@ -1,56 +0,0 @@
/**
******************************************************************************
* @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;
}

27
modules/robotics/utils.h
View File

@ -1,27 +0,0 @@
/**
******************************************************************************
* @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 <stdint.h>
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

26
modules/super_cap/super_cap.c
View File

@ -1,8 +1,14 @@
/*
* @Descripttion:
* @version:
* @Author: Chenfu
* @Date: 2022-12-02 21:32:47
* @LastEditTime: 2022-12-05 15:29:49
*/
#include "super_cap.h" #include "super_cap.h"
#include "memory.h" #include "memory.h"
#include "stdlib.h" #include "stdlib.h"
static SuperCapInstance *super_cap_instance = NULL; // 可以由app保存此指针 static SuperCapInstance *super_cap_instance = NULL; // 可以由app保存此指针
static void SuperCapRxCallback(CANInstance *_instance) static void SuperCapRxCallback(CANInstance *_instance)
@ -11,10 +17,9 @@ static void SuperCapRxCallback(CANInstance *_instance)
SuperCap_Msg_s *Msg; SuperCap_Msg_s *Msg;
rxbuff = _instance->rx_buff; rxbuff = _instance->rx_buff;
Msg = &super_cap_instance->cap_msg; Msg = &super_cap_instance->cap_msg;
Msg->input_vol = (int16_t)(rxbuff[1] << 8 | rxbuff[0]); Msg->vol = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]);
Msg->cap_vol = (int16_t)(rxbuff[3] << 8 | rxbuff[2]); Msg->current = (uint16_t)(rxbuff[2] << 8 | rxbuff[3]);
Msg->input_cur = (int16_t)(rxbuff[5] << 8 | rxbuff[4]); Msg->power = (uint16_t)(rxbuff[4] << 8 | rxbuff[5]);
Msg->power_set = (int16_t)(rxbuff[7] << 8 | rxbuff[6]);
} }
SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config) SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config)
@ -24,8 +29,6 @@ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config)
supercap_config->can_config.can_module_callback = SuperCapRxCallback; supercap_config->can_config.can_module_callback = SuperCapRxCallback;
super_cap_instance->can_ins = CANRegister(&supercap_config->can_config); 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; return super_cap_instance;
} }
@ -35,15 +38,6 @@ void SuperCapSend(SuperCapInstance *instance, uint8_t *data)
CANTransmit(instance->can_ins,1); 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) SuperCap_Msg_s SuperCapGet(SuperCapInstance *instance)
{ {
return instance->cap_msg; return instance->cap_msg;

25
modules/super_cap/super_cap.h
View File

@ -1,16 +1,21 @@
/*
* @Descripttion:
* @version:
* @Author: Chenfu
* @Date: 2022-12-02 21:32:47
* @LastEditTime: 2022-12-05 15:25:46
*/
#ifndef SUPER_CAP_H #ifndef SUPER_CAP_H
#define SUPER_CAP_H #define SUPER_CAP_H
#include "bsp_can.h" #include "bsp_can.h"
#include "controller.h"
#pragma pack(1) #pragma pack(1)
typedef struct typedef struct
{ {
int16_t input_vol; // 输入电压 uint16_t vol; // 电压
int16_t cap_vol; // 电容电压 uint16_t current; // 电流
int16_t input_cur; // 输入电流 uint16_t power; // 功率
int16_t power_set; // 设定功率
} SuperCap_Msg_s; } SuperCap_Msg_s;
#pragma pack() #pragma pack()
@ -19,14 +24,12 @@ typedef struct
{ {
CANInstance *can_ins; // CAN实例 CANInstance *can_ins; // CAN实例
SuperCap_Msg_s cap_msg; // 超级电容信息 SuperCap_Msg_s cap_msg; // 超级电容信息
PIDInstance buffer_pid; // 对缓冲功率进行闭环
} SuperCapInstance; } SuperCapInstance;
/* 超级电容初始化配置 */ /* 超级电容初始化配置 */
typedef struct typedef struct
{ {
CAN_Init_Config_s can_config; CAN_Init_Config_s can_config;
PID_Init_Config_s buffer_config_pid;
} SuperCap_Init_Config_s; } SuperCap_Init_Config_s;
/** /**
@ -45,12 +48,4 @@ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config);
*/ */
void SuperCapSend(SuperCapInstance *instance, uint8_t *data); void SuperCapSend(SuperCapInstance *instance, uint8_t *data);
/**
* @brief 发送超级电容目标功率
*
* @param instance 超级电容实例
* @param power_set 超级电容目标功率
*/
void SuperCapSetPower(SuperCapInstance *instance, float power_set);
#endif // !SUPER_CAP_Hd #endif // !SUPER_CAP_Hd

61
modules/vofa/vofa.c
View File

@ -6,13 +6,6 @@
* @LastEditTime: 2022-12-05 14:15:53 * @LastEditTime: 2022-12-05 14:15:53
*/ */
#include "vofa.h" #include "vofa.h"
#include "usbd_cdc_if.h"
typedef union
{
float float_t;
uint8_t uint8_t[4];
} send_float;
/*VOFA浮点协议*/ /*VOFA浮点协议*/
void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart ) void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart )
@ -36,57 +29,5 @@ void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart
send_data[4 * num + 2] = 0x80; send_data[4 * num + 2] = 0x80;
send_data[4 * num + 3] = 0x7f; //加上协议要求的4个尾巴 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<DataSendBuf[3]+4;i++)
{
sc+=DataSendBuf[i];
ac+=sc;
}
DataSendBuf[cnt++]=sc;
DataSendBuf[cnt++]=ac;
// for(uint8_t i=0;i<cnt;i++)
// HAL_UART_Transmit(&huart6,&DataSendBuf[i],1,100);
CDC_Transmit_FS(DataSendBuf, cnt);
} }

8
modules/vofa/vofa.h
View File

@ -11,7 +11,11 @@
#include "bsp_usart.h" #include "bsp_usart.h"
#include "usart.h" #include "usart.h"
typedef union
{
float float_t;
uint8_t uint8_t[4];
} send_float;
void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart); void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart);
void ANODT_SendF1(int32_t Angle,int32_t speed_rpm,int32_t Angle_target,int32_t speed_target);
#endif // !1#define #endif // !1#define