LYL/R2_NEW
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修改一点bug，开自动，还会跑等等

Browse Source
This commit is contained in:
ZHAISHUI04 2025-04-28 22:23:08 +08:00
parent 5ff6e56d70
commit 3b24dc2760
12 changed files with 230 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
MDK-ARM/R2.uvoptx
View File

@ -160,6 +160,41 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>LD_raw,0x0A</ItemText> <ItemText>LD_raw,0x0A</ItemText>
</Ww> </Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>can,0x0A</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_rc,0x0A</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>chassis</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>nuc_raw,0x0A</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>nucbuf</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
@ -387,7 +422,7 @@
<Group> <Group>
<GroupName>Drivers/STM32F4xx_HAL_Driver</GroupName> <GroupName>Drivers/STM32F4xx_HAL_Driver</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -659,7 +694,7 @@
<Group> <Group>
<GroupName>Drivers/CMSIS</GroupName> <GroupName>Drivers/CMSIS</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -679,7 +714,7 @@
<Group> <Group>
<GroupName>Middlewares/FreeRTOS</GroupName> <GroupName>Middlewares/FreeRTOS</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>

BIN
MDK-ARM/R2/R2.axf
View File

Binary file not shown.

23
User/Module/Chassis.c
View File

@ -99,9 +99,9 @@ int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算 void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算
{ {
c->hopemotorout.OmniSpeedOut[3] = -Vx+Vy+Vw;//右前 c->hopemotorout.OmniSpeedOut[3] = -Vx+Vy+Vw;//右前
c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw;//右后 c->hopemotorout.OmniSpeedOut[0] = -Vx-Vy+Vw;//右后
c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后 c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后
c->hopemotorout.OmniSpeedOut[0] = Vx+Vy+Vw;//左前 c->hopemotorout.OmniSpeedOut[1] = Vx+Vy+Vw;//左前
} }
//bool isArrive(fp32 real_pos,fp32 target_pos,fp32 mistake) //bool isArrive(fp32 real_pos,fp32 target_pos,fp32 mistake)
@ -131,11 +131,23 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
/* /*
在cmd里对数据进行处理 包括方向和油门 在cmd里对数据进行处理 包括方向和油门
6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同*/ 6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同*/
if(c->chassis_ctrl .mode != Pitch){ if(c->chassis_ctrl .mode == Pitch){
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
}
else if(c->chassis_ctrl .mode == UP_Adjust)
{
c->move_vec.Vw = ctrl->Vw*6000;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
}
else {
c->move_vec.Vw = ctrl->Vw*6000; c->move_vec.Vw = ctrl->Vw*6000;
c->move_vec.Vx = ctrl->Vy*6000; c->move_vec.Vx = ctrl->Vy*6000;
c->move_vec.Vy = ctrl->Vx*6000; c->move_vec.Vy = ctrl->Vx*6000;
} }
break; break;
@ -174,8 +186,7 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
break; break;
case AUTO_PICK: //自动视觉
case AUTO_PICK: //自动视觉
c->move_vec.Vx =ctrl->Vx*6000 ; c->move_vec.Vx =ctrl->Vx*6000 ;
c->move_vec.Vy =ctrl->Vy *6000; c->move_vec.Vy =ctrl->Vy *6000;
@ -208,7 +219,9 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
break ; break ;
} }
break ;
} }

8
User/Module/config.c
View File

@ -76,11 +76,11 @@ static const ConfigParam_t param ={
}, },
/*投球*/ /*投球*/
.PitchConfig_Config = { .PitchConfig_Config = {
.m2006_init_angle =-150, .m2006_init_angle =-170,
.m2006_trigger_angle =0, .m2006_trigger_angle =0,
.go1_init_position = 0, .go1_init_position = -50,
.go1_release_threshold =0, .go1_release_threshold =-210,
.m2006_Screw_init=0 .m2006_Screw_init=0,
}, },

47
User/Module/up.c
View File

