新增图传链路
This commit is contained in:
parent
ceec256ff1
commit
59d43f4c7b
|
@ -53,7 +53,7 @@ include_directories(Inc Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_D
|
||||||
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/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/power_meter
|
modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa modules/power_meter modules/auto_aim
|
||||||
modules/motor/DJImotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor modules/motor/ECmotor
|
modules/motor/DJImotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor modules/motor/ECmotor
|
||||||
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)
|
||||||
|
|
|
@ -43,9 +43,6 @@ static Subscriber_t *chassis_sub; // 用于订阅底盘的控
|
||||||
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 referee_info_t* referee_data; // 用于获取裁判系统的数据
|
|
||||||
static Referee_Interactive_info_t ui_data; // UI数据,将底盘中的数据传入此结构体的对应变量中,UI会自动检测是否变化,对应显示UI
|
|
||||||
|
|
||||||
static SuperCapInstance *cap; // 超级电容
|
static SuperCapInstance *cap; // 超级电容
|
||||||
static PowerMeterInstance *power_meter; //功率计
|
static PowerMeterInstance *power_meter; //功率计
|
||||||
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
|
||||||
|
@ -110,7 +107,7 @@ void ChassisInit()
|
||||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
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 = {
|
||||||
|
|
|
@ -8,6 +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 "referee_task.h"
|
||||||
|
#include "referee_UI.h"
|
||||||
// bsp
|
// bsp
|
||||||
#include "bsp_dwt.h"
|
#include "bsp_dwt.h"
|
||||||
#include "bsp_log.h"
|
#include "bsp_log.h"
|
||||||
|
@ -19,6 +22,8 @@
|
||||||
/* 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
|
||||||
|
@ -30,8 +35,15 @@ static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信
|
||||||
static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
|
static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
|
||||||
|
|
||||||
static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回
|
static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回
|
||||||
static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回
|
static VT_ctrl_t vt_data[2]; // 图传链路下发的键鼠遥控数据 与遥控器数据格式保持一致
|
||||||
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 Publisher_t *gimbal_cmd_pub; // 云台控制消息发布者
|
static Publisher_t *gimbal_cmd_pub; // 云台控制消息发布者
|
||||||
static Subscriber_t *gimbal_feed_sub; // 云台反馈信息订阅者
|
static Subscriber_t *gimbal_feed_sub; // 云台反馈信息订阅者
|
||||||
|
@ -45,11 +57,16 @@ 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; // 用于获取裁判系统的数据
|
||||||
|
static Referee_Interactive_info_t ui_data; // UI数据,将底盘中的数据传入此结构体的对应变量中,UI会自动检测是否变化,对应显示UI
|
||||||
|
|
||||||
void RobotCMDInit()
|
void RobotCMDInit()
|
||||||
{
|
{
|
||||||
rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
|
rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
|
||||||
vision_recv_data = VisionInit(&huart1); // 视觉通信串口
|
vision_recv_data = VisionInit(&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));
|
||||||
shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
|
shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
|
||||||
|
@ -73,7 +90,7 @@ void RobotCMDInit()
|
||||||
#endif // GIMBAL_BOARD
|
#endif // GIMBAL_BOARD
|
||||||
gimbal_cmd_send.pitch = 0;
|
gimbal_cmd_send.pitch = 0;
|
||||||
|
|
||||||
robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
|
robot_state = ROBOT_STOP; // 轮腿 上电后进入ROBOT_STOP 保证安全(对应倒地状态)
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -81,10 +98,12 @@ void RobotCMDInit()
|
||||||
* 单圈绝对角度的范围是0~360,说明文档中有图示
|
* 单圈绝对角度的范围是0~360,说明文档中有图示
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
|
|
||||||
static void CalcOffsetAngle()
|
static void CalcOffsetAngle()
|
||||||
{
|
{
|
||||||
// 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
|
// 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
|
||||||
static float angle;
|
static float angle;
|
||||||
|
static float offset_angle;
|
||||||
angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
|
angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
|
||||||
#if YAW_ECD_GREATER_THAN_4096 // 如果大于180度
|
#if YAW_ECD_GREATER_THAN_4096 // 如果大于180度
|
||||||
if (angle > YAW_ALIGN_ANGLE && angle <= 180.0f + YAW_ALIGN_ANGLE)
|
if (angle > YAW_ALIGN_ANGLE && angle <= 180.0f + YAW_ALIGN_ANGLE)
|
||||||
|
@ -110,36 +129,76 @@ static void CalcOffsetAngle()
|
||||||
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_NO_FOLLOW;
|
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||||
gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
|
gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 云台参数,确定云台控制数据
|
// 云台参数,确定云台控制数据
|
||||||
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的增量进行控制
|
||||||
// ...
|
// ...
|
||||||
|
float yaw_add = -0.0005f * (float) rc_data[TEMP].rc.rocker_l_;
|
||||||
|
float pitch_add = -0.0003f * (float) rc_data[TEMP].rc.rocker_l1;
|
||||||
|
|
||||||
|
|
||||||
|
gimbal_cmd_send.yaw += yaw_add;
|
||||||
|
gimbal_cmd_send.pitch += pitch_add;
|
||||||
|
|
||||||
|
// 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)) {
|
||||||
|
trajectory_cal.v0 = 30; //弹速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;
|
||||||
|
|
||||||
|
auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
|
||||||
|
|
||||||
|
gimbal_cmd_send.yaw = trajectory_cal.cmd_yaw * 180 / PI;
|
||||||
|
|
||||||
|
gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI;
|
||||||
|
|
||||||
|
float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw);
|
||||||
|
if (yaw_err <= 0.008) //3度
|
||||||
|
aim_select.suggest_fire = 1;
|
||||||
|
else
|
||||||
|
aim_select.suggest_fire = 0;
|
||||||
}
|
}
|
||||||
// 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制
|
|
||||||
if (switch_is_down(rc_data[TEMP].rc.switch_left) || vision_recv_data->target_state == NO_TARGET)
|
|
||||||
{ // 按照摇杆的输出大小进行角度增量,增益系数需调整
|
|
||||||
gimbal_cmd_send.yaw += 0.0002f * (float)rc_data[TEMP].rc.rocker_l_;
|
|
||||||
gimbal_cmd_send.pitch += 0.0005f * (float)rc_data[TEMP].rc.rocker_l1;
|
|
||||||
}
|
}
|
||||||
// 云台软件限位
|
// 云台软件限位
|
||||||
|
|
||||||
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
|
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
|
||||||
chassis_cmd_send.vx = 8.0f * (float)rc_data[TEMP].rc.rocker_r1; // _水平方向
|
chassis_cmd_send.vx = 8.0f * (float)rc_data[TEMP].rc.rocker_r1; // _水平方向
|
||||||
chassis_cmd_send.vy = 8.0f * (float)rc_data[TEMP].rc.rocker_r_; // 1数值方向
|
chassis_cmd_send.vy = 8.0f * (float)rc_data[TEMP].rc.rocker_r_; // 1数值方向
|
||||||
|
//chassis_cmd_send.wz = 8.0f * (float)rc_data[TEMP].rc.rocker_r_; // 1数值方向
|
||||||
chassis_cmd_send.wz = 80.0f * (float)rc_data[TEMP].rc.rocker_l_; // 1数值方向
|
|
||||||
|
|
||||||
// 发射参数
|
// 发射参数
|
||||||
if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
|
if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
|
||||||
|
@ -248,6 +307,84 @@ static void MouseKeySet()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 输入为(图传链路)键鼠时模式和控制量设置
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
static void VTMouseKeySet()
|
||||||
|
{
|
||||||
|
// 鼠标解析
|
||||||
|
vt_data[TEMP].mouse.x = referee_data->RemoteControl.mouse_x; //!< Mouse X axis
|
||||||
|
vt_data[TEMP].mouse.y = referee_data->RemoteControl.mouse_y; //!< Mouse Y axis
|
||||||
|
vt_data[TEMP].mouse.press_l = referee_data->RemoteControl.left_button_down; //!< Mouse Left Is Press ?
|
||||||
|
vt_data[TEMP].mouse.press_r = referee_data->RemoteControl.right_button_down; //!< Mouse Right Is Press ?
|
||||||
|
|
||||||
|
// 位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位,msb在最后
|
||||||
|
*(uint16_t *)&vt_data[TEMP].key[KEY_PRESS] = referee_data->RemoteControl.keyboard_value;
|
||||||
|
|
||||||
|
if (vt_data[TEMP].key[KEY_PRESS].ctrl) // ctrl键按下
|
||||||
|
vt_data[TEMP].key[KEY_PRESS_WITH_CTRL] = vt_data[TEMP].key[KEY_PRESS];
|
||||||
|
else
|
||||||
|
memset(&vt_data[TEMP].key[KEY_PRESS_WITH_CTRL], 0, sizeof(Key_t));
|
||||||
|
if (vt_data[TEMP].key[KEY_PRESS].shift) // shift键按下
|
||||||
|
vt_data[TEMP].key[KEY_PRESS_WITH_SHIFT] = vt_data[TEMP].key[KEY_PRESS];
|
||||||
|
else
|
||||||
|
memset(&vt_data[TEMP].key[KEY_PRESS_WITH_SHIFT], 0, sizeof(Key_t));
|
||||||
|
|
||||||
|
uint16_t key_now = vt_data[TEMP].key[KEY_PRESS].keys, // 当前按键是否按下
|
||||||
|
key_last = vt_data[LAST].key[KEY_PRESS].keys, // 上一次按键是否按下
|
||||||
|
key_with_ctrl = vt_data[TEMP].key[KEY_PRESS_WITH_CTRL].keys, // 当前ctrl组合键是否按下
|
||||||
|
key_with_shift = vt_data[TEMP].key[KEY_PRESS_WITH_SHIFT].keys, // 当前shift组合键是否按下
|
||||||
|
key_last_with_ctrl = vt_data[LAST].key[KEY_PRESS_WITH_CTRL].keys, // 上一次ctrl组合键是否按下
|
||||||
|
key_last_with_shift = vt_data[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_data[TEMP].key_count[KEY_PRESS][i]++;
|
||||||
|
// 当前ctrl组合键按下,上一次ctrl组合键没有按下,则ctrl组合键按下计数加1(检测到上升沿)
|
||||||
|
if ((key_with_ctrl & j) && !(key_last_with_ctrl & j))
|
||||||
|
vt_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][i]++;
|
||||||
|
// 当前shift组合键按下,上一次shift组合键没有按下,则shift组合键按下计数加1(检测到上升沿)
|
||||||
|
if ((key_with_shift & j) && !(key_last_with_shift & j))
|
||||||
|
vt_data[TEMP].key_count[KEY_PRESS_WITH_SHIFT][i]++;
|
||||||
|
}
|
||||||
|
|
||||||
|
memcpy(&vt_data[LAST], &vt_data[TEMP], sizeof(VT_ctrl_t)); // 保存上一次的数据,用于按键持续按下和切换的判断
|
||||||
|
|
||||||
|
chassis_cmd_send.vx = vt_data[TEMP].key[KEY_PRESS].w * 1500 - vt_data[TEMP].key[KEY_PRESS].s * 1500; // 系数待测
|
||||||
|
//chassis_cmd_send.vy = vt_data[TEMP].key[KEY_PRESS].s * 800 - vt_data[TEMP].key[KEY_PRESS].d * 800;
|
||||||
|
|
||||||
|
gimbal_cmd_send.yaw -= (float)vt_data[TEMP].mouse.x / 660 * 5; // 系数待测
|
||||||
|
gimbal_cmd_send.pitch += (float)vt_data[TEMP].mouse.y / 660 * 10;
|
||||||
|
|
||||||
|
|
||||||
|
switch (vt_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮
|
||||||
|
{
|
||||||
|
case 0:
|
||||||
|
shoot_cmd_send.friction_mode = FRICTION_OFF;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
shoot_cmd_send.friction_mode = FRICTION_ON;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
switch (vt_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键侧身
|
||||||
|
{
|
||||||
|
case 0:
|
||||||
|
//chassis_cmd_send.offset_angle = CalcOffsetAngle();
|
||||||
|
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
//chassis_cmd_send.offset_angle = loop_float_constrain(CalcOffsetAngle() + 90,-180,180);
|
||||||
|
chassis_cmd_send.chassis_mode = CHASSIS_LATERAL;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等
|
* @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等
|
||||||
* 停止的阈值'300'待修改成合适的值,或改为开关控制.
|
* 停止的阈值'300'待修改成合适的值,或改为开关控制.
|
||||||
|
@ -294,9 +431,17 @@ void RobotCMDTask()
|
||||||
CalcOffsetAngle();
|
CalcOffsetAngle();
|
||||||
// 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
|
// 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
|
||||||
if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
|
if (switch_is_down(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)) // 遥控器左侧开关状态为[上],键盘控制
|
||||||
|
{
|
||||||
MouseKeySet();
|
MouseKeySet();
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
|
||||||
|
VTMouseKeySet();
|
||||||
|
|
||||||
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
|
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
|
||||||
|
|
||||||
|
|
|
@ -1,5 +1,20 @@
|
||||||
#ifndef ROBOT_CMD_H
|
#ifndef ROBOT_CMD_H
|
||||||
#define ROBOT_CMD_H
|
#define ROBOT_CMD_H
|
||||||
|
#include "remote_control.h"
|
||||||
|
|
||||||
|
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; //图传链路下发的遥控数据
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
|
@ -10,6 +10,7 @@
|
||||||
#include "vofa.h"
|
#include "vofa.h"
|
||||||
|
|
||||||
#include "power_meter.h"
|
#include "power_meter.h"
|
||||||
|
#include "referee_task.h"
|
||||||
|
|
||||||
|
|
||||||
static attitude_t *gimba_IMU_data; // 云台IMU数据
|
static attitude_t *gimba_IMU_data; // 云台IMU数据
|
||||||
|
@ -23,11 +24,12 @@ 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 current_feedforward = 3000.0f;
|
static float current_feedforward = 0.0f;//3000.0f;
|
||||||
|
static float GRAVITY_FEED = 0; //重力补偿前馈力矩系数
|
||||||
sin_input_generate_t sinInputGenerate;
|
sin_input_generate_t sinInputGenerate;
|
||||||
void GimbalInit()
|
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 = {
|
||||||
|
@ -77,7 +79,7 @@ void GimbalInit()
|
||||||
},
|
},
|
||||||
.controller_param_init_config = {
|
.controller_param_init_config = {
|
||||||
.angle_PID = {
|
.angle_PID = {
|
||||||
.Kp = 1.3f, // 10
|
.Kp = 0.8f, // 10
|
||||||
.Ki = 0,
|
.Ki = 0,
|
||||||
.Kd = 0.0f,
|
.Kd = 0.0f,
|
||||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||||
|
@ -85,8 +87,8 @@ void GimbalInit()
|
||||||
.MaxOut = 500,
|
.MaxOut = 500,
|
||||||
},
|
},
|
||||||
.speed_PID = {
|
.speed_PID = {
|
||||||
.Kp = -2500, // 50
|
.Kp = 5000.0f,//2500, // 50
|
||||||
.Ki = -200, // 350
|
.Ki = 2000.0f,//200, // 350
|
||||||
.Kd = 0, // 0
|
.Kd = 0, // 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 = 10000,
|
||||||
|
@ -103,7 +105,7 @@ void GimbalInit()
|
||||||
.outer_loop_type = ANGLE_LOOP,
|
.outer_loop_type = ANGLE_LOOP,
|
||||||
.close_loop_type = ANGLE_LOOP|SPEED_LOOP|CURRENT_LOOP,
|
.close_loop_type = ANGLE_LOOP|SPEED_LOOP|CURRENT_LOOP,
|
||||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||||
//.feedforward_flag = CURRENT_FEEDFORWARD,
|
.feedforward_flag = CURRENT_FEEDFORWARD,
|
||||||
},
|
},
|
||||||
.motor_type = GM6020,
|
.motor_type = GM6020,
|
||||||
.motor_control_type = CURRENT_CONTROL,//CURRENT_CONTROL
|
.motor_control_type = CURRENT_CONTROL,//CURRENT_CONTROL
|
||||||
|
@ -127,6 +129,17 @@ void GimbalTask()
|
||||||
// 后续增加未收到数据的处理
|
// 后续增加未收到数据的处理
|
||||||
SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
|
SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
|
||||||
|
|
||||||
|
// if(gimbal_cmd_recv.pitch_motor_ecd + (pitch_add) >= PITCH_MAX_ECD)
|
||||||
|
// {
|
||||||
|
// if(pitch_add > 0) gimbal_cmd_send.pitch = PITCH_MAX_ECD;
|
||||||
|
// // pitch_add = PITCH_MAX_ECD - gimbal_fetch_data.pitch_motor_ecd;
|
||||||
|
// }
|
||||||
|
// else if(gimbal_cmd_recv.pitch_motor_ecd + (pitch_add) <= PITCH_MIN_ECD)
|
||||||
|
// {
|
||||||
|
// if(pitch_add < 0) gimbal_cmd_send.pitch = PITCH_MIN_ECD;
|
||||||
|
// // pitch_add = PITCH_MIN_ECD - gimbal_fetch_data.pitch_motor_ecd;
|
||||||
|
// }
|
||||||
|
|
||||||
// @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)
|
||||||
|
@ -170,9 +183,10 @@ void GimbalTask()
|
||||||
|
|
||||||
//ANODT_SendF1(input*1000,pitch_motor->measure.speed_aps*1000,0,0);
|
//ANODT_SendF1(input*1000,pitch_motor->measure.speed_aps*1000,0,0);
|
||||||
|
|
||||||
float theta = pitch_motor->measure.angle_single_round - 6200 * ECD_ANGLE_COEF_DJI;
|
float theta = gimba_IMU_data->Pitch * DEGREE_2_RAD;
|
||||||
|
|
||||||
|
current_feedforward = GRAVITY_FEED * arm_cos_f32(theta);
|
||||||
|
|
||||||
float gravity_feed = 3800*arm_cos_f32(theta/180*PI);
|
|
||||||
float vofa_send_data[6];
|
float vofa_send_data[6];
|
||||||
vofa_send_data[0] = pitch_motor->measure.speed_aps;
|
vofa_send_data[0] = pitch_motor->measure.speed_aps;
|
||||||
vofa_send_data[1] = gimba_IMU_data->Pitch;
|
vofa_send_data[1] = gimba_IMU_data->Pitch;
|
||||||
|
@ -187,6 +201,8 @@ void GimbalTask()
|
||||||
// 设置反馈数据,主要是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;
|
||||||
|
gimbal_feedback_data.pitch_motor_ecd = pitch_motor->measure.angle_single_round + pitch_motor->motor_controller.angle_PID.Err;
|
||||||
|
|
||||||
|
|
||||||
// 推送消息
|
// 推送消息
|
||||||
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
|
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
|
||||||
|
|
|
@ -26,11 +26,14 @@
|
||||||
|
|
||||||
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
|
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
|
||||||
// 云台参数
|
// 云台参数
|
||||||
#define YAW_CHASSIS_ALIGN_ECD 8012 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
|
#define YAW_CHASSIS_ALIGN_ECD 5046 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
|
||||||
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
|
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
|
||||||
#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
|
#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
|
||||||
#define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪,则填写陀螺仪的角度)
|
#define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪,则填写陀螺仪的角度)
|
||||||
#define PITCH_MIN_ANGLE 0 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪,则填写陀螺仪的角度)
|
#define PITCH_MIN_ANGLE 0 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪,则填写陀螺仪的角度)
|
||||||
|
|
||||||
|
#define PITCH_MAX_ECD 5100 * ECD_ANGLE_COEF_DJI
|
||||||
|
#define PITCH_MIN_ECD 3900 * ECD_ANGLE_COEF_DJI
|
||||||
// 发射参数
|
// 发射参数
|
||||||
#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
|
#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
|
||||||
#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
|
#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
|
||||||
|
@ -86,6 +89,7 @@ typedef enum
|
||||||
CHASSIS_ROTATE, // 小陀螺模式
|
CHASSIS_ROTATE, // 小陀螺模式
|
||||||
CHASSIS_NO_FOLLOW, // 不跟随,允许全向平移
|
CHASSIS_NO_FOLLOW, // 不跟随,允许全向平移
|
||||||
CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式,底盘叠加角度环控制
|
CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式,底盘叠加角度环控制
|
||||||
|
CHASSIS_LATERAL, // 侧向对敌
|
||||||
} chassis_mode_e;
|
} chassis_mode_e;
|
||||||
|
|
||||||
// 云台模式设置
|
// 云台模式设置
|
||||||
|
@ -198,6 +202,7 @@ typedef struct
|
||||||
{
|
{
|
||||||
attitude_t gimbal_imu_data;
|
attitude_t gimbal_imu_data;
|
||||||
uint16_t yaw_motor_single_round_angle;
|
uint16_t yaw_motor_single_round_angle;
|
||||||
|
float pitch_motor_ecd;
|
||||||
} Gimbal_Upload_Data_s;
|
} Gimbal_Upload_Data_s;
|
||||||
|
|
||||||
typedef struct
|
typedef struct
|
||||||
|
|
|
@ -52,7 +52,7 @@ void ShootInit()
|
||||||
.motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
|
.motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
|
||||||
},
|
},
|
||||||
.motor_type = M3508};
|
.motor_type = M3508};
|
||||||
friction_config.can_init_config.tx_id = 3,
|
friction_config.can_init_config.tx_id = 1,
|
||||||
friction_l = DJIMotorInit(&friction_config);
|
friction_l = DJIMotorInit(&friction_config);
|
||||||
|
|
||||||
friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
|
friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
|
||||||
|
@ -63,7 +63,7 @@ void ShootInit()
|
||||||
Motor_Init_Config_s loader_config = {
|
Motor_Init_Config_s loader_config = {
|
||||||
.can_init_config = {
|
.can_init_config = {
|
||||||
.can_handle = &hcan2,
|
.can_handle = &hcan2,
|
||||||
.tx_id = 1
|
.tx_id = 3
|
||||||
},
|
},
|
||||||
.controller_param_init_config = {
|
.controller_param_init_config = {
|
||||||
.angle_PID = {
|
.angle_PID = {
|
||||||
|
@ -110,22 +110,26 @@ void stalled_detect()
|
||||||
static float last_total_angle = 0;
|
static float last_total_angle = 0;
|
||||||
static uint8_t stalled_cnt = 0;
|
static uint8_t stalled_cnt = 0;
|
||||||
|
|
||||||
last_detect_time = detect_time;
|
|
||||||
detect_time = DWT_GetTimeline_ms();
|
detect_time = DWT_GetTimeline_ms();
|
||||||
|
|
||||||
|
//last_detect_time + 200 > detect_time
|
||||||
if(detect_time - last_detect_time < 200) // 200ms 检测一次
|
if(detect_time - last_detect_time < 200) // 200ms 检测一次
|
||||||
return;
|
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.load_mode == LOAD_BURSTFIRE)
|
if(shoot_cmd_recv.load_mode == LOAD_BURSTFIRE)
|
||||||
{
|
{
|
||||||
float reference_angle = (detect_time - last_detect_time) * loader->motor_controller.pid_ref * 1e3f;
|
//if(real_angle < reference_angle * 0.2f)
|
||||||
float real_angle = loader->measure.total_angle - last_total_angle;
|
if(abs(loader->measure.real_current)>=9500)
|
||||||
if(real_angle < reference_angle * 0.2f)
|
|
||||||
{
|
{
|
||||||
//stalled_cnt ++;
|
//stalled_cnt ++;
|
||||||
shoot_feedback_data.stalled_flag = 1;
|
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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -179,7 +183,7 @@ void ShootTask()
|
||||||
// 连发模式,对速度闭环,射频后续修改为可变,目前固定为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, 15* 360 * REDUCTION_RATIO_LOADER / 8);//shoot_cmd_recv.shoot_rate
|
||||||
|
|
||||||
// x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
|
// x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
|
||||||
break;
|
break;
|
||||||
|
@ -187,9 +191,9 @@ void ShootTask()
|
||||||
// 也有可能需要从switch-case中独立出来
|
// 也有可能需要从switch-case中独立出来
|
||||||
case LOAD_REVERSE:
|
case LOAD_REVERSE:
|
||||||
DJIMotorOuterLoop(loader, SPEED_LOOP);
|
DJIMotorOuterLoop(loader, SPEED_LOOP);
|
||||||
DJIMotorSetRef(loader, 8 * 360 * REDUCTION_RATIO_LOADER / 8); // 固定速度反转
|
DJIMotorSetRef(loader, -8 * 360 * REDUCTION_RATIO_LOADER / 8); // 固定速度反转
|
||||||
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
|
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
|
||||||
dead_time = 500; // 翻转500ms
|
dead_time = 1000; // 翻转500ms
|
||||||
shoot_feedback_data.stalled_flag = 0 ; //清除堵转标志
|
shoot_feedback_data.stalled_flag = 0 ; //清除堵转标志
|
||||||
// ...
|
// ...
|
||||||
break;
|
break;
|
||||||
|
@ -204,18 +208,18 @@ void ShootTask()
|
||||||
// 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
|
// 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
|
||||||
switch (shoot_cmd_recv.bullet_speed)
|
switch (shoot_cmd_recv.bullet_speed)
|
||||||
{
|
{
|
||||||
case SMALL_AMU_15:
|
// case SMALL_AMU_15:
|
||||||
DJIMotorSetRef(friction_l, 0);
|
// DJIMotorSetRef(friction_l, 0);
|
||||||
DJIMotorSetRef(friction_r, 0);
|
// DJIMotorSetRef(friction_r, 0);
|
||||||
break;
|
// break;
|
||||||
case SMALL_AMU_18:
|
// case SMALL_AMU_18:
|
||||||
DJIMotorSetRef(friction_l, 0);
|
// DJIMotorSetRef(friction_l, 0);
|
||||||
DJIMotorSetRef(friction_r, 0);
|
// DJIMotorSetRef(friction_r, 0);
|
||||||
break;
|
// break;
|
||||||
case SMALL_AMU_30:
|
// case SMALL_AMU_30:
|
||||||
DJIMotorSetRef(friction_l, 0);
|
// DJIMotorSetRef(friction_l, 0);
|
||||||
DJIMotorSetRef(friction_r, 0);
|
// DJIMotorSetRef(friction_r, 0);
|
||||||
break;
|
// break;
|
||||||
default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
|
default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
|
||||||
DJIMotorSetRef(friction_l, 30000);
|
DJIMotorSetRef(friction_l, 30000);
|
||||||
DJIMotorSetRef(friction_r, 30000);
|
DJIMotorSetRef(friction_r, 30000);
|
||||||
|
|
|
@ -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); // 发送
|
||||||
}
|
}
|
||||||
|
|
|
@ -22,12 +22,12 @@ void OnProjectLoad (void) {
|
||||||
//
|
//
|
||||||
// Dialog-generated settings
|
// Dialog-generated settings
|
||||||
//
|
//
|
||||||
Project.AddPathSubstitute ("D:/CLion/Project/wheel_legged_gimbal", "$(ProjectDir)");
|
|
||||||
Project.AddPathSubstitute ("d:/clion/project/wheel_legged_gimbal", "$(ProjectDir)");
|
|
||||||
Project.SetDevice ("STM32F407IG");
|
Project.SetDevice ("STM32F407IG");
|
||||||
Project.SetHostIF ("USB", "17935099");
|
Project.SetHostIF ("USB", "17935099");
|
||||||
Project.SetTargetIF ("SWD");
|
Project.SetTargetIF ("SWD");
|
||||||
Project.SetTIFSpeed ("Automatic");
|
Project.SetTIFSpeed ("5 MHz");
|
||||||
|
Project.AddPathSubstitute ("D:/CLion/Project/wheel_legged_gimbal", "$(ProjectDir)");
|
||||||
|
Project.AddPathSubstitute ("d:/clion/project/wheel_legged_gimbal", "$(ProjectDir)");
|
||||||
//
|
//
|
||||||
// User settings
|
// User settings
|
||||||
//
|
//
|
||||||
|
|
|
@ -1,35 +1,52 @@
|
||||||
|
|
||||||
|
|
||||||
OpenDocument="gimbal.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/gimbal/gimbal.c", Line=0
|
GraphedExpression="((vt_data[0]).mouse).x", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#e56a6f
|
||||||
OpenDocument="controller.h", FilePath="D:/CLion/Project/wheel_legged_gimbal/modules/algorithm/controller.h", Line=74
|
OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/cmd/robot_cmd.c", Line=89
|
||||||
OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/bsp/dwt/bsp_dwt.c", Line=101
|
|
||||||
OpenDocument="main.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Src/main.c", Line=70
|
OpenDocument="main.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Src/main.c", Line=70
|
||||||
OpenDocument="arm_mat_mult_f32.c", FilePath="C:/Users/clamar01/fork3/CMSIS_5/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c", Line=0
|
OpenDocument="dji_motor.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/modules/motor/DJImotor/dji_motor.c", Line=137
|
||||||
OpenDocument="stm32f4xx_hal_spi.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c", Line=3479
|
OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/bsp/dwt/bsp_dwt.c", Line=101
|
||||||
OpenDocument="ins_task.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/modules/imu/ins_task.c", Line=30
|
OpenDocument="tasks.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3414
|
||||||
OpenDocument="tasks.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3400
|
OpenDocument="SEGGER_RTT_printf.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Middlewares/Third_Party/SEGGER/RTT/SEGGER_RTT_printf.c", Line=108
|
||||||
OpenDocument="dji_motor.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/modules/motor/DJImotor/dji_motor.c", Line=157
|
OpenDocument="shoot.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/shoot/shoot.c", Line=99
|
||||||
|
OpenDocument="robot.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/robot.c", Line=8
|
||||||
|
OpenDocument="master_process.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/modules/master_machine/master_process.c", Line=155
|
||||||
|
OpenDocument="stm32f4xx_hal_spi.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c", Line=1274
|
||||||
|
OpenDocument="gimbal.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/gimbal/gimbal.c", Line=125
|
||||||
|
OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/wheel_legged_gimbal/Startup/startup_stm32f407ighx.s", Line=52
|
||||||
|
OpenDocument="controller.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/modules/algorithm/controller.c", Line=57
|
||||||
OpenToolbar="Debug", Floating=0, x=0, y=0
|
OpenToolbar="Debug", Floating=0, x=0, y=0
|
||||||
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=695, h=518, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
|
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=695, h=394, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
|
||||||
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=569, h=219, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=569, h=278, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||||
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=695, h=102, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=695, h=226, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||||
OpenWindow="Memory 1", DockArea=BOTTOM, x=2, y=0, w=406, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x0
|
OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=128, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x0
|
||||||
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=569, h=417, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=569, h=358, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||||
OpenWindow="Data Sampling", DockArea=BOTTOM, x=1, y=0, w=1022, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
|
OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=986, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||||
OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=695, h=518, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="5 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="541;-9", CodeGraphLegendShown=0, CodeGraphLegendPosition="499;6"
|
OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=264, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
|
||||||
OpenWindow="Console", DockArea=BOTTOM, x=0, y=0, w=490, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=695, h=394, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="313;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="326;0"
|
||||||
|
OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=539, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||||
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
|
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
|
||||||
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;118;100]
|
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;118;456]
|
||||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;922]
|
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[233;100;100;100;784]
|
||||||
TableHeader="Data Sampling Setup", SortCol="Type", SortOrder="DESCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[359;100;100;100;100;100;100;107;107]
|
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ((vt_data[0]).mouse).x"], ColWidths=[100;100;100]
|
||||||
|
TableHeader="Data Sampling Setup", SortCol="Type", SortOrder="DESCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[359;100;100;100;100;102;100;107;107]
|
||||||
TableHeader="Power Sampling", SortCol="Index", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
|
TableHeader="Power Sampling", SortCol="Index", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
|
||||||
TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110]
|
TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110]
|
||||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[166;134;100;148]
|
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[166;134;100;148]
|
||||||
TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
|
TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
|
||||||
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;351]
|
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;351]
|
||||||
TableHeader="Source Files", SortCol="File", SortOrder="DESCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[233;100;100;100;784]
|
WatchedExpression="friction_l", RefreshRate=2, Window=Watched Data 1
|
||||||
WatchedExpression="INS", RefreshRate=2, Window=Watched Data 1
|
WatchedExpression="yaw_motor", RefreshRate=2, Window=Watched Data 1
|
||||||
WatchedExpression="pitch_motor", Window=Watched Data 1
|
WatchedExpression="rc_data", RefreshRate=2, Window=Watched Data 1
|
||||||
WatchedExpression="current_feedforward", Window=Watched Data 1
|
WatchedExpression="gimbal_cmd_recv", RefreshRate=2, Window=Watched Data 1
|
||||||
WatchedExpression="yaw_motor", RefreshRate=5, Window=Watched Data 1
|
WatchedExpression="pitch_motor", RefreshRate=2, Window=Watched Data 1
|
||||||
WatchedExpression="included", Window=Watched Data 1
|
WatchedExpression="shoot_feedback_data", RefreshRate=2, Window=Watched Data 1
|
||||||
|
WatchedExpression="shoot_cmd_recv", RefreshRate=5, Window=Watched Data 1
|
||||||
|
WatchedExpression="loader", RefreshRate=2, Window=Watched Data 1
|
||||||
|
WatchedExpression="referee_data", RefreshRate=2, Window=Watched Data 1
|
||||||
|
WatchedExpression="GRAVITY_FEED", RefreshRate=2, Window=Watched Data 1
|
||||||
|
WatchedExpression="GRAVITY_FEED", Window=Watched Data 1
|
||||||
|
WatchedExpression="GRAVITY_FEED", Window=Watched Data 1
|
||||||
|
WatchedExpression="rc_data", Window=Watched Data 1
|
||||||
|
WatchedExpression="gimbal_cmd_send", RefreshRate=2, Window=Watched Data 1
|
||||||
|
WatchedExpression="vt_data", RefreshRate=2, Window=Watched Data 1
|
||||||
|
WatchedExpression="chassis_cmd_send", RefreshRate=2, Window=Watched Data 1
|
|
@ -1,7 +1,29 @@
|
||||||
#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 );
|
||||||
|
@ -11,21 +33,29 @@ static uint16_t crc_tab16[256];
|
||||||
*要检查的字节也是一个参数。字符串中的字节数为
|
*要检查的字节也是一个参数。字符串中的字节数为
|
||||||
*受恒定大小最大值的限制。
|
*受恒定大小最大值的限制。
|
||||||
*/
|
*/
|
||||||
uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes)
|
uint16_t crc_16(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;
|
//// if (!crc_tab16_init)
|
||||||
if (!crc_tab16_init)
|
//// init_crc16_tab();
|
||||||
init_crc16_tab();
|
// crc = CRC_START_16;
|
||||||
crc = CRC_START_16;
|
// 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;
|
||||||
|
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 crc;
|
return wCRC;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -36,8 +66,7 @@ uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes)
|
||||||
*要检查的字节数也是一个参数。
|
*要检查的字节数也是一个参数。
|
||||||
*/
|
*/
|
||||||
|
|
||||||
uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes)
|
uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes) {
|
||||||
{
|
|
||||||
uint16_t crc;
|
uint16_t crc;
|
||||||
const uint8_t *ptr;
|
const uint8_t *ptr;
|
||||||
uint16_t a;
|
uint16_t a;
|
||||||
|
@ -48,10 +77,9 @@ 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;
|
||||||
}
|
}
|
||||||
|
@ -62,11 +90,10 @@ 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];
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -77,18 +104,15 @@ 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
|
||||||
|
@ -99,3 +123,12 @@ 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]);
|
||||||
|
}
|
|
@ -11,4 +11,6 @@ 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
|
||||||
|
|
|
@ -0,0 +1,193 @@
|
||||||
|
//
|
||||||
|
// Created by SJQ on 2024/1/26.
|
||||||
|
//
|
||||||
|
|
||||||
|
#include "auto_aim.h"
|
||||||
|
//
|
||||||
|
// Created by sph on 2024/1/21.
|
||||||
|
//
|
||||||
|
#include "auto_aim.h"
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 选择目标装甲板
|
||||||
|
* @author SJQ
|
||||||
|
* @param[in] trajectory_cal:弹道解算结构体
|
||||||
|
* @retval 返回空
|
||||||
|
*/
|
||||||
|
void 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;
|
||||||
|
}
|
||||||
|
} 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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} 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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @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;
|
||||||
|
}
|
||||||
|
|
||||||
|
void 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;
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
}
|
|
@ -0,0 +1,83 @@
|
||||||
|
//
|
||||||
|
// Created by SJQ on 2024/1/26.
|
||||||
|
//
|
||||||
|
|
||||||
|
#ifndef WHEEL_LEGGED_GIMBAL_AUTO_AIM_H
|
||||||
|
#define WHEEL_LEGGED_GIMBAL_AUTO_AIM_H
|
||||||
|
|
||||||
|
//
|
||||||
|
// Created by sph on 2024/1/21.
|
||||||
|
//
|
||||||
|
|
||||||
|
#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;
|
||||||
|
|
||||||
|
|
||||||
|
void 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);
|
||||||
|
void auto_aim(Aim_Select_Type_t *aim_sel,Trajectory_Type_t *trajectory_cal,RecievePacket_t *receive_packet);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#endif //WHEEL_LEGGED_GIMBAL_AUTO_AIM_H
|
|
@ -168,7 +168,8 @@ void INS_Task(void)
|
||||||
INS.Roll = QEKF_INS.Roll;
|
INS.Roll = QEKF_INS.Roll;
|
||||||
INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
|
INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
|
||||||
|
|
||||||
VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
|
//VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
|
||||||
|
VisionSetAltitude(INS.Yaw * PI / 180, INS.Pitch * PI / 180);
|
||||||
}
|
}
|
||||||
|
|
||||||
// temperature control
|
// temperature control
|
||||||
|
|
|
@ -13,25 +13,42 @@
|
||||||
#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 Vision_Recv_s recv_data;
|
static RecievePacket_t recv_data;
|
||||||
static Vision_Send_s send_data;
|
static SendPacket_t 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.enemy_color = enemy_color;
|
send_data.detect_color = enemy_color;
|
||||||
send_data.work_mode = work_mode;
|
send_data.reserved = 0;
|
||||||
send_data.bullet_speed = bullet_speed;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void VisionSetAltitude(float yaw, float pitch, float roll)
|
//void VisionSetAltitude(float yaw, float pitch)
|
||||||
|
//{
|
||||||
|
// send_data.yaw = yaw;
|
||||||
|
// send_data.pitch = pitch;
|
||||||
|
//}
|
||||||
|
void VisionSetAltitude(float yaw, float pitch)
|
||||||
{
|
{
|
||||||
send_data.yaw = yaw;
|
send_data.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()导致的死锁,使得无法
|
||||||
|
@ -113,17 +130,24 @@ 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));
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* 视觉通信初始化 */
|
/* 视觉通信初始化 */
|
||||||
Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
|
RecievePacket_t *VisionInit(UART_HandleTypeDef *_handle)
|
||||||
{
|
{
|
||||||
UNUSED(_handle); // 仅为了消除警告
|
UNUSED(_handle); // 仅为了消除警告
|
||||||
USB_Init_Config_s conf = {.rx_cbk = DecodeVision};
|
USB_Init_Config_s conf = {.rx_cbk = DecodeVision};
|
||||||
|
@ -142,14 +166,23 @@ Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
|
||||||
|
|
||||||
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(1);
|
||||||
|
VisionSetAim(recv_data.x,recv_data.y,recv_data.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
|
||||||
|
|
|
@ -8,22 +8,19 @@
|
||||||
#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,
|
||||||
|
@ -35,8 +32,7 @@ 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;
|
||||||
|
@ -45,22 +41,21 @@ typedef struct
|
||||||
float yaw;
|
float yaw;
|
||||||
} Vision_Recv_s;
|
} Vision_Recv_s;
|
||||||
|
|
||||||
typedef enum
|
typedef enum {
|
||||||
{
|
// COLOR_NONE = 0,
|
||||||
COLOR_NONE = 0,
|
// COLOR_BLUE = 1,
|
||||||
COLOR_BLUE = 1,
|
// COLOR_RED = 2,
|
||||||
COLOR_RED = 2,
|
ENEMY_COLOR_RED = 0,
|
||||||
|
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,
|
||||||
|
@ -69,8 +64,7 @@ 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;
|
||||||
|
@ -79,6 +73,38 @@ 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()
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -86,7 +112,8 @@ 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(UART_HandleTypeDef *_handle);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 发送视觉数据
|
* @brief 发送视觉数据
|
||||||
|
@ -101,7 +128,8 @@ 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 设置发送数据的姿态部分
|
||||||
|
@ -109,6 +137,8 @@ void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Spee
|
||||||
* @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
|
|
@ -83,6 +83,8 @@ 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;
|
||||||
|
|
||||||
/* 命令码数据段长,根据官方协议来定义长度,还有自定义数据长度 */
|
/* 命令码数据段长,根据官方协议来定义长度,还有自定义数据长度 */
|
||||||
|
@ -102,6 +104,8 @@ typedef enum
|
||||||
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_remote_control = 12, // 0x0304
|
||||||
|
|
||||||
} JudgeDataLength_e;
|
} JudgeDataLength_e;
|
||||||
|
|
||||||
/****************************接收数据的详细说明****************************/
|
/****************************接收数据的详细说明****************************/
|
||||||
|
@ -227,6 +231,20 @@ typedef struct
|
||||||
float bullet_speed;
|
float bullet_speed;
|
||||||
} ext_shoot_data_t;
|
} ext_shoot_data_t;
|
||||||
|
|
||||||
|
/****************************图传链路数据****************************/
|
||||||
|
/* ID: 0x0304 Byte: 7 图传链路键鼠遥控数据 */
|
||||||
|
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;
|
||||||
|
}remote_control_t;
|
||||||
|
/****************************图传链路数据****************************/
|
||||||
|
|
||||||
/****************************机器人交互数据****************************/
|
/****************************机器人交互数据****************************/
|
||||||
/****************************机器人交互数据****************************/
|
/****************************机器人交互数据****************************/
|
||||||
/* 发送的内容数据段最大为 113 检测是否超出大小限制?实际上图形段不会超,数据段最多30个,也不会超*/
|
/* 发送的内容数据段最大为 113 检测是否超出大小限制?实际上图形段不会超,数据段最多30个,也不会超*/
|
||||||
|
|
|
@ -95,6 +95,9 @@ static void JudgeReadData(uint8_t *buff)
|
||||||
case ID_student_interactive: // 0x0301 syhtodo接收代码未测试
|
case ID_student_interactive: // 0x0301 syhtodo接收代码未测试
|
||||||
memcpy(&referee_info.ReceiveData, (buff + DATA_Offset), LEN_receive_data);
|
memcpy(&referee_info.ReceiveData, (buff + DATA_Offset), LEN_receive_data);
|
||||||
break;
|
break;
|
||||||
|
case ID_remote_control: // 0x0302
|
||||||
|
memcpy(&referee_info.RemoteControl, (buff + DATA_Offset), LEN_remote_control);
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -38,6 +38,8 @@ typedef struct
|
||||||
ext_robot_hurt_t RobotHurt; // 0x0206
|
ext_robot_hurt_t RobotHurt; // 0x0206
|
||||||
ext_shoot_data_t ShootData; // 0x0207
|
ext_shoot_data_t ShootData; // 0x0207
|
||||||
|
|
||||||
|
remote_control_t RemoteControl; // 0x304
|
||||||
|
|
||||||
// 自定义交互数据的接收
|
// 自定义交互数据的接收
|
||||||
Communicate_ReceiveData_t ReceiveData;
|
Communicate_ReceiveData_t ReceiveData;
|
||||||
|
|
||||||
|
|
|
@ -113,6 +113,8 @@ typedef struct
|
||||||
uint8_t key_count[3][16];
|
uint8_t key_count[3][16];
|
||||||
} RC_ctrl_t;
|
} RC_ctrl_t;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/* ------------------------- Internal Data ----------------------------------- */
|
/* ------------------------- Internal Data ----------------------------------- */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
Loading…
Reference in New Issue