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

Compare commits

merge into: LYL:main
Branches Tags
LYL:main
LYL:Cooperation
LYL:chassis
...
pull from: LYL:chassis
Branches Tags
LYL:Cooperation
LYL:chassis
LYL:main

4 Commits
main ... chassis

Author SHA1 Message Date
ZHAISHUI04
2e0c364913 整体代码 2025-06-04 01:00:21 +08:00
ZHAISHUI04
0c35d85d44 Merge branch 'chassis' of ssh://gitea.qutrobot.top:222/LYL/R2_NEW into chassis 2025-05-31 15:30:16 +08:00
ZHAISHUI04
64cdeb7ae1 把上层也改了，避免新机构影响 2025-05-31 15:30:03 +08:00
ZHAISHUI04
0d7a931faf 本分支为底盘更改，更改后合并到主分支 2025-05-24 17:46:18 +08:00
13 changed files with 201 additions and 291 deletions
Show Stats Expand all files Collapse all files

1
Core/Src/main.c
View File

@ -80,6 +80,7 @@ int main(void)
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */

122
MDK-ARM/R2.uvoptx
View File

@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf>
<nTsel>6</nTsel>
<nTsel>3</nTsel>
<sDll></sDll>
<sDllPa></sDllPa>
<sDlgDll></sDlgDll>
@ -114,7 +114,7 @@
<tDlgDll></tDlgDll>
<tDlgPa></tDlgPa>
<tIfile></tIfile>
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
<pMon>BIN\CMSIS_AGDI.dll</pMon>
</DebugOpt>
<TargetDriverDllRegistry>
<SetRegEntry>
@ -140,7 +140,7 @@
<SetRegEntry>
<Number>0</Number>
<Key>DLGUARM</Key>
<Name>(105=-1,-1,-1,-1,0)</Name>
<Name></Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
@ -158,27 +158,27 @@
<Ww>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>rc_ctrl,0x0A</ItemText>
<ItemText>Nor_Vx</ItemText>
</Ww>
<Ww>
<count>1</count>
<WinNumber>1</WinNumber>
<ItemText>LD_raw,0x0A</ItemText>
<ItemText>Nor_Vy</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>can,0x0A</ItemText>
<ItemText>count,0x0A</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText>
<ItemText>count</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_rc,0x0A</ItemText>
<ItemText>NUC_StartSending,0x0A</ItemText>
</Ww>
<Ww>
<count>5</count>
@ -188,127 +188,37 @@
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>nuc_raw,0x0A</ItemText>
<ItemText>PIAN</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>nucbuf</ItemText>
<ItemText>shang_yaw</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc</ItemText>
<ItemText>pian_yaw</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>up_cmd,0x0A</ItemText>
<ItemText>pid,0x0A</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>can_out</ItemText>
<ItemText>gyro_kp</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>Nor_Vx</ItemText>
<ItemText>rc_ctrl,0x0A</ItemText>
</Ww>
<Ww>
<count>12</count>
<count>38</count>
<WinNumber>1</WinNumber>
<ItemText>Nor_Vy</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>b</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>count,0x0A</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>count</ItemText>
</Ww>
<Ww>
<count>16</count>
<WinNumber>1</WinNumber>
<ItemText>raw_rx1,0x0A</ItemText>
</Ww>
<Ww>
<count>17</count>
<WinNumber>1</WinNumber>
<ItemText>gyro_kp,0x0A</ItemText>
</Ww>
<Ww>
<count>18</count>
<WinNumber>1</WinNumber>
<ItemText>nucbuf,0x0A</ItemText>
</Ww>
<Ww>
<count>19</count>
<WinNumber>1</WinNumber>
<ItemText>vofa_send,0x0A</ItemText>
</Ww>
<Ww>
<count>20</count>
<WinNumber>1</WinNumber>
<ItemText>SendBuffer,0x10</ItemText>
</Ww>
<Ww>
<count>21</count>
<WinNumber>1</WinNumber>
<ItemText>NUC_StartSending,0x0A</ItemText>
</Ww>
<Ww>
<count>22</count>
<WinNumber>1</WinNumber>
<ItemText>PIAN</ItemText>
</Ww>
<Ww>
<count>23</count>
<WinNumber>1</WinNumber>
<ItemText>pid</ItemText>
</Ww>
<Ww>
<count>24</count>
<WinNumber>1</WinNumber>
<ItemText>bmi088</ItemText>
</Ww>
<Ww>
<count>25</count>
<WinNumber>1</WinNumber>
<ItemText>ist8310</ItemText>
</Ww>
<Ww>
<count>26</count>
<WinNumber>1</WinNumber>
<ItemText>imu_eulr,0x0A</ItemText>
</Ww>
<Ww>
<count>27</count>
<WinNumber>1</WinNumber>
<ItemText>imu_temp,0x0A</ItemText>
</Ww>
<Ww>
<count>28</count>
<WinNumber>1</WinNumber>
<ItemText>htim10,0x0A</ItemText>
</Ww>
<Ww>
<count>29</count>
<WinNumber>1</WinNumber>
<ItemText>pulse</ItemText>
</Ww>
<Ww>
<count>30</count>
<WinNumber>1</WinNumber>
<ItemText>imu_temp</ItemText>
<ItemText>cmd_fromnuc</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>

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

