From 2f41e67de080bb7e5fd05e6b197f3c2332d1451c Mon Sep 17 00:00:00 2001 From: NeoZng Date: Sun, 11 Dec 2022 20:48:24 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E6=88=90=E4=BA=86LKmotor=E6=A8=A1?= =?UTF-8?q?=E5=9D=97=E7=9A=84=E9=87=8D=E6=9E=84=EF=BC=8C=E4=BC=98=E5=8C=96?= =?UTF-8?q?=E4=BA=86DJIMotor=E7=9A=84=E5=8F=8D=E9=A6=88=E8=AE=A1=E7=AE=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 3 +- application/chassis/chassis.c | 72 ++++++++++----------- application/cmd/robot_cmd.c | 109 ++++++++++++++++---------------- application/gimbal/gimbal.c | 44 +++---------- application/robot_def.h | 15 ++--- application/shoot/shoot.c | 17 ++--- bsp/bsp_can.h | 2 + modules/motor/HT04.c | 10 +-- modules/motor/HT04.h | 8 +-- modules/motor/LK9025.c | 90 +++++++++++++++++--------- modules/motor/LK9025.h | 58 ++++++++++++----- modules/motor/dji_motor.c | 44 ++++++------- modules/motor/dji_motor.h | 4 +- modules/remote/remote_control.c | 25 ++++---- modules/remote/remote_control.h | 18 ++++-- 15 files changed, 274 insertions(+), 245 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index e9a8cc4..f8f50e8 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -40,7 +40,8 @@ "super_cap.h": "c", "motor_def.h": "c", "quaternionekf.h": "c", - "shoot.h": "c" + "shoot.h": "c", + "lk9025.h": "c" }, "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools", } \ No newline at end of file diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 6d63e1f..f4173c6 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -65,13 +65,13 @@ void ChassisInit() .Kp = 10, .Ki = 0, .Kd = 0, - .MaxOut = 2000, + .MaxOut = 4000, }, .current_PID = { - .Kp = 1.2, + .Kp = 1, .Ki = 0, .Kd = 0, - .MaxOut = 2000, + .MaxOut = 4000, }, }, .controller_setting_init_config = { @@ -83,23 +83,23 @@ void ChassisInit() .motor_type = M3508, }; - chassis_motor_config.can_init_config.tx_id = 1; + chassis_motor_config.can_init_config.tx_id = 4; chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; motor_lf = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id = 2, + chassis_motor_config.can_init_config.tx_id = 3, chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; motor_rf = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id = 3, + chassis_motor_config.can_init_config.tx_id = 1, chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; motor_lb = DJIMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id = 4, + chassis_motor_config.can_init_config.tx_id = 2, chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; motor_rb = DJIMotorInit(&chassis_motor_config); - referee_data = RefereeInit(&huart6); //裁判系统初始化 + referee_data = RefereeInit(&huart6); // 裁判系统初始化 SuperCap_Init_Config_s cap_conf = { .can_config = { @@ -107,23 +107,23 @@ void ChassisInit() .tx_id = 0x302, .rx_id = 0x301, }}; - cap = SuperCapInit(&cap_conf); //超级电容初始化 + cap = SuperCapInit(&cap_conf); // 超级电容初始化 // 发布订阅初始化,如果为双板,则需要can comm来传递消息 #ifdef CHASSIS_BOARD - Chassis_IMU_data=INS_Init(); // 底盘IMU初始化 + Chassis_IMU_data = INS_Init(); // 底盘IMU初始化 CANComm_Init_Config_s comm_conf = { - .can_config={ - .can_handle=&hcan2, - .tx_id=0x311, - .rx_id=0x312, + .can_config = { + .can_handle = &hcan2, + .tx_id = 0x311, + .rx_id = 0x312, }, - .recv_data_len=sizeof(Chassis_Ctrl_Cmd_s), - .send_data_len=sizeof(Chassis_Upload_Data_s), + .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s), + .send_data_len = sizeof(Chassis_Upload_Data_s), }; chasiss_can_comm = CANCommInit(&comm_conf); // can comm初始化 -#endif // CHASSIS_BOARD +#endif // CHASSIS_BOARD #ifdef ONE_BOARD chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s)); @@ -137,14 +137,14 @@ void ChassisInit() #define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD) /** * @brief 计算每个轮毂电机的输出,正运动学解算 - * 用宏进行预替换减小开销 + * 用宏进行预替换减小开销,运动解算具体过程参考教程 */ static void MecanumCalculate() { vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER; vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER; - vt_lb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * LB_CENTER; - vt_rb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * RB_CENTER; + vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * LB_CENTER; + vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER; } /** @@ -153,7 +153,7 @@ static void MecanumCalculate() */ static void LimitChassisOutput() { - // 限制待添加 + // 功率限制待添加 // referee_data->PowerHeatData.chassis_power; // referee_data->PowerHeatData.chassis_power_buffer; @@ -170,8 +170,8 @@ static void LimitChassisOutput() */ static void EstimateSpeed() { - // 根据电机速度和imu的速度解算 - // chassis_feedback_data.vx vy wz + // 根据电机速度和imu的速度解算,利用加速度计判断是否打滑(如果有) + // chassis_feedback_data.vx vy wz = // ... } @@ -183,18 +183,18 @@ void ChassisTask() SubGetMessage(chassis_sub, &chassis_cmd_recv); #endif #ifdef CHASSIS_BOARD - 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 - if (chassis_cmd_recv.chassis_mode==CHASSIS_ZERO_FORCE) - { - DJIMotorStop(motor_lf); // 如果出现重要模块离线或遥控器设置为急停,让电机停止 + if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) + { // 如果出现重要模块离线或遥控器设置为急停,让电机停止 + DJIMotorStop(motor_lf); DJIMotorStop(motor_rf); DJIMotorStop(motor_lb); DJIMotorStop(motor_rb); } else - { + { // 正常工作 DJIMotorEnable(motor_lf); DJIMotorEnable(motor_rf); DJIMotorEnable(motor_lb); @@ -202,14 +202,13 @@ void ChassisTask() } // 根据控制模式设定旋转速度 - // 后续增加不同状态的过渡模式? switch (chassis_cmd_recv.chassis_mode) { case CHASSIS_NO_FOLLOW: chassis_cmd_recv.wz = 0; // 底盘不旋转,但维持全向机动,一般用于调整云台姿态 break; case CHASSIS_FOLLOW_GIMBAL_YAW: - chassis_cmd_recv.wz = 0.05f * powf(chassis_cmd_recv.wz, 2.0f); // 跟随,不单独设置pid,以误差角平方为速度输出 + chassis_cmd_recv.wz = 0.05f * powf(chassis_cmd_recv.wz, 2.0f); // 跟随,不单独设置pid,以误差角度平方为速度输出 break; case CHASSIS_ROTATE: // chassis_cmd_recv.wz=sin(t) // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 @@ -220,10 +219,11 @@ void ChassisTask() // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上 // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东) - chassis_vx = chassis_cmd_recv.vx * arm_cos_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD) - - chassis_cmd_recv.vy * arm_sin_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD); - chassis_vy = chassis_cmd_recv.vx * arm_sin_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD) - - chassis_cmd_recv.vy * arm_cos_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD); + static float sin_theta, cos_theta; + cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD); + sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD); + chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta; + chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta; // 根据控制模式进行正运动学解算,计算底盘输出 MecanumCalculate(); @@ -243,9 +243,9 @@ void ChassisTask() // 推送反馈消息 #ifdef ONE_BOARD - PubPushMessage(chassis_pub, &chassis_feedback_data); + PubPushMessage(chassis_pub, (void *)&chassis_feedback_data); #endif #ifdef CHASSIS_BOARD - CANCommSend(chasiss_can_comm,(void*)&chassis_feedback_data); + CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data); #endif // CHASSIS_BOARD } \ No newline at end of file diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 273fa43..35b2a89 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -7,43 +7,42 @@ #include "general_def.h" #include "dji_motor.h" -// 自动将编码器转换成角度值 +// 私有宏,自动将编码器转换成角度值 #define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF) #define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF) /* gimbal_cmd应用包含的模块实例指针和交互信息存储*/ -#ifdef GIMBAL_BOARD +#ifdef GIMBAL_BOARD // 对双板的兼容,条件编译 #include "can_comm.h" static CANCommInstance *cmd_can_comm; // 双板通信 #endif #ifdef ONE_BOARD -static Publisher_t *chassis_cmd_pub -static Subscriber_t *chassis_feed_sub; -#endif // ONE_BOARD -static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关 +static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者 +static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者 +#endif // ONE_BOARD + +static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关 static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等 static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回 static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回 static Vision_Send_s vision_send_data; // 视觉发送数据 -static Publisher_t *gimbal_cmd_pub; -static Gimbal_Ctrl_Cmd_s gimbal_cmd_send; // 传递给云台的控制信息 -static Subscriber_t *gimbal_feed_sub; +static Publisher_t *gimbal_cmd_pub; // 云台控制消息发布者 +static Subscriber_t *gimbal_feed_sub; // 云台反馈信息订阅者 +static Gimbal_Ctrl_Cmd_s gimbal_cmd_send; // 传递给云台的控制信息 static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息 -static Publisher_t *shoot_cmd_pub; -static Shoot_Ctrl_Cmd_s shoot_cmd_send; // 传递给发射的控制信息 -static Subscriber_t *shoot_feed_sub; +static Publisher_t *shoot_cmd_pub; // 发射控制消息发布者 +static Subscriber_t *shoot_feed_sub; // 发射反馈信息订阅者 +static Shoot_Ctrl_Cmd_s shoot_cmd_send; // 传递给发射的控制信息 static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息 - - -static Robot_Status_e robot_state; +static Robot_Status_e robot_state; // 机器人整体工作状态 void GimbalCMDInit() { - rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意dbus协议串口需加反相器 + rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个 vision_recv_data = VisionInit(&huart1); // 视觉通信串口 gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); @@ -51,24 +50,24 @@ void GimbalCMDInit() shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s)); shoot_feed_sub = SubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s)); -#ifdef ONE_BOARD +#ifdef ONE_BOARD // 双板兼容 chassis_cmd_pub = PubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s)); chassis_feed_sub = SubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s)); #endif // ONE_BOARD #ifdef GIMBAL_BOARD - CANComm_Init_Config_s comm_conf={ - .can_config={ - .can_handle=&hcan1, - .tx_id=0x312, - .rx_id=0x311, + CANComm_Init_Config_s comm_conf = { + .can_config = { + .can_handle = &hcan1, + .tx_id = 0x312, + .rx_id = 0x311, }, - .recv_data_len=sizeof(Chassis_Upload_Data_s), - .send_data_len=sizeof(Chassis_Ctrl_Cmd_s), + .recv_data_len = sizeof(Chassis_Upload_Data_s), + .send_data_len = sizeof(Chassis_Ctrl_Cmd_s), }; - cmd_can_comm=CANCommInit(&comm_conf); + cmd_can_comm = CANCommInit(&comm_conf); #endif // GIMBAL_BOARD - robot_state=ROBOT_WORKING; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 + robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 } /** @@ -98,53 +97,52 @@ static void CalcOffsetAngle() } /** - * @brief 输入为遥控器(调试时)的模式和控制量设置 + * @brief 控制输入为遥控器(调试时)的模式和控制量设置 * */ static void RemoteControlSet() { // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据? - if (switch_is_down(rc_data[TEMP].rc.s[0])) // 右侧开关状态[下],底盘跟随云台 + if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台 chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; - else if (switch_is_mid(rc_data[TEMP].rc.s[0])) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 + else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; // 云台参数,确定云台控制数据 - if (switch_is_mid(rc_data[TEMP].rc.s[1])) // 左侧开关状态为[中],视觉模式 + if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式 { // 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制 // ... } // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制 - if (switch_is_down(rc_data[TEMP].rc.s[1]) || vision_recv_data->target_state == NO_TARGET) + if (switch_is_down(rc_data[TEMP].rc.switch_left) || vision_recv_data->target_state == NO_TARGET) { // 按照摇杆的输出大小进行角度增量,增益系数需调整 - gimbal_cmd_send.yaw += 0.0015f * (float)rc_data[TEMP].rc.joystick[2]; - gimbal_cmd_send.pitch += 0.002f * (float)rc_data[TEMP].rc.joystick[3]; + gimbal_cmd_send.yaw += 0.0015f * (float)rc_data[TEMP].rc.rocker_l_; + gimbal_cmd_send.pitch += 0.002f * (float)rc_data[TEMP].rc.rocker_l1; gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; } // 底盘参数,目前没有加入小陀螺(调试似乎没有必要),系数需要调整 - chassis_cmd_send.vx = 10.0f * (float)rc_data[TEMP].rc.joystick[0]; - chassis_cmd_send.vy = 10.0f * (float)rc_data[TEMP].rc.joystick[1]; + chassis_cmd_send.vx = 10.0f * (float)rc_data[TEMP].rc.rocker_r_; + chassis_cmd_send.vy = 10.0f * (float)rc_data[TEMP].rc.rocker_r1; // 发射参数 - if (switch_is_up(rc_data[TEMP].rc.s[0])) // 右侧开关状态[上],弹舱打开 - {// 弹舱舵机控制,待添加servo_motor模块,开启 - - } + if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 + { // 弹舱舵机控制,待添加servo_motor模块,开启 + } else ; // 弹舱舵机控制,待添加servo_motor模块,关闭 // 摩擦轮控制,后续可以根据左侧拨轮的值大小切换射频 - if (rc_data[TEMP].rc.joystick[4] < -100) + if (rc_data[TEMP].rc.dial < -100) shoot_cmd_send.friction_mode = FRICTION_ON; else shoot_cmd_send.friction_mode = FRICTION_OFF; // 拨弹控制,目前固定为连发 - if (rc_data[TEMP].rc.joystick[4] <-500) + if (rc_data[TEMP].rc.dial < -500) shoot_cmd_send.load_mode = LOAD_BURSTFIRE; else shoot_cmd_send.load_mode = LOAD_STOP; - shoot_cmd_send.shoot_rate=1; + shoot_cmd_send.shoot_rate = 1; } /** @@ -158,21 +156,22 @@ static void MouseKeySet() /** * @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等 * '300'待修改成合适的值,或改为开关控制 + * @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停) * */ static void EmergencyHandler() { - // 拨轮的向下拨超过一半,注意向下拨轮是正 - if (rc_data[TEMP].rc.joystick[4] > 300 || robot_state==ROBOT_STOP) // 还需添加重要应用和模块离线的判断 + // 拨轮的向下拨超过一半,注意向打时下拨轮是正 + if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断 { robot_state = ROBOT_STOP; // 遥控器左上侧拨轮打满,进入紧急停止模式 gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; shoot_cmd_send.shoot_mode = SHOOT_OFF; } - if(switch_is_up(rc_data[TEMP].rc.s[0])) + if (switch_is_up(rc_data[TEMP].rc.switch_right)) { - robot_state = ROBOT_WORKING; // 遥控器右侧开关为[上],恢复正常运行 + robot_state = ROBOT_READY; // 遥控器右侧开关为[上],恢复正常运行 shoot_cmd_send.shoot_mode = SHOOT_ON; } } @@ -181,10 +180,10 @@ void GimbalCMDTask() { // 从其他应用获取回传数据 #ifdef ONE_BOARD - SubGetMessage(chassis_feed_sub, &chassis_fetch_data); + SubGetMessage(chassis_feed_sub, (void*)&chassis_fetch_data); #endif // ONE_BOARD #ifdef GIMBAL_BOARD - chassis_fetch_data=*(Chassis_Upload_Data_s*)CANCommGet(cmd_can_comm); + chassis_fetch_data = *(Chassis_Upload_Data_s *)CANCommGet(cmd_can_comm); #endif // GIMBAL_BOARD SubGetMessage(shoot_feed_sub, &shoot_fetch_data); SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data); @@ -192,14 +191,14 @@ void GimbalCMDTask() // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过私有变量完成 CalcOffsetAngle(); - if (switch_is_down(rc_data[TEMP].rc.s[1])) // 遥控器左侧开关状态为[下],遥控器控制 + if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制 RemoteControlSet(); - else if (switch_is_up(rc_data[TEMP].rc.s[1])) // 遥控器左侧开关状态为[上],键盘控制 + else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制 MouseKeySet(); EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 - // 设置视觉发送数据 + // 设置视觉发送数据,还需增加加速度和角速度数据 vision_send_data.bullet_speed = chassis_fetch_data.bullet_speed; vision_send_data.enemy_color = chassis_fetch_data.enemy_color; vision_send_data.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; @@ -209,12 +208,12 @@ void GimbalCMDTask() // 推送消息,双板通信,视觉通信等 // 应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置 #ifdef ONE_BOARD - SubGetMessage(chassis_feed_sub, &chassis_fetch_data); + PubPushMessage(chassis_cmd_pub, (void *)&chassis_cmd_send); #endif // ONE_BOARD #ifdef GIMBAL_BOARD - CANCommSend(cmd_can_comm,(void*)&chassis_cmd_send); + CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send); #endif // GIMBAL_BOARD - PubPushMessage(shoot_cmd_pub, &shoot_cmd_send); - PubPushMessage(gimbal_cmd_pub, &gimbal_cmd_send); + PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send); + PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send); VisionSend(&vision_send_data); } diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index d1853d4..8ff0541 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -9,10 +9,10 @@ static attitude_t *Gimbal_IMU_data; // 云台IMU数据 static DJIMotorInstance *yaw_motor; // yaw电机 static DJIMotorInstance *pitch_motor; // pitch电机 -static Publisher_t *gimbal_pub; -static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给gimbal_cmd的云台状态信息 -static Subscriber_t *gimbal_sub; -static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自gimbal_cmd的控制信息 +static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd) +static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 +static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息 +static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息 void GimbalInit() { @@ -26,17 +26,17 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp = 10, + .Kp = 20, .Ki = 0, .Kd = 0, .MaxOut = 2000, - .DeadBand=0.3, + .DeadBand = 0.3, }, .speed_PID = { .Kp = 10, .Ki = 0, .Kd = 0, - .MaxOut = 2000, + .MaxOut = 4000, }, .other_angle_feedback_ptr = &Gimbal_IMU_data->YawTotalAngle, // 还需要增加角速度额外反馈指针 @@ -62,7 +62,7 @@ void GimbalInit() .Ki = 0, .Kd = 0, .MaxOut = 4000, - .DeadBand=0.3, + .DeadBand = 0.3, }, .speed_PID = { .Kp = 10, @@ -91,15 +91,6 @@ void GimbalInit() gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); } -// /** -// * @brief -// * -// */ -// static void TransitionMode() -// { - -// } - void GimbalTask() { // 获取云台控制数据 @@ -107,7 +98,6 @@ void GimbalTask() SubGetMessage(gimbal_sub, &gimbal_cmd_recv); // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref - // 是否要增加不同模式之间的过渡? switch (gimbal_cmd_recv.gimbal_mode) { // 停止 @@ -140,25 +130,11 @@ void GimbalTask() default: break; } - // 过渡示例: - /* 需要给每个case增加如下判断,并添加一个过渡行为函数和过渡标志位 - case xxx: - if(last_mode!=xxx) - { - transition_flag=1; - } - break; - - void TransitMode() - { - motor_output=lpf_coef * last_output+(1 - lpf_coef) * concur_output; - } - */ // 设置反馈数据 gimbal_feedback_data.gimbal_imu_data = *Gimbal_IMU_data; gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->motor_measure.angle_single_round; - // 推送消息 - PubPushMessage(gimbal_pub, &gimbal_feedback_data); + // 推送消息 + PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data); } \ No newline at end of file diff --git a/application/robot_def.h b/application/robot_def.h index 67b1933..19fddcc 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -17,13 +17,13 @@ #include "stdint-gcc.h" /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ -// #define ONE_BOARD // 单板控制整车 +#define ONE_BOARD // 单板控制整车 // #define CHASSIS_BOARD //底盘板 -#define GIMBAL_BOARD //云台板 +// #define GIMBAL_BOARD //云台板 /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ // 云台参数 -#define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 +#define YAW_CHASSIS_ALIGN_ECD 4000 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 #define YAW_ECD_GREATER_THAN_4096 0 // yaw电机的初始编码器值是否大于4096,是为1,否为0 #define PITCH_HORIZON_ECD 0 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 // 发射参数 @@ -43,7 +43,7 @@ (defined(ONE_BOARD) && defined(GIMBAL_BOARD)) || \ (defined(CHASSIS_BOARD) && defined(GIMBAL_BOARD)) #error Conflict board definition! You can only define one type. -#endif +#endif #pragma pack(1) // 压缩结构体,取消字节对齐 @@ -56,7 +56,7 @@ typedef enum { ROBOT_STOP, - ROBOT_WORKING, + ROBOT_READY, } Robot_Status_e; // 应用状态 @@ -88,13 +88,12 @@ typedef enum GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式 } gimbal_mode_e; - // 发射模式设置 typedef enum { - SHOOT_ON=0, + SHOOT_ON = 0, SHOOT_OFF, -}shoot_mode_e; +} shoot_mode_e; typedef enum { FRICTION_OFF, // 摩擦轮关闭 diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 8a8d40c..c6776cd 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -32,10 +32,10 @@ void ShootInit() .Kp = 10, .Ki = 0, .Kd = 0, - .MaxOut = 200, + .MaxOut = 2000, }, .current_PID = { - .Kp = 10, + .Kp = 1, .Ki = 0, .Kd = 0, .MaxOut = 2000, @@ -61,10 +61,10 @@ void ShootInit() .Kp = 10, .Ki = 0, .Kd = 0, - .MaxOut = 200, + .MaxOut = 2000, }, .current_PID = { - .Kp = 5, + .Kp = 1, .Ki = 0, .Kd = 0, .MaxOut = 2000, @@ -200,8 +200,8 @@ void ShootTask() DJIMotorSetRef(friction_r, 0); break; default: - DJIMotorSetRef(friction_l, 200); - DJIMotorSetRef(friction_r, 200); + DJIMotorSetRef(friction_l, 4000); + DJIMotorSetRef(friction_r, 4000); break; } // 关闭摩擦轮 if (shoot_cmd_recv.friction_mode==FRICTION_OFF) @@ -210,8 +210,6 @@ void ShootTask() DJIMotorSetRef(friction_r, 0); } - - // 开关弹舱盖 if (shoot_cmd_recv.lid_mode == LID_CLOSE) { @@ -221,4 +219,7 @@ void ShootTask() { //... } + + // 反馈数据 + PubPushMessage(shoot_pub,(void*)&shoot_feedback_data); } \ No newline at end of file diff --git a/bsp/bsp_can.h b/bsp/bsp_can.h index 605df2b..c820d25 100644 --- a/bsp/bsp_can.h +++ b/bsp/bsp_can.h @@ -23,6 +23,7 @@ typedef struct _ uint8_t rx_len; // 接收长度,可能为0-8 // 接收的回调函数,用于解析接收到的数据 void (*can_module_callback)(struct _ *); // callback needs an instance to tell among registered ones + void* id; } CANInstance; #pragma pack() @@ -33,6 +34,7 @@ typedef struct uint32_t tx_id; uint32_t rx_id; void (*can_module_callback)(CANInstance *); + void* id; } CAN_Init_Config_s; /** diff --git a/modules/motor/HT04.c b/modules/motor/HT04.c index 3c47aec..4b04d15 100644 --- a/modules/motor/HT04.c +++ b/modules/motor/HT04.c @@ -1,7 +1,7 @@ #include "HT04.h" #include "memory.h" -joint_instance *joint_motor_info[HT_MOTOR_CNT]; +HKMotor_Measure_t *joint_motor_info[HT_MOTOR_CNT]; static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits) { @@ -36,15 +36,15 @@ static void DecodeJoint(CANInstance *motor_instance) } } -joint_instance *HTMotorInit(CAN_Init_Config_s config) +HKMotor_Measure_t *HTMotorInit(CAN_Init_Config_s config) { static uint8_t idx; - joint_motor_info[idx] = (joint_instance *)malloc(sizeof(joint_instance)); + joint_motor_info[idx] = (HKMotor_Measure_t *)malloc(sizeof(HKMotor_Measure_t)); joint_motor_info[idx]->motor_can_instace = CANRegister(&config); return joint_motor_info[idx++]; } -void JointControl(joint_instance *_instance, float current) +void JointControl(HKMotor_Measure_t *_instance, float current) { uint16_t tmp; LIMIT_MIN_MAX(current, T_MIN, T_MAX); @@ -54,7 +54,7 @@ void JointControl(joint_instance *_instance, float current) CANTransmit(_instance->motor_can_instace); } -void SetJointMode(joint_mode cmd, joint_instance *_instance) +void SetJointMode(joint_mode cmd, HKMotor_Measure_t *_instance) { static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00}; buf[7] = (uint8_t)cmd; diff --git a/modules/motor/HT04.h b/modules/motor/HT04.h index 904f96f..1e8bce4 100644 --- a/modules/motor/HT04.h +++ b/modules/motor/HT04.h @@ -24,7 +24,7 @@ typedef struct // HT04 PIDInstance pid; CANInstance *motor_can_instace; -} joint_instance; +} HKMotor_Measure_t; typedef enum { @@ -33,10 +33,10 @@ typedef enum CMD_ZERO_POSITION = 0xfe } joint_mode; -joint_instance *HTMotorInit(CAN_Init_Config_s config); +HKMotor_Measure_t *HTMotorInit(CAN_Init_Config_s config); -void JointControl(joint_instance *_instance, float current); +void JointControl(HKMotor_Measure_t *_instance, float current); -void SetJointMode(joint_mode cmd, joint_instance *_instance); +void SetJointMode(joint_mode cmd, HKMotor_Measure_t *_instance); #endif // !HT04_H#define HT04_H \ No newline at end of file diff --git a/modules/motor/LK9025.c b/modules/motor/LK9025.c index 6d5d4e1..82ca1a2 100644 --- a/modules/motor/LK9025.c +++ b/modules/motor/LK9025.c @@ -1,47 +1,75 @@ #include "LK9025.h" +#include "stdlib.h" -static driven_instance *driven_motor_info[LK_MOTOR_CNT]; +static uint8_t idx; +static LKMotorInstance* lkmotor_instance[LK_MOTOR_MX_CNT]={NULL}; -static void DecodeDriven(CANInstance *_instance) +static void LKMotorDecode(CANInstance *_instance) { - for (size_t i = 0; i < LK_MOTOR_CNT; i++) + static LKMotor_Measure_t* measure; + static uint8_t* rx_buff; + rx_buff=_instance->rx_buff; + measure=&((LKMotorInstance*)_instance)->measure; + + measure->last_ecd=measure->ecd; + measure->ecd=(uint16_t)((rx_buff[7] << 8) | rx_buff[6]); + measure->angle_single_round=ECD_ANGLE_COEF*measure->ecd; + measure->speed_aps=(1-SPEED_SMOOTH_COEF)*measure->speed_aps+ + SPEED_SMOOTH_COEF*(float)((int16_t)(rx_buff[5] << 8 | rx_buff[4])); + measure->real_current=(1-CURRENT_SMOOTH_COEF)*measure->real_current+ + CURRENT_SMOOTH_COEF*(float)((int16_t)(rx_buff[3] << 8 | rx_buff[2])); + measure->temperate=rx_buff[1]; + + //计算多圈角度 + if (measure->ecd - measure->last_ecd > 32678) + measure->total_round--; + else if (measure->ecd - measure->last_ecd < -32678) + measure->total_round++; + measure->total_angle = measure->total_round * 360 + measure->angle_single_round; +} + + +void LKMotorControl() +{ + for (size_t i = 0; i < idx; i++) { - if (driven_motor_info[i]->motor_can_instance == _instance) - { - driven_motor_info[i]->last_ecd = driven_motor_info[i]->ecd; - driven_motor_info[i]->ecd = (uint16_t)((_instance->rx_buff[7] << 8) | _instance->rx_buff[6]); - driven_motor_info[i]->speed_rpm = (uint16_t)(_instance->rx_buff[5] << 8 | _instance->rx_buff[4]); - driven_motor_info[i]->real_current = (uint16_t)(_instance->rx_buff[3] << 8 | _instance->rx_buff[2]); - driven_motor_info[i]->temperate = _instance->rx_buff[1]; - break; - } + } } -driven_instance *LKMotroInit(CAN_Init_Config_s config) +LKMotorInstance *LKMotroInit(Motor_Init_Config_s *config) { - static uint8_t idx; - driven_motor_info[idx] = (driven_instance *)malloc(sizeof(driven_instance)); - config.can_module_callback = DecodeDriven; - driven_motor_info[idx]->motor_can_instance = CANRegister(&config); - return driven_motor_info[idx++]; + lkmotor_instance[idx]=(LKMotorInstance*)malloc(sizeof(LKMotorInstance)); + memset(lkmotor_instance[idx],0,sizeof(LKMotorInstance)); + + lkmotor_instance[idx]->motor_settings=config->controller_setting_init_config; + PID_Init(&lkmotor_instance[idx]->current_PID,&config->controller_param_init_config.current_PID); + PID_Init(&lkmotor_instance[idx]->current_PID,&config->controller_param_init_config.current_PID); + PID_Init(&lkmotor_instance[idx]->current_PID,&config->controller_param_init_config.current_PID); + lkmotor_instance[idx]->other_angle_feedback_ptr=config->controller_param_init_config.other_angle_feedback_ptr; + lkmotor_instance[idx]->other_speed_feedback_ptr=config->controller_param_init_config.other_speed_feedback_ptr; + + config->can_init_config.can_module_callback=LKMotorDecode; + config->can_init_config.rx_id=0x140+config->can_init_config.tx_id; + config->can_init_config.tx_id=config->can_init_config.tx_id+0x240; + lkmotor_instance[idx]->motor_can_ins=CANRegister(&config->can_init_config); + + LKMotorEnable(lkmotor_instance[idx]); + return lkmotor_instance[idx++]; } -void DrivenControl(int16_t motor1_current, int16_t motor2_current) + +void LKMotorStop(LKMotorInstance *motor) { - LIMIT_MIN_MAX(motor1_current, I_MIN, I_MAX); - LIMIT_MIN_MAX(motor2_current, I_MIN, I_MAX); - driven_motor_info[0]->motor_can_instance->tx_buff[0] = motor1_current; - driven_motor_info[0]->motor_can_instance->tx_buff[1] = motor1_current >> 8; - driven_motor_info[0]->motor_can_instance->tx_buff[2] = motor2_current; - driven_motor_info[0]->motor_can_instance->tx_buff[3] = motor2_current >> 8; - CANTransmit(driven_motor_info[0]->motor_can_instance); + motor->stop_flag=MOTOR_STOP; } -void SetDrivenMode(driven_mode cmd, uint16_t motor_id) +void LKMotorEnable(LKMotorInstance *motor) { - static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00}; - // code goes here ... - - // CANTransmit(driven_mode) + motor->stop_flag=MOTOR_ENALBED; +} + +void LKMotorSetRef(LKMotorInstance *motor, float ref) +{ + motor->pid_ref=ref; } diff --git a/modules/motor/LK9025.h b/modules/motor/LK9025.h index 8c8b8d1..7c9360c 100644 --- a/modules/motor/LK9025.h +++ b/modules/motor/LK9025.h @@ -6,32 +6,60 @@ #include "controller.h" #include "motor_def.h" -#define LK_MOTOR_CNT 2 +#define LK_MOTOR_MX_CNT 4 + #define I_MIN -2000 #define I_MAX 2000 +#define CURRENT_SMOOTH_COEF 0.9f +#define SPEED_SMOOTH_COEF 0.85f +#define REDUCTION_RATIO_DRIVEN 1 +#define ECD_ANGLE_COEF (360.0f/65536.0f) typedef struct // 9025 { - uint16_t last_ecd; - uint16_t ecd; - int16_t speed_rpm; - int16_t real_current; - uint8_t temperate; + uint16_t last_ecd;// 上一次读取的编码器值 + uint16_t ecd; // + float angle_single_round; // 单圈角度 + float speed_aps; // speed angle per sec(degree:°) + int16_t real_current; // 实际电流 + uint8_t temperate; //温度,C° - PIDInstance *pid; - CANInstance *motor_can_instance; + float total_angle; // 总角度 + int32_t total_round; //总圈数 -} driven_instance; +} LKMotor_Measure_t; -typedef enum +typedef struct { - unused = 0, -} driven_mode; + LKMotor_Measure_t measure; -driven_instance *LKMotroInit(CAN_Init_Config_s config); + Motor_Control_Setting_s motor_settings; -void DrivenControl(int16_t motor1_current, int16_t motor2_current); + float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针 + float *other_speed_feedback_ptr; + PIDInstance current_PID; + PIDInstance speed_PID; + PIDInstance angle_PID; + float pid_ref; + + Motor_Working_Type_e stop_flag; // 启停标志 + + CANInstance* motor_can_ins; + +}LKMotorInstance; + + +LKMotorInstance *LKMotroInit(Motor_Init_Config_s* config); + +void LKMotorSetRef(LKMotorInstance* motor,float ref); + +void LKMotorControl(); + +void LKMotorStop(LKMotorInstance *motor); + +void LKMotorEnable(LKMotorInstance *motor); + +void LKMotorSetRef(LKMotorInstance *motor,float ref); -void SetDrivenMode(driven_mode cmd, uint16_t motor_id); #endif // LK9025_H diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index 0c3655b..4fce573 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -123,33 +123,26 @@ static void DecodeDJIMotor(CANInstance *_instance) // 由于需要多次变址访存,直接将buff和measure地址保存在寄存器里避免多次存取 static uint8_t *rxbuff; static DJI_Motor_Measure_s *measure; + rxbuff = _instance->rx_buff; + measure = &((DJIMotorInstance*)_instance->id)->motor_measure; // measure要多次使用,保存指针减小访存开销 - for (size_t i = 0; i < idx; i++) - { - if (dji_motor_info[i]->motor_can_instance == _instance) - { - rxbuff = _instance->rx_buff; - measure = &dji_motor_info[i]->motor_measure; // measure要多次使用,保存指针减小访存开销 - uint8_t nice; - // resolve data and apply filter to current and speed - measure->last_ecd = measure->ecd; - measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1]; - measure->angle_single_round = ECD_ANGLE_COEF * (float)measure->ecd; - measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps + RPM_2_ANGLE_PER_SEC * - SPEED_SMOOTH_COEF *(float)((int16_t)(rxbuff[2] << 8 | rxbuff[3])) ; - measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current + - CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5])); - measure->temperate = rxbuff[6]; + uint8_t nice; + // resolve data and apply filter to current and speed + measure->last_ecd = measure->ecd; + measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1]; + measure->angle_single_round = ECD_ANGLE_COEF * (float)measure->ecd; + measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps + RPM_2_ANGLE_PER_SEC * + SPEED_SMOOTH_COEF *(float)((int16_t)(rxbuff[2] << 8 | rxbuff[3])) ; + measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current + + CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5])); + measure->temperate = rxbuff[6]; - // multi rounds calc,计算的前提是两次采样间电机转过的角度小于180° - if (measure->ecd - measure->last_ecd > 4096) - measure->total_round--; - else if (measure->ecd - measure->last_ecd < -4096) - measure->total_round++; - measure->total_angle = measure->total_round * 360 + measure->angle_single_round; - break; - } - } + // multi rounds calc,计算的前提是两次采样间电机转过的角度小于180° + if (measure->ecd - measure->last_ecd > 4096) + measure->total_round--; + else if (measure->ecd - measure->last_ecd < -4096) + measure->total_round++; + measure->total_angle = measure->total_round * 360 + measure->angle_single_round; } // 电机初始化,返回一个电机实例 @@ -174,6 +167,7 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) // register motor to CAN bus config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback + config->can_init_config.id=dji_motor_info[idx]; //set id,eq to address(it is identity) dji_motor_info[idx]->motor_can_instance = CANRegister(&config->can_init_config); DJIMotorEnable(dji_motor_info[idx]); diff --git a/modules/motor/dji_motor.h b/modules/motor/dji_motor.h index e155d43..5dde915 100644 --- a/modules/motor/dji_motor.h +++ b/modules/motor/dji_motor.h @@ -22,8 +22,8 @@ #define DJI_MOTOR_CNT 12 /* 滤波系数设置为1的时候即关闭滤波 */ -#define SPEED_SMOOTH_COEF 0.9f // better to be greater than 0.85 -#define CURRENT_SMOOTH_COEF 0.98f // this coef *must* be greater than 0.95 +#define SPEED_SMOOTH_COEF 0.85f // better to be greater than 0.85 +#define CURRENT_SMOOTH_COEF 0.9f // this coef *must* be greater than 0.9 #define ECD_ANGLE_COEF 0.043945f // 360/8192,将编码器值转化为角度制 /* DJI电机CAN反馈信息*/ diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index 9af9d9d..9960294 100644 --- a/modules/remote/remote_control.c +++ b/modules/remote/remote_control.c @@ -9,22 +9,19 @@ static RC_ctrl_t rc_ctrl[2]; //[0]:当前数据,[1]:上一次的数据.用于按 // 遥控器拥有的串口实例 static USARTInstance *rc_usart_instance; - /** * @brief 矫正遥控器摇杆的值 - * + * */ static void RectifyRCjoystick() { for (uint8_t i = 0; i < 5; i++) { - if(rc_ctrl[TEMP].rc.joystick[i]>660 || rc_ctrl[TEMP].rc.joystick[i]<-660) - rc_ctrl[TEMP].rc.joystick[i]=0; + if (*(&rc_ctrl[TEMP].rc.rocker_l_+i) > 660 || *(&rc_ctrl[TEMP].rc.rocker_l_+i) < -660) + *(&rc_ctrl[TEMP].rc.rocker_l_+i) = 0; } - } - /** * @brief remote control protocol resolution * @param[in] sbus_buf: raw data point @@ -35,15 +32,15 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf) { memcpy(&rc_ctrl[1], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据 // 摇杆,直接解算时减去偏置 - rc_ctrl[TEMP].rc.joystick[0] = ((sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 0 - rc_ctrl[TEMP].rc.joystick[1] = (((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 1 - rc_ctrl[TEMP].rc.joystick[2] = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 2 - rc_ctrl[TEMP].rc.joystick[3] = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 3 - rc_ctrl[TEMP].rc.joystick[4] = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF)- RC_CH_VALUE_OFFSET; // 左侧拨轮 + rc_ctrl[TEMP].rc.rocker_r_ = ((sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 0 + rc_ctrl[TEMP].rc.rocker_r1 = (((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 1 + rc_ctrl[TEMP].rc.rocker_l_ = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 2 + rc_ctrl[TEMP].rc.rocker_l1 = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 3 + rc_ctrl[TEMP].rc.dial = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF) - RC_CH_VALUE_OFFSET; // 左侧拨轮 RectifyRCjoystick(); // 开关,0左1右 - rc_ctrl[TEMP].rc.s[0] = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left - rc_ctrl[TEMP].rc.s[1] = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right + rc_ctrl[TEMP].rc.switch_right = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left + rc_ctrl[TEMP].rc.switch_left = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right // 鼠标解析 rc_ctrl[TEMP].mouse.x = sbus_buf[6] | (sbus_buf[7] << 8); //!< Mouse X axis @@ -98,5 +95,5 @@ RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle) conf.usart_handle = rc_usart_handle; conf.recv_buff_size = REMOTE_CONTROL_FRAME_SIZE; rc_usart_instance = USARTRegister(&conf); - return (RC_ctrl_t*)&rc_ctrl; + return (RC_ctrl_t *)&rc_ctrl; } \ No newline at end of file diff --git a/modules/remote/remote_control.h b/modules/remote/remote_control.h index 1b5b3cb..f655316 100644 --- a/modules/remote/remote_control.h +++ b/modules/remote/remote_control.h @@ -17,7 +17,7 @@ #include "main.h" #include "usart.h" -// +// #define LAST 1 #define TEMP 0 @@ -27,7 +27,7 @@ #define KEY_PRESS_WITH_CTRL 2 #define KEY_PRESS_WITH_SHIFT 3 -// 检查接受值是否出错 +// 检查接收值是否出错 #define RC_CH_VALUE_MIN ((uint16_t)364) #define RC_CH_VALUE_OFFSET ((uint16_t)1024) #define RC_CH_VALUE_MAX ((uint16_t)1684) @@ -87,8 +87,14 @@ typedef struct { struct { - int16_t joystick[5]; // 右|0 ,右-1 ,左-2 ,左|3 ,拨轮4 - uint8_t s[2]; //[0]:left [1]:right + int16_t rocker_l_; // 左水平 + int16_t rocker_l1; // 左竖直 + int16_t rocker_r_; // 右水平 + int16_t rocker_r1; // 右竖直 + int16_t dial; // 侧边拨轮 + + uint8_t switch_left; // 左侧开关 + uint8_t switch_right; // 右侧开关 } rc; struct { @@ -101,12 +107,10 @@ typedef struct uint16_t key_temp; uint8_t key[4][16]; // 当前使用的键盘索引 - Key_t key_test[4]; // 改为位域后的键盘索引,空间减少8倍,速度增加16~倍 + // Key_t key_test[4]; // 改为位域后的键盘索引,空间减少8倍,速度增加16~倍 } RC_ctrl_t; - - /* ------------------------- Internal Data ----------------------------------- */ /**