From a32ac620ea763674218eb71a9bd7414f1b5c81f4 Mon Sep 17 00:00:00 2001 From: shmily744 <1527550984@qq.com> Date: Sun, 17 Mar 2024 21:45:23 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E5=BA=95=E7=9B=98id=EF=BC=8C?= =?UTF-8?q?=E9=80=9A=E4=BF=A1=EF=BC=8C=E4=BA=91=E5=8F=B0=E8=B7=9F=E9=9A=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/chassis.c | 20 +++---- application/cmd/robot_cmd.c | 6 +- application/gimbal/gimbal.c | 2 +- application/robot_def.h | 2 +- modules/master_machine/master_process.c | 35 +++++------ modules/vofa/vofa.c | 79 ++++++++++++------------- sentry_left.jdebug.user | 71 ++++++++++------------ 7 files changed, 102 insertions(+), 113 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 3817a6e..3f53d1a 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -93,21 +93,21 @@ void ChassisInit() .motor_type = M3508, }; // @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改. - 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); - chassis_motor_config.can_init_config.tx_id = 1; - chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL; + chassis_motor_config.can_init_config.tx_id = 2; + chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_rf = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id = 3; + chassis_motor_config.can_init_config.tx_id = 4; - 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_REVERSE; motor_lb = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id = 4; - chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL; + chassis_motor_config.can_init_config.tx_id = 1; + chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_rb = DJIMotorInit(&chassis_motor_config); referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI @@ -260,7 +260,7 @@ void ChassisTask() DJIMotorEnable(motor_lb); DJIMotorEnable(motor_rb); } - chassis_cmd_recv.offset_angle = chassis_cmd_recv.offset_angle * RAD_2_DEGREE; + //chassis_cmd_recv.offset_angle = chassis_cmd_recv.offset_angle * RAD_2_DEGREE; // 根据控制模式设定旋转速度 switch (chassis_cmd_recv.chassis_mode) { @@ -270,7 +270,7 @@ void ChassisTask() //chassis_cmd_recv.wz = 100.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle); break; case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 - //chassis_cmd_recv.wz = 1.5f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle); + chassis_cmd_recv.wz = 0.05f * chassis_cmd_recv.offset_angle; break; case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 chassis_cmd_recv.wz = -5; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 16792ea..99b0ef3 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -14,7 +14,7 @@ #include "bsp_log.h" // 私有宏,自动将编码器转换成角度值 -#define YAW_ALIGN_ANGLE 0.87f //(YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360 +#define YAW_ALIGN_ANGLE 272.799561F //(YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360 #define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360 /* cmd应用包含的模块实例指针和交互信息存储*/ @@ -124,8 +124,8 @@ static void RemoteControlSet() } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 { - chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; - gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; + chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; } // 云台参数,确定云台控制数据 diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 654717d..64f9497 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -248,7 +248,7 @@ void GimbalTask() big_yaw_relative_angle = big_yaw_motor->measure.total_angle * (4.0f/3.0f); - gimbal_feedback_data.yaw_motor_single_round_angle = loop_float_constrain(big_yaw_relative_angle,-PI,PI); + gimbal_feedback_data.yaw_motor_single_round_angle = loop_float_constrain(big_yaw_relative_angle,0,2*PI) * RAD_2_DEGREE; gimbal_feedback_data.mini_yaw_encode_angle = yaw_motor->measure.angle_single_round; // 推送消息 diff --git a/application/robot_def.h b/application/robot_def.h index f5025e1..6126437 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -27,7 +27,7 @@ /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ // 云台参数 #define YAW_CHASSIS_ALIGN_ECD 8012 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 -#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 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 #define PITCH_MAX_ANGLE 39 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) #define PITCH_MIN_ANGLE -18 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) diff --git a/modules/master_machine/master_process.c b/modules/master_machine/master_process.c index 6c00de1..b13730c 100644 --- a/modules/master_machine/master_process.c +++ b/modules/master_machine/master_process.c @@ -49,22 +49,6 @@ void VisionSetAim(float aim_x, float aim_y, float aim_z) { } -/** - * @brief 离线回调函数,将在daemon.c中被daemon task调用 - * @attention 由于HAL库的设计问题,串口开启DMA接收之后同时发送有概率出现__HAL_LOCK()导致的死锁,使得无法 - * 进入接收中断.通过daemon判断数据更新,重新调用服务启动函数以解决此问题. - * - * @param id vision_usart_instance的地址,此处没用. - */ -static void VisionOfflineCallback(void *id) -{ -#ifdef VISION_USE_UART - USARTServiceInit(vision_usart_instance); -#endif // !VISION_USE_UART - LOGWARNING("[vision] vision offline, restart communication."); - - memset(&recv_data,0,sizeof(recv_data)); -} #ifdef VISION_USE_UART @@ -143,12 +127,29 @@ static void DecodeVision(uint16_t recv_len) { if(VerifyCRC16CheckSum(vis_recv_buff,sizeof(recv_data))) { - DaemonReload(vision_daemon_instance); //喂狗 + DaemonReload(vision_daemon_instance); memcpy(&recv_data,vis_recv_buff,sizeof(recv_data)); } } } +/** + * @brief 离线回调函数,将在daemon.c中被daemon task调用 + * @attention 由于HAL库的设计问题,串口开启DMA接收之后同时发送有概率出现__HAL_LOCK()导致的死锁,使得无法 + * 进入接收中断.通过daemon判断数据更新,重新调用服务启动函数以解决此问题. + * + * @param id vision_usart_instance的地址,此处没用. + */ +static void VisionOfflineCallback(void *id) +{ +#ifdef VISION_USE_UART + USARTServiceInit(vision_usart_instance); +#endif // !VISION_USE_UART + USB_Init_Config_s conf = {.rx_cbk = DecodeVision}; + vis_recv_buff = USBInit(conf); + LOGWARNING("[vision] vision offline, restart communication."); +} + /* 视觉通信初始化 */ RecievePacket_t *VisionInit(UART_HandleTypeDef *_handle) { diff --git a/modules/vofa/vofa.c b/modules/vofa/vofa.c index 26fe198..34850a8 100644 --- a/modules/vofa/vofa.c +++ b/modules/vofa/vofa.c @@ -9,17 +9,14 @@ #include "usbd_cdc_if.h" /*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) { static uint8_t i = 0; - send_float temp[num]; //定义缓冲区数组 + send_float temp[num]; //定义缓冲区数组 uint8_t send_data[4 * num + 4]; //定义通过串口传出去的数组，数量是所传数据的字节数加上4个字节的尾巴 - for (i = 0; i < num; i++) - { + for (i = 0; i < num; i++) { temp[i].float_t = data[i]; //将所传数据移到缓冲区数组 } - for (i = 0; i < num; i++) - { + for (i = 0; i < num; i++) { send_data[4 * i] = temp[i].uint8_t[0]; send_data[4 * i + 1] = temp[i].uint8_t[1]; send_data[4 * i + 2] = temp[i].uint8_t[2]; @@ -30,8 +27,8 @@ void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart send_data[4 * num + 2] = 0x80; send_data[4 * num + 3] = 0x7f; //加上协议要求的4个尾巴 - //HAL_UART_Transmit(huart, (uint8_t *)send_data, 4 * num + 4, 100); - CDC_Transmit_FS((uint8_t *)send_data,4 * num + 4); + 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)) @@ -40,45 +37,45 @@ void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart #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灵活格式帧 +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; //数据长度 + 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(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(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(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); + 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