Binary file not shown.

171
User/Module/Chassis.c
View File

@ -83,6 +83,9 @@ int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
fp32 gyro_kp=1.0f;
fp32 PIAN=0;
fp32 shang_yaw=0;
fp32 pian_yaw=0;
void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算,
{
fp64 Nor_Vx,Nor_Vy;//归一化后的数据
@ -92,23 +95,41 @@ void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆
// c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw;//右后
// c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后
// c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw;//左前
// c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//右前
// c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//右后
// c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//左后
// c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//左前
c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw ;//右前
if(Vw)
{
c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw ;//右前
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw ;//右后
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw ;//左后
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw ;//左前
// if(!Vw){
// PIAN = jiuzheng(c->pos088 .imu_eulr .yaw );
}
else {
c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z*gyro_kp;//右前
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z*gyro_kp;//右后
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z*gyro_kp;//左后
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z*gyro_kp;//左前
}
//
// if(Vx || Vy){
// PIAN = jiuzheng(c->pos088 .imu_eulr .yaw );
// c->hopemotorout.OmniSpeedOut[0] -= PIAN ;//右前
// c->hopemotorout.OmniSpeedOut[1] -= PIAN ;//右后
// c->hopemotorout.OmniSpeedOut[2] -= PIAN ;//左后
// c->hopemotorout.OmniSpeedOut[3] -= PIAN ;//左前
// }
// else
// {
// PIAN = 0;
// shang_yaw=c->pos088 .imu_eulr .yaw ;
// pian_yaw=0;
// }
// c->hopemotorout.OmniSpeedOut[0] += PIAN ;//右前
// c->hopemotorout.OmniSpeedOut[1] += PIAN ;//右后
// c->hopemotorout.OmniSpeedOut[2] += PIAN ;//左后
// c->hopemotorout.OmniSpeedOut[3] += PIAN ;//左前
//
c->vofa_send[0]=c->pos088.bmi088.gyro.z;
c->vofa_send[1]=PIAN;
c->vofa_send[2]=c->pos088.imu_eulr.yaw;
// c->vofa_send[3]=
// c->vofa_send[2]=c->motorfeedback .rotor_rpm3508 [1];
}
@ -151,87 +172,57 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
break;
case AUTO : //在自动模式下
switch(c->chassis_ctrl.mode ){
case AUTO: // 自动模式
switch (c->chassis_ctrl.mode) {
case AUTO_NAVI: //自动雷达
// //这套是全向轮的方向,一定要注意这里的xy方向
c->move_vec.Vw =ctrl->cmd_MID360 .posw *1000 ;
c->move_vec.Vy =-ctrl->cmd_MID360.posy *1000 ;
c->move_vec.Vx =-ctrl->cmd_MID360.posx *1000 ;
// c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw);
// c->move_vec.Vy =LowPassFilter2p_Apply(&(c->filled[2]),c->move_vec.Vy);
// c->move_vec.Vx =LowPassFilter2p_Apply(&(c->filled[3]),c->move_vec.Vx);
//
// c->move_vec.Vw =PID_calc(&(c->pid.chassis_NaviWzPID),c->pos088.bmi088.gyro.z,c->move_vec.Vw);
// c->move_vec.Vx =PID_calc(&(c->pid.chassis_NaviVxPID),c->pos088.bmi088.accl.y,c->move_vec.Vx);
// c->move_vec.Vy =PID_calc(&(c->pid.chassis_NaviVyPID),c->pos088.bmi088.accl.x,c->move_vec.Vy);
// //电机速度限幅
case AUTO_MID360: // 自动雷达
// 全向轮方向, 注意xy方向
c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000;
c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
abs_limit_fp(&c->move_vec.Vx,2000.0f);
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
abs_limit_fp(&c->move_vec.Vy,2000.0f);
c->NUC_send .send [0]=0;
break;
abs_limit_fp(&c->move_vec.Vw,2000.0f);
case AUTO_MID360_Pitch:
c->move_vec.Vw = 0;
c->move_vec.Vy = 0;
c->move_vec.Vx = 0;
c->NUC_send .send [0]=0;
break;
case AUTO_MID360_Adjust:
c->move_vec.Vw = ctrl->Vw * 6000;
c->move_vec.Vx = ctrl->Vy * 6000;
c->move_vec.Vy = ctrl->Vx * 6000;
c->NUC_send .send [0]=1;
break;
default:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
c->NUC_send .send [0]=0;
break;
}
break;
if(ctrl->status[5] ==1)
{
c->move_vec.Vw = c->move_vec.Vw * 0.8f;
c->move_vec.Vx = c->move_vec.Vx * 0.5f;
c->move_vec.Vy = c->move_vec.Vy * 0.5f;
}
default:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
break;
}
break;
case AUTO_NAVI_Pitch: //自动视觉
c->move_vec.Vw =0;
c->move_vec.Vy =0;
c->move_vec.Vx =0 ;
// if(abs_value(ctrl ->cmd_pick .posx )>20)
// {
// c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_HIGN),(c->move_vec .Vw) ,0);
//
// }
// else if(abs_value(ctrl ->cmd_pick .posx )<0.1)
// {
// c->move_vec.Vw =0;
// }
// else
// c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_LOW),(c->move_vec .Vw) ,0);
// c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw);
//
// c->vofa_send[0]=c->move_vec.Vw;
// c->vofa_send[1]=-ctrl->cmd_pick .posx;
//
break ;
}
break ;
}
//电机速度限幅
abs_limit_fp(&c->move_vec.Vx,6000.0f);
abs_limit_fp(&c->move_vec.Vy,6000.0f);
abs_limit_fp(&c->move_vec.Vw,6000.0f);
// 电机速度限幅
abs_limit_fp(&c->move_vec.Vx, 6000.0f);
abs_limit_fp(&c->move_vec.Vy, 6000.0f);
abs_limit_fp(&c->move_vec.Vw, 6000.0f);
Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw);
@ -278,8 +269,6 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
};
fp32 jiuzheng(fp32 yaw)
{
static fp32 pian_yaw=0;
static fp32 shang_yaw=0;
static int is=0;
@ -289,9 +278,9 @@ fp32 jiuzheng(fp32 yaw)
is=1;
}
pian_yaw+= (yaw - shang_yaw);
pian_yaw+= (yaw - shang_yaw)*1000;
shang_yaw=yaw ;
return PID_calc(&pid,pian_yaw,0);
return PID_calc (&pid,pian_yaw,0);
}

