解决nuc控制数据不清空问题
This commit is contained in:
parent
bd16e41962
commit
2e854ade02
2
.gitignore
vendored
2
.gitignore
vendored
@ -17,7 +17,7 @@
|
||||
*.dbg*
|
||||
*.uvguix.*
|
||||
.mxproject
|
||||
|
||||
*.axf
|
||||
RTE/
|
||||
Templates/
|
||||
Examples/
|
||||
|
||||
@ -28,7 +28,7 @@
|
||||
<TargetOption>
|
||||
<CLKADS>25000000</CLKADS>
|
||||
<OPTTT>
|
||||
<gFlags>0</gFlags>
|
||||
<gFlags>1</gFlags>
|
||||
<BeepAtEnd>1</BeepAtEnd>
|
||||
<RunSim>0</RunSim>
|
||||
<RunTarget>1</RunTarget>
|
||||
@ -77,7 +77,7 @@
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<IsCurrentTarget>1</IsCurrentTarget>
|
||||
</OPTFL>
|
||||
<CpuCode>0</CpuCode>
|
||||
<CpuCode>18</CpuCode>
|
||||
<DebugOpt>
|
||||
<uSim>0</uSim>
|
||||
<uTrg>1</uTrg>
|
||||
@ -103,7 +103,7 @@
|
||||
<bEvRecOn>1</bEvRecOn>
|
||||
<bSchkAxf>0</bSchkAxf>
|
||||
<bTchkAxf>0</bTchkAxf>
|
||||
<nTsel>0</nTsel>
|
||||
<nTsel>3</nTsel>
|
||||
<sDll></sDll>
|
||||
<sDllPa></sDllPa>
|
||||
<sDlgDll></sDlgDll>
|
||||
@ -114,9 +114,34 @@
|
||||
<tDlgDll></tDlgDll>
|
||||
<tDlgPa></tDlgPa>
|
||||
<tIfile></tIfile>
|
||||
<pMon>BIN\UL2CM3.DLL</pMon>
|
||||
<pMon>BIN\CMSIS_AGDI.dll</pMon>
|
||||
</DebugOpt>
|
||||
<TargetDriverDllRegistry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>ARMRTXEVENTFLAGS</Key>
|
||||
<Name>-L70 -Z18 -C0 -M0 -T1</Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>DLGTARM</Key>
|
||||
<Name>(1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0)</Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>ARMDBGFLAGS</Key>
|
||||
<Name></Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>DLGUARM</Key>
|
||||
<Name></Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>CMSIS_AGDI</Key>
|
||||
<Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>UL2CM3</Key>
|
||||
@ -170,19 +195,59 @@
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>NUC_send,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>9</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>NUC_send</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>10</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>nuc_raw,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>11</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ang_58</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>12</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ang_66</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>13</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd_fromnuc,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>14</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>Nuc,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>15</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rec_flag</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>16</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ctrl</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
<THDelay>0</THDelay>
|
||||
</Tracepoint>
|
||||
<DebugFlag>
|
||||
<trace>0</trace>
|
||||
<periodic>0</periodic>
|
||||
<aLwin>0</aLwin>
|
||||
<periodic>1</periodic>
|
||||
<aLwin>1</aLwin>
|
||||
<aCover>0</aCover>
|
||||
<aSer1>0</aSer1>
|
||||
<aSer2>0</aSer2>
|
||||
<aPa>0</aPa>
|
||||
<viewmode>0</viewmode>
|
||||
<viewmode>1</viewmode>
|
||||
<vrSel>0</vrSel>
|
||||
<aSym>0</aSym>
|
||||
<aTbox>0</aTbox>
|
||||
@ -817,7 +882,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/Module</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1025,7 +1090,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/Task</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1233,7 +1298,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/Device</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
|
||||
@ -17,8 +17,8 @@
|
||||
<TargetCommonOption>
|
||||
<Device>STM32F407IGHx</Device>
|
||||
<Vendor>STMicroelectronics</Vendor>
|
||||
<PackID>Keil.STM32F4xx_DFP.2.15.0</PackID>
|
||||
<PackURL>http://www.keil.com/pack/</PackURL>
|
||||
<PackID>Keil.STM32F4xx_DFP.3.0.0</PackID>
|
||||
<PackURL>https://www.keil.com/pack/</PackURL>
|
||||
<Cpu>IRAM(0x20000000-0x2001BFFF) IRAM2(0x2001C000-0x2001FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4") TZ</Cpu>
|
||||
<FlashUtilSpec></FlashUtilSpec>
|
||||
<StartupFile></StartupFile>
|
||||
|
||||
Binary file not shown.
@ -98,12 +98,14 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
|
||||
c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);
|
||||
|
||||
/*初始数据*/
|
||||
|
||||
c->move_vec.Vx = 0;
|
||||
c->move_vec.Vy = 0;
|
||||
c->move_vec.Vw = 0;
|
||||
c->to_nuc.send = 0;
|
||||
c->sick_cali.is_reach = 0;
|
||||
|
||||
c->vofa_send[0]= KalmanFilter(&c->extKalman[0] , c->sick_cali.sick_dis[0]);
|
||||
c->vofa_send[1]= KalmanFilter(&c->extKalman[1] , c->sick_cali.sick_dis[1]);
|
||||
c->vofa_send[1]= KalmanFilter(&c->extKalman[1] , c->sick_cali.sick_dis[1]);
|
||||
c->vofa_send[2]= KalmanFilter(&c->extKalman[2] , c->sick_cali.sick_dis[2]);
|
||||
|
||||
|
||||
@ -129,9 +131,9 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
|
||||
break;
|
||||
case Chassis_Adjust_Sick:
|
||||
|
||||
sick_calibration(c, ctrl, out);
|
||||
// sick_calibration(c, ctrl, out);
|
||||
// c->to_nuc.send = (c->sick_cali.is_reach == 1) ? 1 : 0;
|
||||
break;
|
||||
break;
|
||||
|
||||
}
|
||||
break;
|
||||
@ -139,18 +141,20 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
|
||||
case AUTO:
|
||||
switch (c->chassis_ctrl.mode) {
|
||||
case AUTO_MID360:
|
||||
// c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000;
|
||||
// c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000;
|
||||
// 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, 5000.0f);
|
||||
// abs_limit_fp(&c->move_vec.Vy, 5000.0f);
|
||||
// abs_limit_fp(&c->move_vec.Vw, 5000.0f);
|
||||
c->move_vec.Vw =-ctrl->cmd_MID360.posw * 3000;
|
||||
|
||||
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);
|
||||
|
||||
break;
|
||||
case AUTO_MID360_Pitch:
|
||||
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 = -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, 5000.0f);
|
||||
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
|
||||
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
|
||||
|
||||
@ -75,7 +75,7 @@ static const ConfigParam_t param ={
|
||||
|
||||
/*投球*/
|
||||
.LaunchCfg = {
|
||||
.m2006_init = -150.0f, // M2006初始角度
|
||||
.m2006_init = -130.0f, // M2006初始角度
|
||||
.m2006_trig = 0.0f, // M2006触发角度
|
||||
.go_pull_pos = -214.0f, // go上升去合并扳机，扳机位置
|
||||
.go_up_speed = 12.0f, // 上升速度
|
||||
|
||||
@ -403,7 +403,14 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
|
||||
LaunchContext_t *LaunchContext = &u->LaunchContext;
|
||||
MID360Context_t *MID360Context=&u->MID360Context;
|
||||
MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg;
|
||||
if(u->MID360Context.Curve==CURVE_58){
|
||||
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.25,3.2,4.3,&u->MID360Context.Curve);
|
||||
}
|
||||
else {
|
||||
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.05,3.2,4.3,&u->MID360Context.Curve);
|
||||
|
||||
}
|
||||
|
||||
if (u->MID360Context.Curve == CURVE_58) {
|
||||
target->Pitch_angle = 58;
|
||||
} else {
|
||||
|
||||
@ -67,10 +67,9 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo
|
||||
|
||||
// 计算66度曲线（偏上）
|
||||
// 计算66度曲线（偏上）
|
||||
static float curve_66(float d) {
|
||||
static float curve_66(float d) {
|
||||
return 3.7028f * d * d + 11.2126f * d -142.9446f;
|
||||
}
|
||||
|
||||
// 计算58度曲线（偏下）
|
||||
// 计算58度曲线（偏下）
|
||||
static float curve_58(float d) {
|
||||
@ -83,6 +82,9 @@ static float curve_58(float d) {
|
||||
曲线x重合区，根据当前函数和变化方向切换
|
||||
*/
|
||||
int abdddd=0;
|
||||
fp32 ang_58;
|
||||
fp32 ang_66;
|
||||
|
||||
float CurveChange(float d, float x, float y, CurveType *cs)
|
||||
{
|
||||
if (d<3.2) abdddd++;
|
||||
@ -98,8 +100,10 @@ float CurveChange(float d, float x, float y, CurveType *cs)
|
||||
|
||||
// 根据当前曲线返回结果
|
||||
if (*cs == CURVE_58) {
|
||||
ang_58=d;
|
||||
return curve_58(d);
|
||||
} else {
|
||||
ang_66=d;
|
||||
return curve_66(d);
|
||||
}
|
||||
}
|
||||
|
||||
@ -83,7 +83,7 @@ typedef struct __attribute__((packed)) {
|
||||
|
||||
typedef struct __attribute__((packed))
|
||||
{
|
||||
uint8_t status;
|
||||
fp32 status;
|
||||
} Protocol_UpDataCMD_t;
|
||||
|
||||
/* 视觉 -> 电控 上层机构数据结构体*/
|
||||
|
||||
@ -87,7 +87,7 @@ return 0;
|
||||
|
||||
int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
|
||||
if(cmd == NULL) return -1;
|
||||
if(n == NULL) return -1;
|
||||
if(n == NULL) return -1;
|
||||
|
||||
cmd->cmd_status = n->status_fromnuc;
|
||||
cmd->raw_status = n->ctrl_status;
|
||||
@ -112,6 +112,16 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
|
||||
cmd ->cmd_pick .posw =n->camera.data3 ;
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
cmd->cmd_MID360.posx = 0;
|
||||
cmd->cmd_MID360.posy = 0;
|
||||
cmd->cmd_MID360.posw = 0;
|
||||
|
||||
cmd->pos =0 ;
|
||||
|
||||
|
||||
break ;
|
||||
|
||||
|
||||
}
|
||||
|
||||
@ -43,7 +43,8 @@ typedef enum{
|
||||
}CMD_mode_t;
|
||||
typedef struct {
|
||||
uint8_t status_fromnuc;
|
||||
uint8_t ctrl_status;
|
||||
uint8_t ctrl_status;
|
||||
|
||||
struct
|
||||
{
|
||||
fp32 vx;
|
||||
|
||||
@ -16,7 +16,7 @@ static CMD_NUC_t Nuc;
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
int rec_flag=0;
|
||||
|
||||
void Task_cmd(void *argument){
|
||||
(void)argument; /*未使用传递参数 消除警告*/
|
||||
@ -38,14 +38,16 @@ void Task_cmd(void *argument){
|
||||
osKernelLock(); /*锁住RTOS内核调度*/
|
||||
|
||||
/*将各任务接收到的原始数据解析为通用的控制命令*/
|
||||
|
||||
|
||||
|
||||
/*注意，不能将nuc和码盘导航一块使用*/
|
||||
|
||||
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK) //nuc
|
||||
CMD_ParseNuc(&cmd,&Nuc);
|
||||
|
||||
|
||||
if((osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK)){ //nuc
|
||||
}
|
||||
else
|
||||
{
|
||||
memset(&Nuc, 0, sizeof(CMD_NUC_t));
|
||||
}
|
||||
CMD_ParseNuc(&cmd,&Nuc);
|
||||
|
||||
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器
|
||||
CMD_ParseRc(&cmd, &rc_ctrl);
|
||||
|
||||
|
||||
@ -47,19 +47,23 @@ void Task_nuc(void *argument){
|
||||
last_online_tick = tick;
|
||||
}
|
||||
else{
|
||||
if (tick - last_online_tick > 300) NUC_HandleOffline(&cmd_fromnuc, &nuc_raw);
|
||||
if (tick - last_online_tick > 300)
|
||||
NUC_HandleOffline(&cmd_fromnuc, &nuc_raw);
|
||||
}
|
||||
|
||||
if (nuc_raw.unc_online) {
|
||||
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc);
|
||||
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0);
|
||||
}
|
||||
else
|
||||
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc);
|
||||
|
||||
|
||||
osMessageQueueGet(task_runtime.msgq.nuc.quat, &(quat), NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.nuc.accl, &(accl), NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.nuc.gyro, &(gyro), NULL, 0);
|
||||
|
||||
//osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0);
|
||||
|
||||
|
||||
bool cmd_update = (osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send,
|
||||
&(NUC_send), NULL, 0) == osOK);
|
||||
@ -73,6 +77,7 @@ void Task_nuc(void *argument){
|
||||
}
|
||||
|
||||
|
||||
|
||||
// NUC_StartSend(&nuc_raw, cmd_update);
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/
|
||||
|
||||
@ -18,7 +18,7 @@
|
||||
#define TASK_FREQ_UP (900u)
|
||||
|
||||
#define TASK_FREQ_CTRL_CMD (500u)
|
||||
#define TASK_FREQ_NUC (250u)
|
||||
#define TASK_FREQ_NUC (500u)
|
||||
#define TASK_FREQ_CAN (1000u)
|
||||
#define TASK_FREQ_RC (1000u)
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user