@ -56,10 +56,12 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
u->motorfeedback .VESC_5065_M1_rpm =can ->motor .chassis5065 .as_array [0].rotor_speed ; u->motorfeedback .VESC.VESC_5065_M1_rpm =can ->motor .chassis5065 .as_array [0].rotor_speed ;
u->motorfeedback .VESC_5065_M2_rpm =can ->motor .chassis5065 .as_array [1].rotor_speed ; u->motorfeedback .VESC.VESC_5065_M2_rpm =can ->motor .chassis5065 .as_array [1].rotor_speed ;
u->motorfeedback .Encoder .angle = can ->Oid.angle ;
u->motorfeedback .Encoder .ecd = can ->Oid.ecd ;
for(int i=0;i<4;i++){ for(int i=0;i<4;i++){
u->motorfeedback .DJmotor_feedback[i].rpm =can ->motor .up_motor3508 .as_array [i].rotor_speed ; u->motorfeedback .DJmotor_feedback[i].rpm =can ->motor .up_motor3508 .as_array [i].rotor_speed ;
u->motorfeedback .DJmotor_feedback[i].ecd =can ->motor .up_motor3508.as_array [i].rotor_ecd ; u->motorfeedback .DJmotor_feedback[i].ecd =can ->motor .up_motor3508.as_array [i].rotor_ecd ;
@ -231,7 +233,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
// //
break; break;
case Pitch : case Pitch :
if (u->PitchContext .PitchState ==PITCH_PREPARE) if (u->PitchContext .PitchState ==PITCH_PREPARE)
{ {
@ -241,7 +243,13 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
Pitch_Process(u,out,c); Pitch_Process(u,out,c);
break ; break ;
case UP_Adjust:
u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
u->PitchContext.PitchConfig.m2006_Screw_init += c->Vy;
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
u->motor_target .Pitch_M2006_angle=u->PitchContext.PitchConfig.m2006_Screw_init;
break ;
case Dribble: case Dribble:
{ {
/*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收 */ /*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收 */
@ -287,14 +295,10 @@ return 0;
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c) int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
{ {
static fp32 go1_position,M2006_Screw_position ;
static int is_initialize=1; // go1_position=u->PitchContext .PitchConfig .go1_init_position ;
if(is_initialize) // M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init;
{
go1_position=u->PitchContext .PitchConfig .go1_init_position ;
M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init;
is_initialize=0;
}
switch(u->PitchContext .PitchState){ switch(u->PitchContext .PitchState){
@ -313,12 +317,10 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
if(u->motorfeedback .DJmotor_feedback [0].total_angle >-1) //当2006的总角度小于1，可以认为已经勾上,误差为1 if(u->motorfeedback .DJmotor_feedback [0].total_angle >-1) //当2006的总角度小于1，可以认为已经勾上,误差为1
{ {
u->motor_target .go_shoot = go1_position;//go下拉 u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
u->motor_target .Pitch_M2006_angle = M2006_Screw_position;//丝杠上的2006 // u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
go1_position = go1_position + c->Vx ; // u->PitchContext.PitchConfig.m2006_Screw_init += c->Vy;
M2006_Screw_position=M2006_Screw_position+ c->Vy;
} }
break ; break ;
@ -336,6 +338,13 @@ return 0;
int8_t Pitch_Adjust(UP_t *u, CAN_Output_t *out,CMD_t *c)
{
u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
u->PitchContext.PitchConfig.m2006_Screw_init += c->Vy;
}

14
User/Module/up.h
View File

@ -157,15 +157,23 @@ typedef struct{
CMD_t *cmd; CMD_t *cmd;
struct{ struct{
struct{
fp32 VESC_5065_M1_rpm; fp32 VESC_5065_M1_rpm;
fp32 VESC_5065_M2_rpm; fp32 VESC_5065_M2_rpm;
}VESC;
GO_MotorData_t *go_data;//存放go电机数据 GO_MotorData_t *go_data;//存放go电机数据
DJmotor_feedback_t DJmotor_feedback[4]; DJmotor_feedback_t DJmotor_feedback[4];
struct {
uint32_t ecd;
float angle;
}Encoder;
}motorfeedback; }motorfeedback;

6
User/device/can_use.c
View File

@ -73,8 +73,8 @@ void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
switch(raw[2])//判断指令id switch(raw[2])//判断指令id
{ {
case 0x01: case 0x01:
feedback->ecd =raw[3]|raw[4]<<8|raw[5]<<16|raw[6]<<24;//接收编码器值 feedback->ecd =raw[3]|raw[4]<<8|raw[5]<<16|raw[6]<<24;
feedback->angle=feedback->ecd*360/CAN_ENCODER_RESOLUTION; feedback->angle=feedback->ecd*360/CAN_ENCODER_RESOLUTION-151.0f;
break; break;
} }
} }
@ -396,7 +396,7 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
// 存储消息到对应的电机结构体中 // 存储消息到对应的电机结构体中
index = can_rx->rx_header.StdId - CAN_CHASSIS_M3508_M1_ID; index = can_rx->rx_header.StdId - CAN_CHASSIS_M3508_M1_ID;
can->recive_flag |= 1 << (index); can->recive_flag |= 1 << (index);
CAN_DJIMotor_Decode(&(can->motor.chassis5065.as_array[index]), can_rx->rx_data); CAN_DJIMotor_Decode(&(can->motor.chassis_motor3508.as_array[index]), can_rx->rx_data);
detect_hook(CHASSIS3508M1_TOE + index); detect_hook(CHASSIS3508M1_TOE + index);
break; break;

59
User/device/cmd.c
View File

@ -61,17 +61,25 @@ static void CMD_RcLostLogic(CMD_t *cmd){
/* 机器人底盘运行模式恢复至放松模式 */ /* 机器人底盘运行模式恢复至放松模式 */
cmd->CMD_CtrlType = RELAXED; cmd->CMD_CtrlType = RELAXED;
} }
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){ int8_t CMD_ParseRc(CMD_t *cmd,const CMD_RC_t *rc){
if (cmd == NULL) return -1; if (cmd == NULL) return -1;
if (rc == NULL) return -1; if (rc == NULL) return -1;
/*c当rc丢控时，恢复机器人至默认状态 */ switch (rc->rc_type){
if ((rc->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.sw_r == CMD_SW_ERR)) {
CMD_RcLostLogic(cmd); case RC_DR16:
} CMD_DR16(cmd,rc);
else { break ;
CMD_RcLogic(rc, cmd);
} case RC_LD:
CMD_LD(cmd,rc);
break ;
case Control_loss:
CMD_RcLostLogic(cmd);
break ;
}
return 0; return 0;
} }
@ -111,8 +119,8 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
/*遥控器，上下层通用，按键控制，统一到cmd*/ /*DR16遥控器数据分配*/
int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) { int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
if(cmd == NULL) return -1; if(cmd == NULL) return -1;
@ -162,3 +170,34 @@ int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) {
return 0; return 0;
} }
int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
if(cmd == NULL) return -1;
cmd->Vx = rc->LD.ch_r_x;
cmd->Vy = rc->LD.ch_r_y;
cmd->Vw = rc->LD.ch_l_x;
cmd->poscamear = rc->LD.ch_l_y;
/*自动手动切换*/
if(rc->LD.key_A ==CMD_SW_DOWN ) cmd ->CMD_CtrlType =RCcontrol;
else if(rc->LD.key_A ==CMD_SW_UP) cmd ->CMD_CtrlType =AUTO;
if(cmd ->CMD_CtrlType ==AUTO){
/*自动下的*/
if(rc->LD.key_E ==CMD_SW_DOWN ) cmd ->CMD_mode =AUTO_NAVI;
else if(rc->LD.key_E ==CMD_SW_UP) cmd ->CMD_mode =AUTO_PICK;
else cmd ->CMD_mode =Normal ;
}
else if(cmd ->CMD_CtrlType ==RCcontrol){
/*手动下的*/
if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode =UP_Adjust;
else if(rc->LD.key_G ==CMD_SW_UP) cmd ->CMD_mode =Pitch;
else cmd ->CMD_mode =Normal;
}
}

26
User/device/cmd.h
View File

@ -21,8 +21,11 @@ typedef enum{
Normal, //无模式 Normal, //无模式
AUTO_NAVI, AUTO_NAVI,
AUTO_PICK, AUTO_PICK,
UP_Adjust,//上层调整
Dribble , //运球 Dribble , //运球
Pitch, //投篮,底盘锁定 Pitch, //投篮,底盘锁定
@ -57,6 +60,8 @@ typedef enum {
CMD_SW_MID = 3, CMD_SW_MID = 3,
CMD_SW_DOWN = 2, CMD_SW_DOWN = 2,
} CMD_SwitchPos_t; } CMD_SwitchPos_t;
typedef enum{ typedef enum{
RC_DR16, RC_DR16,
RC_LD , RC_LD ,
@ -87,14 +92,14 @@ typedef struct {
fp32 ch_r_y ; fp32 ch_r_y ;
fp32 ch_l_x ; fp32 ch_l_x ;
fp32 ch_l_y ; fp32 ch_l_y ;
int16_t key_A ; CMD_SwitchPos_t key_A ;
// int16_t key_B ;//无 // int16_t key_B ;//<EFBFBD><EFBFBD>
int16_t key_C ; CMD_SwitchPos_t key_C ;
// int16_t key_D ;//无 // int16_t key_D ;//<EFBFBD><EFBFBD>
int16_t key_E ; CMD_SwitchPos_t key_E ;
int16_t key_F; CMD_SwitchPos_t key_F;
int16_t key_G ; CMD_SwitchPos_t key_G ;
int16_t key_H ; CMD_SwitchPos_t key_H ;
int16_t knob_left;//左旋钮 int16_t knob_left;//左旋钮
int16_t knob_right;//右旋钮 int16_t knob_right;//右旋钮
@ -148,10 +153,11 @@ int8_t CMD_Init(CMD_t *cmd);
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) ; static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) ;
static void CMD_RcLostLogic(CMD_t *cmd); static void CMD_RcLostLogic(CMD_t *cmd);
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc); int8_t CMD_ParseRc(CMD_t *cmd,const CMD_RC_t *rc);
int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n); int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n);
int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) ; int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) ;
int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc);
#endif #endif