6
User/Module/config.c
View File

@ -1,4 +1,4 @@
#include "config.h"
#include "config.h"
#include "flash.h"
#include "string.h"
#define DEBUG
@ -110,13 +110,13 @@ static const ConfigParam_t param ={
},
/*投球*/
.PitchConfig_Config = {
.m2006_init_angle =-90,
.m2006_init_angle =-100,
.m2006_trigger_angle =0,
.go1_init_position = -50,
.go1_Receive_ball = -5, //偏下
.go1_release_threshold =-210,
.m2006_Screw_init=0,
.Pitch_angle =56,
.Pitch_angle =58,
},

37
User/Module/up.c
View File

@ -317,7 +317,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
{
if(u->DribbleContext.DribbleState== DRIBBLE_PREPARE){
u->DribbleContext .DribbleState=DRIBBLE_TRANSLATE;
u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;
}
//光电状态更新
Dribble_Process(u,out);
@ -331,7 +331,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
case AUTO:
switch(c-> CMD_mode)
{
case AUTO_NAVI_Pitch:
case AUTO_MID360_Pitch:
u->PitchContext .PitchConfig .go1_Receive_ball=LowPassFilter2p_Apply(&u->filled[0],c->pos);
if (u->PitchContext .PitchState ==PITCH_PREPARE)
@ -344,7 +344,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
Pitch_Process(u,out);
break ;
case AUTO_NAVI:
case AUTO_MID360:
u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
u->PitchContext .PitchState = PITCH_PREPARE;
@ -430,6 +430,8 @@ return 0;
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
{
static int common_speed_flag=0;//是否共速
static int common_speed_reverse_flag=0;//是否共速
switch (u->DribbleContext.DribbleState) {
@ -444,8 +446,8 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
case DRIBBLE_PROCESS_DOWN:
u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
u->motor_target.Dribble_M3508_speed[1]=-u->DribbleContext .DribbleConfig.m3508_dribble_speed;
u->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_speed;
u->motor_target.Dribble_M3508_speed[1]=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
u->motor_target.Dribble_M3508_speed[2]=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm,
u->motorfeedback .DJmotor_feedback [1].rpm,
@ -461,10 +463,11 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
if(common_speed_flag){
if(u->DribbleContext .DribbleConfig .light_ball_flag == 0){//球下落检测，反转
u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
u->motor_target.Dribble_M3508_speed[1]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
u->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
u->motor_target.Dribble_M3508_speed[1]=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
u->motor_target.Dribble_M3508_speed[2]=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
u->DribbleContext .DribbleState=DRIBBLE_PROCESS_UP;
}
}
@ -472,20 +475,24 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
break;
case DRIBBLE_PROCESS_UP:
common_speed_flag =0;
common_speed_flag =0;
if((u->motorfeedback .DJmotor_feedback [0].rpm<-2000)&&
(u->motorfeedback .DJmotor_feedback [1].rpm>2000)&&
(u->motorfeedback .DJmotor_feedback [2].rpm>2000)
if((u->motorfeedback .DJmotor_feedback [0].rpm<-500)&&
(u->motorfeedback .DJmotor_feedback [1].rpm<-500)&&
(u->motorfeedback .DJmotor_feedback [2].rpm<-500)
){
if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){
common_speed_reverse_flag=1;
}
if(common_speed_reverse_flag){
if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){
u->DribbleContext .DribbleState=DRIBBLE_DONE;
RELAY1_TOGGLE(0); //关闭气缸
}
}
}
}
break ;
case DRIBBLE_DONE:
common_speed_reverse_flag=0;
for(int i=0;i<3;i++){
u->motor_target.Dribble_M3508_speed[i]=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0
}

3
User/device/can_use.c
View File

@ -93,7 +93,8 @@ void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
{
case 0x01:
feedback->ecd =raw[3]|raw[4]<<8|raw[5]<<16|raw[6]<<24;
feedback->angle=(fp32)feedback->ecd*360/CAN_ENCODER_RESOLUTION-151.0f;
feedback->angle=(fp32)(2048-(fp32)feedback->ecd)/(2048-1797)*(70.0f-48.0f)+48.0f;
break;
}
}

28
User/device/cmd.c
View File

@ -99,17 +99,17 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
switch(cmd->cmd_status){
case MID:
cmd->cmd_MID360.posx = n->navi.vx;
cmd->cmd_MID360.posy = n->navi.vy;
cmd->cmd_MID360.posw = n->navi.wz;
cmd->cmd_MID360.posx = n->MID360.vx;
cmd->cmd_MID360.posy = n->MID360.vy;
cmd->cmd_MID360.posw = n->MID360.wz;
cmd->pos =n->navi .pos ;
cmd->pos =n->MID360 .pos ;
break;
case PICK :
cmd ->cmd_pick .posx =n->pick .posx ;
cmd ->cmd_pick .posy =n->pick .posy ;
cmd ->cmd_pick .posw =n->pick .posw ;
case VISION :
cmd ->cmd_pick .posx =n->camera.data1 ;
cmd ->cmd_pick .posy =n->camera.data2 ;
cmd ->cmd_pick .posw =n->camera.data3 ;
break;
@ -153,11 +153,11 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
{
cmd ->CMD_CtrlType =AUTO;
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =AUTO_NAVI; //左中，右中，雷达
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360; //左中，右中，雷达
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =AUTO_NAVI_Pitch; //左中，右下，视觉
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =AUTO_MID360_Pitch; //左中，右下，视觉
}
else if(rc->dr16.sw_l==CMD_SW_DOWN)
@ -191,9 +191,12 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
if(cmd ->CMD_CtrlType ==AUTO){
/*自动下的*/
if (rc->LD.key_C ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360_Adjust;
else {
if(rc->LD.key_E ==CMD_SW_DOWN ) cmd ->CMD_mode =Normal;
else if(rc->LD.key_E ==CMD_SW_UP) cmd ->CMD_mode =AUTO_NAVI_Pitch;
else if(rc->LD .key_E ==CMD_SW_MID ) cmd ->CMD_mode =AUTO_NAVI;
else if(rc->LD.key_E ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360_Pitch;
else if(rc->LD .key_E ==CMD_SW_MID ) cmd ->CMD_mode =AUTO_MID360;
else cmd ->CMD_mode =Normal ;
}
@ -205,3 +208,4 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
else cmd ->CMD_mode =Normal;
}
}

22
User/device/cmd.h
View File

@ -8,7 +8,7 @@
#include <string.h>
#define MID (0x09)
#define PICK (0x06)
#define VISION (0x02)
#define NUC (0x08)
typedef enum{
@ -23,8 +23,9 @@ typedef enum{
Normal, //无模式
AUTO_NAVI,
AUTO_NAVI_Pitch,
AUTO_MID360,
AUTO_MID360_Pitch,
AUTO_MID360_Adjust,//雷达调整
UP_Adjust,//上层调整
@ -47,18 +48,13 @@ typedef struct {
fp32 angle;
char flag;
}navi;
}MID360;
struct
{
fp32 posx;
fp32 posy;
fp32 posw;
}pick;
struct
{
fp32 angle;
}sick_cali;
fp32 data1;
fp32 data2;
fp32 data3;
}camera;//相机
} CMD_NUC_t;
/* 拨杆位置 */

68
User/device/nuc.c
View File

@ -32,6 +32,7 @@ int8_t NUC_StartReceiving() {
int8_t NUC_StartSending(fp32 *data) {
union
{
float x[4];
@ -81,33 +82,34 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
n->ctrl_status =nucbuf[2];
switch (n->status_fromnuc)
{
// case MID://控制帧0x09
// /* 协议格式
// 0xFF HEAD
// 0x09 控制帧
// 0x01 相机帧
// vx fp32
// vy fp32
// wz fp32
// 0xFE TAIL
// */
//// if(nucbuf[15]!=TAIL)goto error;
// instance.data[3] = nucbuf[6];
// instance.data[2] = nucbuf[5];
// instance.data[1] = nucbuf[4];
// instance.data[0] = nucbuf[3];
// n->navi.vx = instance.x[0]; //
// instance.data[7] = nucbuf[10];
// instance.data[6] = nucbuf[9];
// instance.data[5] = nucbuf[8];
// instance.data[4] = nucbuf[7];
// n->navi.vy = instance.x[1];//
// instance.data[11] = nucbuf[14];
// instance.data[10] = nucbuf[13];
// instance.data[9] = nucbuf[12];
// instance.data[8] = nucbuf[11];
// n->navi.wz = instance.x[2];//
// break;
case VISION://控制帧0x02
/* 协议格式
0xFF HEAD
0x02 控制帧
0x01 相机帧
vx fp32
vy fp32
wz fp32
0xFE TAIL
*/
instance.data[3] = nucbuf[3];
instance.data[2] = nucbuf[4];
instance.data[1] = nucbuf[5];
instance.data[0] = nucbuf[6];
n->camera .data1 = instance.x[0]; //
instance.data[7] = nucbuf[7];
instance.data[6] = nucbuf[8];
instance.data[5] = nucbuf[9];
instance.data[4] = nucbuf[10];
n->camera .data2 = instance.x[1];//
instance.data[11] = nucbuf[11];
instance.data[10] = nucbuf[12];
instance.data[9] = nucbuf[13];
instance.data[8] = nucbuf[14];
n->camera .data3 = instance.x[2];//
break;
case MID ://控制帧0x08
/* 协议格式
0xFF HEAD
@ -123,29 +125,29 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
instance.data[2] = nucbuf[5];
instance.data[1] = nucbuf[4];
instance.data[0] = nucbuf[3];
n->navi.vx = instance.x[0]; //
n->MID360.vx = instance.x[0]; //
instance.data[7] = nucbuf[10];
instance.data[6] = nucbuf[9];
instance.data[5] = nucbuf[8];
instance.data[4] = nucbuf[7];
n->navi.vy = instance.x[1];//
n->MID360.vy = instance.x[1];//
instance.data[11] = nucbuf[14];
instance.data[10] = nucbuf[13];
instance.data[9] = nucbuf[12];
instance.data[8] = nucbuf[11];
n->navi.wz = instance.x[2];//
n->MID360.wz = instance.x[2];//
instance.data[15] = nucbuf[18];
instance.data[14] = nucbuf[17];
instance.data[13] = nucbuf[16];
instance.data[12] = nucbuf[15];
n->navi.pos = instance.x[3];//
n->MID360.pos = instance.x[3];//
instance.data[19] = nucbuf[22];
instance.data[18] = nucbuf[21];
instance.data[17] = nucbuf[20];
instance.data[16] = nucbuf[19];
n->navi.angle = instance.x[4];//
n->MID360.angle = instance.x[4];//
n->navi.flag = nucbuf[23];//
n->MID360.flag = nucbuf[23];//
break;

20
User/device/rc.c
View File

@ -5,15 +5,15 @@
/* Includes ----------------------------------------------------------------- */
#include "rc.h"
#include "cmd.h"
#include <string.h>
#include "bsp_usart.h"
#include "error_detect.h"
//#define DR16
#define LD_t
#define DR16
//#define LD_t
#ifdef DR16
#define FRAME_LEN 36
@ -100,15 +100,15 @@ int8_t LD_ParseRaw( LD_raw_t *raw, LD_Data_t *LD)
if (raw->ch[0] < 0)
raw->map_ch[0] = map_fp32(raw->ch[0], -696, 2, -1, 0);
raw->map_ch[0] = map_fp32(raw->ch[0], -700, -2, -1, 0);
else
raw->map_ch[0] = map_fp32(raw->ch[0], 2, 704, 0, 1);
raw->map_ch[0] = map_fp32(raw->ch[0], -2, 700, 0, 1);
// ch[1]
if (raw->ch[1] < 0)
raw->map_ch[1] = map_fp32(raw->ch[1], -638, 5, -1, 0);
raw->map_ch[1] = map_fp32(raw->ch[1], -700, 4, -1, 0);
else
raw->map_ch[1] = map_fp32(raw->ch[1], 5, 762, 0, 1);
raw->map_ch[1] = map_fp32(raw->ch[1], 4, 700, 0, 1);
raw->map_ch[2] = map_fp32(raw->ch[2], 2, 1377, 0, 1);
@ -220,8 +220,10 @@ 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_y = 2 * ((float)dr16->data.ch_l_y - RC_CH_VALUE_MID) / full_range;
rc->dr16.sw_l = (CMD_SwitchPos_DR16_t)dr16->data.sw_l;
rc->dr16.sw_r = (CMD_SwitchPos_DR16_t)dr16->data.sw_r;
// rc->dr16.sw_l = (CMD_SwitchPos_DR16_t)dr16->data.sw_l;
// rc->dr16.sw_r = (CMD_SwitchPos_DR16_t)dr16->data.sw_r;
rc->dr16.sw_l = (CMD_SwitchPos_t)dr16->data.sw_l;
rc->dr16.sw_r = (CMD_SwitchPos_t)dr16->data.sw_r;
rc->dr16.key = dr16->data.key;

12
User/device/vofa.c
View File

@ -28,13 +28,11 @@ void vofa_tx_main(float *data)
/*在下面使用对应的串口发送函数*/
// CDC_Transmit_FS( ( uint8_t *)fdata, sizeof(fdata));
// osDelay(1);
// CDC_Transmit_FS( tail, 4);
// HAL_UART_Transmit_DMA(&huart1, ( uint8_t *)fdata, sizeof(fdata));
// osDelay(1);
// HAL_UART_Transmit_DMA(&huart1, tail, 4);
// osDelay(1);
HAL_UART_Transmit_DMA(&huart6, ( uint8_t *)fdata, sizeof(fdata));
osDelay(1);
HAL_UART_Transmit_DMA(&huart6, tail, 4);
osDelay(1);
}

2
User/task/nuc_task.c
View File

@ -29,7 +29,7 @@ void Task_nuc(void *argument){
task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());
#endif
NUC_StartReceiving();
NUC_StartSending(send);
// NUC_StartSending(NUC_send.send);
if (NUC_WaitDmaCplt()){
NUC_RawParse(&cmd_fromnuc);
}