80
User/device/rc.c
View File

@ -17,8 +17,8 @@
#ifdef DR16 #ifdef DR16
#define FRAME_LEN 36 #define FRAME_LEN 36
#endif
#elif defined(LD_t) #ifdef LD_t
#define FRAME_LEN 25 #define FRAME_LEN 25
@ -30,7 +30,7 @@
#define RC_CH_VALUE_MAX ((uint16_t)1684) #define RC_CH_VALUE_MAX ((uint16_t)1684)
static osThreadId_t thread_alert; static osThreadId_t thread_alert;
static uint8_t cbuf[FRAME_LEN]; //此处设置为两帧字节的长度 若为一帧会出现乱码的情况 uint8_t cbuf[FRAME_LEN]; //此处设置为两帧字节的长度 若为一帧会出现乱码的情况
/*通用，初始化，串口回调注册，dma数据接收*/ /*通用，初始化，串口回调注册，dma数据接收*/
int8_t RC_SBUS_Init(void ) int8_t RC_SBUS_Init(void )
@ -39,6 +39,8 @@ int8_t RC_SBUS_Init(void )
BSP_UART_RegisterCallback(BSP_UART_REMOTE, BSP_UART_IDLE_LINE_CB, BSP_UART_RegisterCallback(BSP_UART_REMOTE, BSP_UART_IDLE_LINE_CB,
RC_SBUS_RxCpltCallback); RC_SBUS_RxCpltCallback);
memset(cbuf, 0, sizeof(cbuf));//初始化清空数据包
return DEVICE_OK; return DEVICE_OK;
} }
@ -98,40 +100,48 @@ int8_t LD_ParseRaw( LD_raw_t *raw, LD_Data_t *LD)
raw->map_ch[0]=map_fp32(raw->ch[0],-719,680,-1,1); raw->map_ch[0]=map_fp32(raw->ch[0],-719,680,-1,1);
raw->map_ch[1]=map_fp32(raw->ch[1],-567,832,-1,1); raw->map_ch[1]=map_fp32(raw->ch[1],-653,746,-1,1);
raw->map_ch[2]=map_fp32(raw->ch[2],95,1482,0,1); raw->map_ch[2]=map_fp32(raw->ch[2],95,1482,0,1);
raw->map_ch[3]=map_fp32(raw->ch[3],-317,375,-1,1); raw->map_ch[3]=map_fp32(raw->ch[3],-317,375,-1,1);
/*非线性映射*/ /*非线性映射*/
raw->map_ch[0]=expo_map(raw->map_ch[0], 0.7f); raw->map_ch[0]=expo_map(raw->map_ch[0],0.7f);
raw->map_ch[1]=expo_map(raw->map_ch[1],0.7f); raw->map_ch[1]=expo_map(raw->map_ch[1],0.7f);
raw->map_ch[2]=expo_map(raw->map_ch[2],0.7f); raw->map_ch[2]=expo_map(raw->map_ch[2],0.7f);
raw->map_ch[3]=expo_map(raw->map_ch[3],0.7f); raw->map_ch[3]=expo_map(raw->map_ch[3],0.7f);
//死区(-30,30) //死区(-30,30)
if(raw->map_ch[0]>-0.05f&&raw->map_ch[0]<0.05f) raw->map_ch[0]=0; if(raw->map_ch[0]>-0.002f&&raw->map_ch[0]<0.002f) raw->map_ch[0]=0;
if(raw->map_ch[1]>-0.05f&&raw->map_ch[1]<0.05f) raw->map_ch[1]=0; if(raw->map_ch[1]>-0.002f&&raw->map_ch[1]<0.002f) raw->map_ch[1]=0;
if(raw->map_ch[2]>-0.05f&&raw->map_ch[2]<0.05f) raw->map_ch[2]=0; if(raw->map_ch[2]>-0.002f&&raw->map_ch[2]<0.002f) raw->map_ch[2]=0;
if(raw->map_ch[3]>-0.05f&&raw->map_ch[3]<0.05f) raw->map_ch[3]=0; if(raw->map_ch[3]>-0.002f&&raw->map_ch[3]<0.002f) raw->map_ch[3]=0;
/*dr16采用的是复制内存，这里因为内存顺序不同没用*/ /*dr16采用的是复制内存，这里因为内存顺序不同没用*/
LD->ch_l_x= raw->map_ch[3]; LD->ch_l_x= raw->map_ch[3];
LD->ch_l_y =raw->map_ch[2]; LD->ch_l_y =raw->map_ch[2];
LD->ch_r_x= raw->map_ch[0]; LD->ch_r_x= raw->map_ch[0];
LD->ch_r_y =raw->map_ch[1]; LD->ch_r_y =raw->map_ch[1];
LD->key_A =raw->sw[0];
// LD->key_B =rc_ctrl->sw[0] // LD->key_A =raw->sw[0];
LD->key_C =raw->sw[2]; // // LD->key_B =rc_ctrl->sw[0]
// LD->key_D =rc_ctrl->sw[0] // LD->key_C =raw->sw[2];
LD->key_E =raw->sw[5]; // // LD->key_D =rc_ctrl->sw[0]
LD->key_F =raw->sw[3]; // LD->key_E =raw->sw[5];
LD->key_G =raw->sw[7]; // LD->key_F =raw->sw[3];
LD->key_H =raw->sw[4]; // LD->key_G =raw->sw[7];
LD->knob_left=raw->sw[1]; // LD->key_H =raw->sw[4];
LD->knob_right =raw->sw[6]; // LD->knob_left=raw->sw[1];
// LD->knob_right =raw->sw[6];
LD->key_A = MapSwitchState(raw->sw[0]);
LD->key_C = MapSwitchState(raw->sw[2]);
LD->key_E = MapSwitchState(raw->sw[5]);
LD->key_F = MapSwitchState(raw->sw[3]);
LD->key_G = MapSwitchState(raw->sw[7]);
LD->key_H = MapSwitchState(raw->sw[4]);
LD->knob_left = MapSwitchState(raw->sw[1]);
LD->knob_right = MapSwitchState(raw->sw[6]);
return 1; return 1;
@ -195,8 +205,8 @@ int8_t RC_ParseRC( DR16_t *dr16,LD_raw_t *LD_raw, LD_Data_t *LD, CMD_RC_t *rc) {
rc->dr16.ch_l_x = 2 * ((float)dr16->data.ch_l_x - RC_CH_VALUE_MID) / full_range; rc->dr16.ch_l_x = 2 * ((float)dr16->data.ch_l_x - RC_CH_VALUE_MID) / full_range;
rc->dr16.ch_l_y = 2 * ((float)dr16->data.ch_l_y - RC_CH_VALUE_MID) / full_range; rc->dr16.ch_l_y = 2 * ((float)dr16->data.ch_l_y - RC_CH_VALUE_MID) / full_range;
rc->dr16.sw_l = (CMD_SwitchPos_t)dr16->data.sw_l; rc->dr16.sw_l = (CMD_SwitchPos_DR16_t)dr16->data.sw_l;
rc->dr16.sw_r = (CMD_SwitchPos_t)dr16->data.sw_r; rc->dr16.sw_r = (CMD_SwitchPos_DR16_t)dr16->data.sw_r;
rc->dr16.key = dr16->data.key; rc->dr16.key = dr16->data.key;
@ -211,10 +221,10 @@ int8_t RC_ParseRC( DR16_t *dr16,LD_raw_t *LD_raw, LD_Data_t *LD, CMD_RC_t *rc) {
memcpy(&rc->LD, LD, sizeof(LD_Data_t)); memcpy(&rc->LD, LD, sizeof(LD_Data_t));
/*离线处理，和dr16位置不同*/ /*离线处理，和dr16位置不同*/
if(LD_raw->sw[7] != 393) if(LD_raw->sw[7] == 300||LD_raw->sw[3]!=1694)
{ {
LD_HandleOffline(LD,rc); LD_HandleOffline(LD,rc);
memset(cbuf, 0, sizeof(cbuf)); //有时候会出现消息数组错位，所以直接清空，在离线和指定按键不对的情况下，原数据不可信 // memset(cbuf, 0, sizeof(cbuf)); //有时候会出现消息数组错位，所以直接清空，在离线和指定按键不对的情况下，原数据不可信
} }
#endif #endif
return DEVICE_OK; return DEVICE_OK;
@ -230,6 +240,7 @@ int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc) {
(void)dr16; (void)dr16;
memset(&rc->dr16 , 0, sizeof(DR16_t)); memset(&rc->dr16 , 0, sizeof(DR16_t));
return 0; return 0;
} }
int8_t LD_HandleOffline(const LD_Data_t *LD, CMD_RC_t *rc) { int8_t LD_HandleOffline(const LD_Data_t *LD, CMD_RC_t *rc) {
@ -240,4 +251,13 @@ int8_t LD_HandleOffline(const LD_Data_t *LD, CMD_RC_t *rc) {
memset(&rc->LD , 0, sizeof(LD_Data_t)); memset(&rc->LD , 0, sizeof(LD_Data_t));
return 0; return 0;
}
/*乐迪用的按键封装 */
CMD_SwitchPos_t MapSwitchState(int16_t value) {
return (value > 300 && value < 500) ? CMD_SW_DOWN :
(value >= 500 && value < 1500) ? CMD_SW_MID :
(value >= 1500 && value < 1700) ? CMD_SW_UP : CMD_SW_ERR;
} }

23
User/device/rc.h
View File

@ -32,7 +32,7 @@ typedef struct {
DR16_Data_t data; DR16_Data_t data;
} DR16_t; } DR16_t;
/*原始数据*/ /*ԭʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
typedef __packed struct typedef __packed struct
{ {
fp32 map_ch[4]; fp32 map_ch[4];
@ -48,17 +48,17 @@ typedef __packed struct
fp32 ch_r_y ; fp32 ch_r_y ;
fp32 ch_l_x ; fp32 ch_l_x ;
fp32 ch_l_y ; fp32 ch_l_y ;
int16_t key_A ; CMD_SwitchPos_t key_A ;
// int16_t key_B ;//无 // int16_t key_B ;//<EFBFBD><EFBFBD>
int16_t key_C ; CMD_SwitchPos_t key_C ;
// int16_t key_D ;//无 // int16_t key_D ;//<EFBFBD><EFBFBD>
int16_t key_E ; CMD_SwitchPos_t key_E ;
int16_t key_F; CMD_SwitchPos_t key_F;
int16_t key_G ; CMD_SwitchPos_t key_G ;
int16_t key_H ; CMD_SwitchPos_t key_H ;
int16_t knob_left;//左旋钮 int16_t knob_left;//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ť
int16_t knob_right;//右旋钮 int16_t knob_right;//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ť
} LD_Data_t; } LD_Data_t;
@ -79,6 +79,7 @@ int8_t DR16_ParseRaw(DR16_t *dr16);
int8_t RC_ParseRC( DR16_t *dr16,LD_raw_t *LD_raw, LD_Data_t *LD, CMD_RC_t *rc) ; int8_t RC_ParseRC( DR16_t *dr16,LD_raw_t *LD_raw, LD_Data_t *LD, CMD_RC_t *rc) ;
int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc) ; int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc) ;
int8_t LD_HandleOffline(const LD_Data_t *LD, CMD_RC_t *rc) ; int8_t LD_HandleOffline(const LD_Data_t *LD, CMD_RC_t *rc) ;
CMD_SwitchPos_t MapSwitchState(int16_t value) ;
#endif #endif

2
User/task/cmd_task.c
View File

@ -39,7 +39,6 @@ void Task_cmd(void *argument){
/*将各任务接收到的原始数据解析为通用的控制命令*/ /*将各任务接收到的原始数据解析为通用的控制命令*/
/*注意，不能将nuc和码盘导航一块使用*/ /*注意，不能将nuc和码盘导航一块使用*/
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK) //nuc if(osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK) //nuc
@ -54,6 +53,7 @@ void Task_cmd(void *argument){
/*将需要与其他任务共享的数据放到消息队列里 此处主要分享给底盘 后续会添加和上层机构的通信 */ /*将需要与其他任务共享的数据放到消息队列里 此处主要分享给底盘 后续会添加和上层机构的通信 */
osMessageQueueReset(task_runtime.msgq.cmd.CHASSIS_cmd_ctrl_t); osMessageQueueReset(task_runtime.msgq.cmd.CHASSIS_cmd_ctrl_t);
osMessageQueuePut(task_runtime.msgq.cmd.CHASSIS_cmd_ctrl_t,&cmd,0,0); osMessageQueuePut(task_runtime.msgq.cmd.CHASSIS_cmd_ctrl_t,&cmd,0,0);
osMessageQueueReset(task_runtime.msgq.cmd.UP_cmd_ctrl_t); osMessageQueueReset(task_runtime.msgq.cmd.UP_cmd_ctrl_t);
osMessageQueuePut(task_runtime.msgq.cmd.UP_cmd_ctrl_t,&cmd,0,0); osMessageQueuePut(task_runtime.msgq.cmd.UP_cmd_ctrl_t,&cmd,0,0);