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

Compare commits

base: LYL:main
Branches Tags
LYL:main
LYL:Cooperation
LYL:chassis
..
compare: LYL:Cooperation
Branches Tags
LYL:Cooperation
LYL:chassis
LYL:main

10 Commits
main ... Cooperatio

Author SHA1 Message Date
ZHAISHUI04
e9710e65a5 sick定位 2025-06-15 16:29:12 +08:00
ZHAISHUI04
afdb0edbc3 sick转接，无报错 2025-06-13 16:24:49 +08:00
ZHAISHUI04
5556d48921 总过程 2025-06-05 16:47:27 +08:00
ZHAISHUI04
9567222df6 改视觉备用 2025-06-03 23:16:33 +08:00
ZHAISHUI04
bcfaac985f 给go加位置限位 2025-06-02 22:01:25 +08:00
ZHAISHUI04
4c8a3945c4 修复一点bug，编码器重新校准 2025-05-31 15:26:23 +08:00
ZHAISHUI04
9fe37c6b89 oid编码器更换，更改can_use处 2025-05-29 19:37:09 +08:00
ZHAISHUI04
553de3fa5e 给nuc发数据 2025-05-28 20:58:14 +08:00
ZHAISHUI04
8ffef38518 go和nuc串口更换，根据位置自动发go的位置 2025-05-24 14:10:05 +08:00
ZHAISHUI04
b2efd7c822 整体配合 2025-05-16 19:34:06 +08:00
24 changed files with 706 additions and 705 deletions
Show Stats Expand all files Collapse all files

108
MDK-ARM/.vscode/c_cpp_properties.json vendored
View File

@ -415,6 +415,114 @@
"__vfp_status(x,y)=0" "__vfp_status(x,y)=0"
], ],
"intelliSenseMode": "${default}" "intelliSenseMode": "${default}"
},
{
"name": "R2",
"includePath": [
"d:\\Desktop\\r2\\R2_NEW\\R2\\Core\\Inc",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Include",
"d:\\Desktop\\r2\\R2_NEW\\R2\\User",
"d:\\Desktop\\r2\\R2_NEW\\R2\\User\\bsp",
"d:\\Desktop\\r2\\R2_NEW\\R2\\User\\device",
"d:\\Desktop\\r2\\R2_NEW\\R2\\User\\task",
"d:\\Desktop\\r2\\R2_NEW\\R2\\User\\Algorithm",
"d:\\Desktop\\r2\\R2_NEW\\R2\\User\\Module",
"d:\\Desktop\\r2\\R2_NEW\\R2\\MDK-ARM",
"d:\\Desktop\\r2\\R2_NEW\\R2\\USB_DEVICE\\App",
"d:\\Desktop\\r2\\R2_NEW\\R2\\USB_DEVICE\\Target",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Inc",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Inc",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Lib\\ARM",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Core\\Src",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Src",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Src",
"d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Src"
],
"defines": [
"USE_HAL_DRIVER",
"STM32F407xx",
"__CC_ARM",
"__arm__",
"__align(x)=",
"__ALIGNOF__(x)=",
"__alignof__(x)=",
"__asm(x)=",
"__forceinline=",
"__restrict=",
"__global_reg(n)=",
"__inline=",
"__int64=long long",
"__INTADDR__(expr)=0",
"__irq=",
"__packed=",
"__pure=",
"__smc(n)=",
"__svc(n)=",
"__svc_indirect(n)=",
"__svc_indirect_r7(n)=",
"__value_in_regs=",
"__weak=",
"__writeonly=",
"__declspec(x)=",
"__attribute__(x)=",
"__nonnull__(x)=",
"__register=",
"__breakpoint(x)=",
"__cdp(x,y,z)=",
"__clrex()=",
"__clz(x)=0U",
"__current_pc()=0U",
"__current_sp()=0U",
"__disable_fiq()=",
"__disable_irq()=",
"__dmb(x)=",
"__dsb(x)=",
"__enable_fiq()=",
"__enable_irq()=",
"__fabs(x)=0.0",
"__fabsf(x)=0.0f",
"__force_loads()=",
"__force_stores()=",
"__isb(x)=",
"__ldrex(x)=0U",
"__ldrexd(x)=0U",
"__ldrt(x)=0U",
"__memory_changed()=",
"__nop()=",
"__pld(...)=",
"__pli(...)=",
"__qadd(x,y)=0",
"__qdbl(x)=0",
"__qsub(x,y)=0",
"__rbit(x)=0U",
"__rev(x)=0U",
"__return_address()=0U",
"__ror(x,y)=0U",
"__schedule_barrier()=",
"__semihost(x,y)=0",
"__sev()=",
"__sqrt(x)=0.0",
"__sqrtf(x)=0.0f",
"__ssat(x,y)=0",
"__strex(x,y)=0U",
"__strexd(x,y)=0",
"__strt(x,y)=",
"__swp(x,y)=0U",
"__usat(x,y)=0U",
"__wfe()=",
"__wfi()=",
"__yield()=",
"__vfp_status(x,y)=0"
],
"intelliSenseMode": "${default}"
} }
], ],
"version": 4 "version": 4

12
MDK-ARM/.vscode/keil-assistant.log vendored
View File

@ -451,3 +451,15 @@
[info] Log at : 2025/3/28|21:46:16|GMT+0800 [info] Log at : 2025/3/28|21:46:16|GMT+0800
[info] Log at : 2025/6/6|21:51:18|GMT+0800
[info] Log at : 2025/6/7|17:45:08|GMT+0800
[info] Log at : 2025/6/7|21:03:23|GMT+0800
[info] Log at : 2025/6/12|16:54:50|GMT+0800
[info] Log at : 2025/6/13|16:30:54|GMT+0800
[info] Log at : 2025/6/14|15:14:16|GMT+0800

3
MDK-ARM/.vscode/settings.json vendored
View File

@ -96,6 +96,7 @@
"limits": "c", "limits": "c",
"gimbal_task.h": "c", "gimbal_task.h": "c",
"gimgal.h": "c", "gimgal.h": "c",
"navi.h": "c" "navi.h": "c",
"up.h": "c"
} }
} }

149
MDK-ARM/R2.uvoptx
View File

@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn> <bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf> <bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf> <bTchkAxf>0</bTchkAxf>
<nTsel>6</nTsel> <nTsel>3</nTsel>
<sDll></sDll> <sDll></sDll>
<sDllPa></sDllPa> <sDllPa></sDllPa>
<sDlgDll></sDlgDll> <sDlgDll></sDlgDll>
@ -114,7 +114,7 @@
<tDlgDll></tDlgDll> <tDlgDll></tDlgDll>
<tDlgPa></tDlgPa> <tDlgPa></tDlgPa>
<tIfile></tIfile> <tIfile></tIfile>
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon> <pMon>BIN\CMSIS_AGDI.dll</pMon>
</DebugOpt> </DebugOpt>
<TargetDriverDllRegistry> <TargetDriverDllRegistry>
<SetRegEntry> <SetRegEntry>
@ -140,7 +140,7 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>DLGUARM</Key> <Key>DLGUARM</Key>
<Name>(105=-1,-1,-1,-1,0)</Name> <Name></Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@ -158,157 +158,22 @@
<Ww> <Ww>
<count>0</count> <count>0</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>rc_ctrl,0x0A</ItemText> <ItemText>can,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>1</count> <count>1</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>LD_raw,0x0A</ItemText> <ItemText>chassis,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>2</count> <count>2</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>can,0x0A</ItemText> <ItemText>UP,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>3</count> <count>3</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText> <ItemText>NUC_send,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>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>up_cmd,0x0A</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>can_out</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>Nor_Vx</ItemText>
</Ww>
<Ww>
<count>12</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>
</Ww> </Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>

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

Binary file not shown.

16
User/Algorithm/user_math.c
View File

@ -216,7 +216,21 @@ void abs_limit_fp(fp32 *num, fp32 Limit)
*num = -Limit; *num = -Limit;
} }
} }
//改原始值，限制最大最小
fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max)
{
if (*num > Limit_max)
{
*num=Limit_max;
return Limit_max;
}
else if (*num < Limit_min)
{
*num=Limit_min;
return Limit_min;
}
}
/* 移动向量 */ /* 移动向量 */
MoveVector_t *mv; MoveVector_t *mv;

1
User/Algorithm/user_math.h
View File

@ -164,5 +164,6 @@ int read_flag_state(uint8_t flag) ;
void normalize_vector(double x, double y, double *out_x, double *out_y) ; void normalize_vector(double x, double y, double *out_x, double *out_y) ;
bool is_reached(float current, float target, float mistake) ; bool is_reached(float current, float target, float mistake) ;
bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) ; bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) ;
fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max);
#endif #endif

405
User/Module/Chassis.c
View File

@ -4,197 +4,143 @@
#include "bsp_buzzer.h" #include "bsp_buzzer.h"
#include "bsp_delay.h" #include "bsp_delay.h"
/*
机器人坐标系，向前x，右y，上yaw
不同于nuc (前x，左y，上yaw)
x
|
/*机器人坐标系，向前x，右y，上yaw --y
不同于nuc (前x，左y，上yaw) */
/*
x
|
--y
*/ */
static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){ static int8_t Chassis_SetCtrl(Chassis_t *c, CMD_t *ctrl) {
if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */ if (c == NULL) return CHASSIS_ERR_NULL;
if (ctrl->CMD_CtrlType== c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode) return CHASSIS_OK; /*模式未改变直接返回*/ if (ctrl->CMD_CtrlType == c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode)
c->chassis_ctrl.ctrl =ctrl->CMD_CtrlType ; return CHASSIS_OK;
c->chassis_ctrl.mode =ctrl->CMD_mode ; c->chassis_ctrl.ctrl = ctrl->CMD_CtrlType;
c->chassis_ctrl.mode = ctrl->CMD_mode;
return CHASSIS_OK; return CHASSIS_OK;
} //设置控制模式 }
/*该函数用来更新can任务获得的电机反馈值*/
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) { int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
if (c == NULL) return CHASSIS_ERR_NULL; if (c == NULL) return CHASSIS_ERR_NULL;
if (can == NULL) return CHASSIS_ERR_NULL; if (can == NULL) return CHASSIS_ERR_NULL;
for (uint8_t i = 0; i < 4; i++) for (uint8_t i = 0; i < 4; i++) {
{ c->motorfeedback.rotor_rpm3508[i] = can->motor.chassis_motor3508.as_array[i].rotor_speed;
c->motorfeedback.rotor_rpm3508[i] = can->motor.chassis_motor3508.as_array[i].rotor_speed; c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current;
c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current; }
} for (uint8_t i = 0; i < 4; i++) {
return CHASSIS_OK;
}
int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
{
if (c == NULL) return CHASSIS_ERR_NULL;
c->param = param; /*初始化参数 */
for(int i =0 ; i < 4 ; i++)
{
PID_init(&(c->pid.chassis_3508VecPID[i]),PID_POSITION_D,&(c->param->M3508_param)); //带D项滤波
}
PID_init(&(c->pid.chassis_PICKWzPID_HIGN),PID_POSITION,&(c->param->chassis_PICKWzPID_HIGN_param));
PID_init(&(c->pid.chassis_PICKWzPID_LOW),PID_POSITION,&(c->param->chassis_PICKWzPID_LOW_param));
PID_init(&(c->pid.sick_CaliforYPID),PID_POSITION,&(c->param->Sick_CaliYparam));
PID_init(&(c->pid.sick_CaliforXPID),PID_POSITION,&(c->param->Sick_CaliXparam));
LowPassFilter2p_Init(&(c->filled[0]),target_freq,80.0f); //给角加速度做滤波
LowPassFilter2p_Init(&(c->filled[1]),target_freq,80.0f); //给w 做滤波
LowPassFilter2p_Init(&(c->filled[2]),target_freq,80.0f); //给y做滤波
LowPassFilter2p_Init(&(c->filled[3]),target_freq,80.0f); //给x 做滤波
//
return CHASSIS_OK;
}
fp32 gyro_kp=1.0f;
fp32 PIAN=0;
void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算,
{
fp64 Nor_Vx,Nor_Vy;//归一化后的数据
normalize_vector(Vx,Vy,&Nor_Vx,&Nor_Vy);
// c->hopemotorout.OmniSpeedOut[0] = -Vx+Vy+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 ;//右前
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 );
// }
// c->hopemotorout.OmniSpeedOut[0] += PIAN ;//右前
// c->hopemotorout.OmniSpeedOut[1] += PIAN ;//右后
// c->hopemotorout.OmniSpeedOut[2] += PIAN ;//左后
// c->hopemotorout.OmniSpeedOut[3] += PIAN ;//左前
}
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
{
if(c ==NULL) return CHASSIS_ERR_NULL;
if(ctrl ==NULL) return CHASSIS_ERR_NULL;
Chassis_SetCtrl(c,ctrl);
//此处对imu加滤波做修正
c->pos088.bmi088.filtered_gyro.z =LowPassFilter2p_Apply(&(c->filled[0]),c->pos088.bmi088.gyro.z);
switch (c->chassis_ctrl .ctrl)
{
case RCcontrol: //手动控制
/* c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值
在cmd里对数据进行处理 包括方向和油门 }
6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同*/
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.Vx = ctrl->Vy*6000;
c->move_vec.Vy = ctrl->Vx*6000;
}
return CHASSIS_OK;
}
int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_freq) {
if (c == NULL) return CHASSIS_ERR_NULL;
c->param = param;
for (int i = 0; i < 4; i++) {
PID_init(&(c->pid.chassis_3508VecPID[i]), PID_POSITION_D, &(c->param->M3508_param));
}
PID_init(&(c->pid.chassis_PICKWzPID_HIGN), PID_POSITION, &(c->param->chassis_PICKWzPID_HIGN_param));
PID_init(&(c->pid.chassis_PICKWzPID_LOW), PID_POSITION, &(c->param->chassis_PICKWzPID_LOW_param));
PID_init(&(c->pid.SickCaliWzPID), PID_POSITION, &(c->param->Sick_CaliWparam));
PID_init(&(c->pid.SickCaliVxPID), PID_POSITION, &(c->param->Sick_CaliXparam));
PID_init(&(c->pid.SickCaliVyPID), PID_POSITION, &(c->param->Sick_CaliYparam));
LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波
LowPassFilter2p_Init(&(c->filled[1]), target_freq, 80.0f); // w滤波
LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f); // y滤波
LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f); // x滤波
c->sick_cali .sickparam=c->param ->sickparam ;
return CHASSIS_OK;
}
fp32 gyro_kp = 1.0f;
fp32 PIAN = 0;
void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
fp64 Nor_Vx, Nor_Vy;
normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy);
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; // 左前
}
int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
if (c == NULL) return CHASSIS_ERR_NULL;
if (ctrl == NULL) return CHASSIS_ERR_NULL;
Chassis_SetCtrl(c, ctrl);
// IMU加滤波修正
c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);
/*初始数据*/
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
c->NUC_send.send[0] = 0;
c->sick_cali.is_reach = 0;
switch (c->chassis_ctrl.ctrl) {
break; case RCcontrol:
case AUTO: // 自动模式
switch (c->chassis_ctrl.mode) { switch (c->chassis_ctrl.mode) {
case Normal:
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, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
c->NUC_send .send [0]=0;
break;
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.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;
c->NUC_send .send [0]=1; case Pitch:
break;
case UP_Adjust:
c->move_vec.Vw = ctrl->Vw * 6000;
break;
case Chassis_Adjust:
sick_calibration(c, ctrl, out);
c->NUC_send.send[0] = (c->sick_cali.is_reach == 1) ? 1 : 0;
break; break;
default: default:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0; break;
c->move_vec.Vy = 0; }
c->NUC_send .send [0]=0; break;
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.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);
break;
case AUTO_MID360_Pitch:
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:
break; break;
} }
break; break;
default: default:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
break; break;
} }
@ -203,65 +149,70 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
abs_limit_fp(&c->move_vec.Vy, 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.Vw, 6000.0f);
Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw); Chassis_speed_calculate(c, c->move_vec.Vx, c->move_vec.Vy, c->move_vec.Vw);
for (uint8_t i = 0 ; i <4 ; i++)
{
c->final_out.final_3508out[i] = PID_calc(&(c->pid.chassis_3508VecPID[i]),c->motorfeedback.rotor_rpm3508[i],c->hopemotorout.OmniSpeedOut[i]);
out->motor_CHASSIS3508 .as_array[i] = c->final_out.final_3508out[i];
}
for (uint8_t i = 0; i < 4; i++) {
c->final_out.final_3508out[i] = PID_calc(&(c->pid.chassis_3508VecPID[i]),
c->motorfeedback.rotor_rpm3508[i],
c->hopemotorout.OmniSpeedOut[i]);
out->motor_CHASSIS3508.as_array[i] = c->final_out.final_3508out[i];
}
// c->vofa_send[0]=c->pos088.bmi088.gyro.z; c->vofa_send[0] = (fp32)c->sick_cali.sick_dis[1] - c->sick_cali.sick_dis[2];
return CHASSIS_OK;
// c->vofa_send[1]=c->motorfeedback .rotor_rpm3508 [0];
// c->vofa_send[2]=c->motorfeedback .rotor_rpm3508 [1];
// c->vofa_send[3]=c->motorfeedback .rotor_rpm3508 [2];
// c->vofa_send[4]=c->motorfeedback .rotor_rpm3508 [3];
//
// c->vofa_send[0]=c->hopemotorout .OmniSpeedOut [0];
// c->vofa_send[1]=c->hopemotorout .OmniSpeedOut [0];
//// c->vofa_send[2]=c->hopemotorout .OmniSpeedOut [2];
// c->vofa_send[3]=c->hopemotorout .OmniSpeedOut [3];
//// c->vofa_send[4]=c->motorfeedback .rotor_rpm3508 [0];
// c->vofa_send[0]=c->motorfeedback .rotor_rpm3508 [0];
//// c->vofa_send[6]=c->motorfeedback .rotor_rpm3508 [2];
// c->vofa_send[2]=c->motorfeedback .rotor_rpm3508 [3];
//
// c->vofa_send[5]=c->pos088 .imu_eulr .yaw ;
return CHASSIS_OK;
} }
pid_type_def pid;
pid_param_t pid_param={
.p = 0.50f,
.i = 0.0f, /*
.d = 0.0f, sick0,下
.i_limit = 50.0f, sick1,左2
.out_limit = 100.0f sick2,左1
};
fp32 jiuzheng(fp32 yaw)
*/
int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
{ {
static fp32 pian_yaw=0; if (c == NULL) return CHASSIS_ERR_NULL;
static fp32 shang_yaw=0; if (ctrl == NULL) return CHASSIS_ERR_NULL;
static int is=0;
if(is==0) fp32 sick0 = c->sick_cali.sick_dis[0] ;
{ fp32 sick1 = c->sick_cali.sick_dis[1];
PID_init (&pid,PID_POSITION ,&pid_param); fp32 sick2 = c->sick_cali.sick_dis[2];
is=1; const sickparam_t *param = &c->sick_cali.sickparam;
}
fp32 diff_yaw = -(fp32)(sick1 - sick2);
pian_yaw+= (yaw - shang_yaw); fp32 diff_y = -(fp32)(sick1 - c->sick_cali.sickparam.y_set);
shang_yaw=yaw ; fp32 diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set);
return PID_calc(&pid,pian_yaw,0);
c->move_vec.Vw = (fabsf(diff_yaw) > param->w_error) ? PID_calc(&(c->pid.SickCaliWzPID), diff_yaw, 0) : 0;
c->move_vec.Vx = (fabsf(diff_x) > param->xy_error) ? PID_calc(&(c->pid.SickCaliVxPID), diff_x, 0) : 0;
c->move_vec.Vy = (fabsf(diff_y) > param->xy_error) ? PID_calc(&(c->pid.SickCaliVyPID), diff_y, 0) : 0;
static uint16_t reach_cnt = 0;
if (fabsf(diff_yaw) <= param->w_error &&
fabsf(diff_x) <= param->xy_error &&
fabsf(diff_y) <= param->xy_error) {
reach_cnt++;
if (reach_cnt >= 50) {
c->sick_cali.is_reach = 1;
}
} else {
reach_cnt = 0;
c->sick_cali.is_reach = 0;
}
return CHASSIS_OK;
} }

55
User/Module/Chassis.h
View File

@ -58,7 +58,12 @@ typedef struct {
AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制） AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
}ChassisImu_t; }ChassisImu_t;
typedef struct {
int32_t w_error;//角度纠正时的允许误差量
int32_t xy_error;//xy允许误差量
int32_t x_set;
int32_t y_set;
}sickparam_t;
/* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */ /* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */
typedef struct typedef struct
@ -74,10 +79,13 @@ typedef struct
pid_param_t NaviVx_param; pid_param_t NaviVx_param;
pid_param_t NaviVy_param; pid_param_t NaviVy_param;
pid_param_t NaviVw_param; pid_param_t NaviVw_param;
pid_param_t Sick_CaliWparam;
pid_param_t Sick_CaliYparam; pid_param_t Sick_CaliYparam;
pid_param_t Sick_CaliXparam; pid_param_t Sick_CaliXparam;
sickparam_t sickparam;
}Chassis_Param_t; }Chassis_Param_t;
@ -90,7 +98,11 @@ typedef struct
fp32 Vw; fp32 Vw;
fp32 mul;//油门倍率 fp32 mul;//油门倍率
}ChassisMove_Vec; }ChassisMove_Vec;
typedef struct
{
fp32 send[4];
}NUC_send_t;
/** /**
* @brief * @brief
@ -144,12 +156,10 @@ typedef struct{
pid_type_def chassis_NaviWzPID; pid_type_def SickCaliWzPID;
pid_type_def chassis_NaviVxPID; pid_type_def SickCaliVxPID;
pid_type_def chassis_NaviVyPID; pid_type_def SickCaliVyPID;
pid_type_def sick_CaliforYPID;
pid_type_def sick_CaliforXPID;
}pid; }pid;
@ -157,45 +167,28 @@ typedef struct{
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */ LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
struct {
int32_t sick_dis[4]; //获取到的sick激光值 int32_t sick_dis[4]; //获取到的sick激光值
sickparam_t sickparam;
int is_reach;
}sick_cali;
NUC_send_t NUC_send;
}Chassis_t; }Chassis_t;
/** int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq);
* @brief
*
* @param c
* @param param
* @param mech_zero
* @param wheelPolar
* @return
*/
int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq);
/**
* \brief
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can); int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can);
/**
* \brief
*/
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out); int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out);
fp32 jiuzheng(fp32 yaw); int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) ;
/// @brief
/// @param c
void vesc_current_detection(Chassis_t *c);
#endif #endif

68
User/Module/config.c
View File

@ -95,13 +95,13 @@ static const ConfigParam_t param ={
/*上层其他参数*/ /*上层其他参数*/
/*运球*/ /*运球*/
.DribbleConfig_Config = { .DribbleCfg = {
.dribble_flag=0, .dribble_flag=0,
.m3508_init_angle = -5, .m3508_init_ang = -5,
.m3508_translate_angle = -930, .m3508_translate_angle = 1000,
.m3508_dribble_Reverse_speed=-3500, .rev_spd=-2000,
. m3508_dribble_speed= 4000, // 转动速度 .spd= 4200, // 转动速度
.m3508_dribble_init_speed=0, .init_spd=0,
.light_3508_flag=0,//3508平移处的光电，0初始，1到位置 .light_3508_flag=0,//3508平移处的光电，0初始，1到位置
.light_ball_flag=0,//检测球的flag .light_ball_flag=0,//检测球的flag
@ -109,21 +109,21 @@ static const ConfigParam_t param ={
}, },
/*投球*/ /*投球*/
.PitchConfig_Config = { .PitchCfg = {
.m2006_init_angle =-90, .m2006_init =-100, //释放的角度
.m2006_trigger_angle =0, .m2006_trig =0, //可以拉住发射的角度
.go1_init_position = -50, .go_init = -50, //仅用在go上电，初始位置
.go1_Receive_ball = -5, //偏下 .go_pull_pos =-210,
.go1_release_threshold =-210, .Pitch_angle =58,
.m2006_Screw_init=0,
.Pitch_angle =56, .pull_speed =5,
}, },
}, },
.chassis = {/**/ .chassis = {/**/
.M3508_param = { .M3508_param = {
.p = 15.1f, .p = 15.1f,
.i = 0.02f, .i = 0.02f,
@ -132,21 +132,49 @@ static const ConfigParam_t param ={
.out_limit =6000.0f, .out_limit =6000.0f,
}, },
/*视觉*/ /*视觉*/
.chassis_PICKWzPID_HIGN_param ={ //高响应 .chassis_PICKWzPID_HIGN_param ={
.p = 1.0f, .p = 1.0f,
.i = 0.03f, .i = 0.03f,
.d = 0.03f, .d = 0.03f,
.i_limit = 100.0f, .i_limit = 100.0f,
.out_limit =2000.0f, .out_limit =2000.0f,
}, },
.chassis_PICKWzPID_LOW_param ={ //高精度 .chassis_PICKWzPID_LOW_param ={
.p = 0.5f, //1.0 0.5 .p = 0.5f, //1.0 0.5
.i = 0.5f, //0.01 0.04 .i = 0.5f, //0.01 0.04
.d = 0.0f, //0.02 0.02 .d = 0.0f, //0.02 0.02
.i_limit = 50.0f, .i_limit = 50.0f,
.out_limit =1000.0f, .out_limit =1000.0f,
}, },
.Sick_CaliWparam ={
.p = 4.5f,
.i = 0.005f,
.d = 0.0f,
.i_limit = 500.0f,
.out_limit =1000.0f,
},
.Sick_CaliYparam ={
.p = 2.9f,
.i = 0.0051f,
.d = 0.0f,
.i_limit = 500.0f,
.out_limit =3000.0f,
},
.Sick_CaliXparam ={
.p = 2.9f,
.i = 0.0065f,
.d = 0.0f,
.i_limit = 500.0f,
.out_limit =3000.0f,
},
.sickparam={
.w_error=5,
.xy_error=5,
.x_set=927,
.y_set=1255,
},
}, },

232
User/Module/up.c
View File

@ -53,17 +53,17 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
// 初始化上层状态机 // 初始化上层状态机
if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球 if (!u->DribbleCont .is_init) { //检查是否为第一次运行状态机，运球
u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值 u->DribbleCont .DribbleConfig = u->param ->DribbleCfg ;//赋值
u->DribbleContext .DribbleState = DRIBBLE_PREPARE; u->DribbleCont .DribbleState = DRIBBLE_PREPARE;
u->DribbleContext .is_initialized = 1; u->DribbleCont .is_init = 1;
} }
if (!u->PitchContext .is_initialized) { if (!u->Pitch_Cfg .is_init) {
u->PitchContext .PitchConfig = u->param ->PitchConfig_Config ;//赋值 u->Pitch_Cfg .PitchConfig = u->param ->PitchCfg ;//赋值
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球 u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球
u->PitchContext .is_initialized = 1; u->Pitch_Cfg .is_init = 1;
} }
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层 BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
@ -96,8 +96,8 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
u->cmd =c; u->cmd =c;
/*光电状态更新*/ /*光电状态更新*/
u->DribbleContext .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin); u->DribbleCont .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin);
u->DribbleContext .DribbleConfig .light_3508_flag =HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin); u->DribbleCont .DribbleConfig .light_3508_flag =HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin);
return 0; return 0;
@ -168,17 +168,8 @@ int8_t GO_SendData(UP_t *u, float pos, float limit)
static int is_calibration=0; static int is_calibration=0;
static fp32 error=0; //误差量 static fp32 error=0; //误差量
// GO_MotorData_t *GO_calibration;//校准前，原始数据
u->motorfeedback .go_data = get_GO_measure_point() ; u->motorfeedback .go_data = get_GO_measure_point() ;
// if(is_calibration==0)
// {
// is_calibration=HAL_GPIO_ReadPin (GPIOE ,GPIO_PIN_9 );
// u->go_cmd.Pos = -50; //上电之后跑
// error= GO_calibration->Pos ;
// }
// u->motorfeedback .go_data = GO_calibration;
// u->motorfeedback .go_data ->Pos= error ;
// u->go_cmd.Pos = pos;
// 读取参数 // 读取参数
float tff = u->go_cmd.T; // 前馈力矩 float tff = u->go_cmd.T; // 前馈力矩
@ -214,7 +205,7 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
DJ_Speed_Control(u,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed[0],u->motor_target .Dribble_M3508_speed[0]); DJ_Speed_Control(u,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed[0],u->motor_target .Dribble_M3508_speed[0]);
DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed[1],u->motor_target .Dribble_M3508_speed[1]); DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed[1],u->motor_target .Dribble_M3508_speed[1]);
DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed[2],u->motor_target .Dribble_M3508_speed[2]); DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed[2],u->motor_target .Dribble_M3508_speed[2]);
DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] , DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] ,
&u->pid .Dribble_translate_M3508_angle , &u->pid .Dribble_translate_M3508_angle ,
&u->pid .Dribble_translate_M3508_speed , &u->pid .Dribble_translate_M3508_speed ,
u->motor_target .Dribble_translate_M3508_angle ); u->motor_target .Dribble_translate_M3508_angle );
@ -230,8 +221,10 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
(PID_calc (&(u->pid .Pitch_M2006_angle ), (PID_calc (&(u->pid .Pitch_M2006_angle ),
u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle)) u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle))
); );
GO_SendData(u,u->motor_target .go_shoot,5 ); GO_SendData(u,
u->motor_target .go_shoot,
u->Pitch_Cfg .PitchConfig .pull_speed );//对应的拉动速度
for(int i=0;i<4;i++){ for(int i=0;i<4;i++){
@ -249,75 +242,78 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
} }
int a=0;
int b=0;
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
{ {
if(u ==NULL) return 0; if(u ==NULL) return 0;
if(out ==NULL) return 0;
if(c ==NULL) return 0;
/*指针简化*/
PitchCfg_t *pitch_cfg = &u->Pitch_Cfg.PitchConfig;
DribbleCfg_t *DribbleCont = &u->DribbleCont.DribbleConfig;
up_motor_target_t *target=&u->motor_target ;
static int is_pitch=1; static int is_pitch=1;
/*config值限位*/
abs_limit_min_max_fp(&u->Pitch_Cfg.PitchConfig.go_release_pos ,-208.0f,2.0f);
switch (c->CMD_CtrlType ) switch (c->CMD_CtrlType )
{ {
case RCcontrol: //在手动模式下 case RCcontrol: //在手动模式下
switch (c-> CMD_mode ) switch (c-> CMD_mode )
{ {
case Normal : case Normal :
/*投篮*/ /*投篮*/
if(is_pitch){ if(is_pitch){
u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ; target->go_shoot =pitch_cfg->go_init ;
u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle; target->Shoot_Pitch_angle=pitch_cfg->Pitch_angle;
is_pitch=0; is_pitch=0;
} //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删 } //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删
u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ; target->Shoot_M2006_angle =u->Pitch_Cfg .PitchConfig .m2006_init ;
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球 u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球
/*运球*/ /*运球*/
RELAY1_TOGGLE(0);//关闭气缸 RELAY1_TOGGLE(0);//关闭气缸
for(int i=0;i<3;i++){ for(int i=0;i<3;i++){
u->motor_target.Dribble_M3508_speed[i]=0; target->Dribble_M3508_speed[i]=0;
} }
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_translate_angle; target->Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig .m3508_translate_angle;
u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态 u->DribbleCont .DribbleState = DRIBBLE_PREPARE; //重置最初状态
break; break;
case Pitch : case Pitch :
if (u->PitchContext .PitchState ==PITCH_PREPARE) if (u->Pitch_Cfg .PitchState ==PITCH_PREPARE)
{ {
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮 u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
} }
Pitch_Process(u,out); Pitch_Process(u,out);
// if (u->PitchContext .PitchState ==PITCH_PREPARE)
// {
// u->CoContext .CoState =CoTRANSLATE_OUT;
// }
// Co_Process(u,out);
break ; break ;
case UP_Adjust: //测试用 case UP_Adjust: //测试用
pitch_cfg ->pull_speed=5;
u->PitchContext.PitchConfig.go1_Receive_ball += c->Vx ; pitch_cfg ->go_release_pos += c->Vx ;
u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100; pitch_cfg->Pitch_angle += c->Vy/100;
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_Receive_ball; target->go_shoot=u->Pitch_Cfg.PitchConfig.go_release_pos;
u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle; target->Shoot_Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle;
break ; break ;
case Dribble: case Dribble:
{ {
if(u->DribbleContext.DribbleState== DRIBBLE_PREPARE){ if(u->DribbleCont.DribbleState== DRIBBLE_PREPARE){
u->DribbleContext .DribbleState=DRIBBLE_TRANSLATE; u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;
} }
//光电状态更新 //光电状态更新
Dribble_Process(u,out); Dribble_Process(u,out);
@ -332,23 +328,28 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
switch(c-> CMD_mode) switch(c-> CMD_mode)
{ {
case AUTO_MID360_Pitch: case AUTO_MID360_Pitch:
u->PitchContext .PitchConfig .go1_Receive_ball=LowPassFilter2p_Apply(&u->filled[0],c->pos); pitch_cfg ->go_init=LowPassFilter2p_Apply(&u->filled[0],c->pos);
if (u->PitchContext .PitchState ==PITCH_PREPARE) if (u->Pitch_Cfg .PitchState ==PITCH_PREPARE)
{ {
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮 u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
} }
/*根据距离实时计算所需pos*/ /*根据距离实时计算所需pos*/
// u->PitchContext .PitchConfig .go1_release_threshold=c->pos; pitch_cfg ->go_release_pos=c->pos;
// if(u->CoContext .CoState == CoPITCH){
// u->CoContext.CoState=CoPITCH_MID360;
// }
// Co_Process(u,out);
Pitch_Process(u,out); Pitch_Process(u,out);
break ; break ;
case AUTO_MID360: case AUTO_MID360:
u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ; target->Shoot_M2006_angle =pitch_cfg ->m2006_init ;
u->PitchContext .PitchState = PITCH_PREPARE;
u->Pitch_Cfg .PitchState = PITCH_PREPARE;
u->DribbleCont .DribbleState = DRIBBLE_PREPARE;
break ; break ;
@ -372,22 +373,24 @@ return 0;
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out) int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
{ {
switch(u->PitchContext .PitchState){ PitchCfg_t *cfg = &u->Pitch_Cfg.PitchConfig;
PitchState_t *state =&u->Pitch_Cfg .PitchState;
up_motor_target_t *target=&u->motor_target ;
switch(*state){
case PITCH_START: case PITCH_START:
// u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 // u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动
cfg->pull_speed=10;
u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold; target->go_shoot = cfg->go_pull_pos;
if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改 if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
u->motor_target .Shoot_M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0 target->Shoot_M2006_angle = cfg->m2006_trig ;//设置2006角度，0
u->Pitch_Cfg .PitchConfig .pull_speed=6;
u->PitchContext .PitchState=PITCH_PULL_TRIGGER; *state=PITCH_PULL_TRIGGER;
}//更改标志位 }//更改标志位
break ; break ;
@ -396,16 +399,16 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1) //当2006的总角度小于1，可以认为已经勾上,误差为1 if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1) //当2006的总角度小于1，可以认为已经勾上,误差为1
{ {
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_Receive_ball; target->go_shoot=cfg->go_release_pos;
if(is_reached (u->motorfeedback .go_data ->Pos ,u->motor_target .go_shoot ,1.0f)) if(is_reached (u->motorfeedback .go_data ->Pos ,target->go_shoot ,1.0f))
{ {
u->PitchContext .PitchState=PITCH_LAUNCHING; *state=PITCH_LAUNCHING;
} }
} }
break ; break ;
case PITCH_LAUNCHING: //等待发射 case PITCH_LAUNCHING: //等待发射
u->PitchContext .PitchState=PITCH_COMPLETE; // *state=PITCH_COMPLETE;
break ; break ;
case PITCH_COMPLETE: //发射完成 case PITCH_COMPLETE: //发射完成
@ -432,41 +435,43 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
static int common_speed_flag=0;//是否共速 static int common_speed_flag=0;//是否共速
static int common_speed_reverse_flag=0;//是否共速 static int common_speed_reverse_flag=0;//是否共速
DribbleCfg_t *DribbleCfg = &u->DribbleCont.DribbleConfig;
up_motor_target_t *target=&u->motor_target ;
DribbleState_t *DribbleState=&u->DribbleCont.DribbleState;
switch (u->DribbleContext.DribbleState) { switch (u->DribbleCont.DribbleState) {
case DRIBBLE_TRANSLATE: case DRIBBLE_TRANSLATE:
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 target->Dribble_translate_M3508_angle =DribbleCfg->m3508_translate_angle;//平行移动
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主 if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主
{ {
u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态 u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态
} }
break; break;
case DRIBBLE_PROCESS_DOWN: case DRIBBLE_PROCESS_DOWN:
u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_speed; target->Dribble_M3508_speed[0]=DribbleCfg->spd;
u->motor_target.Dribble_M3508_speed[1]=-u->DribbleContext .DribbleConfig.m3508_dribble_speed; target->Dribble_M3508_speed[1]=DribbleCfg->spd;
u->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_speed; target->Dribble_M3508_speed[2]=DribbleCfg->spd;
if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm, if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm,
u->motorfeedback .DJmotor_feedback [1].rpm, u->motorfeedback .DJmotor_feedback [1].rpm,
u->motorfeedback .DJmotor_feedback [2].rpm, u->motorfeedback .DJmotor_feedback [2].rpm,
u->motor_target.Dribble_M3508_speed [0], DribbleCfg->spd,
u->motor_target.Dribble_M3508_speed [1], DribbleCfg->spd,
u->motor_target.Dribble_M3508_speed[2], DribbleCfg->spd, 50.0f,50)
50.0f,50)
) { ) {
RELAY1_TOGGLE(1); //速度达到后打开气缸 RELAY1_TOGGLE(1); //速度达到后打开气缸
common_speed_flag =1; common_speed_flag =1;
} }
if(common_speed_flag){ if(common_speed_flag){
if(u->DribbleContext .DribbleConfig .light_ball_flag == 0){//球下落检测，反转 if(u->DribbleCont .DribbleConfig .light_ball_flag == 0){//球下落检测，反转
u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed; target->Dribble_M3508_speed[0]=DribbleCfg->rev_spd;
u->motor_target.Dribble_M3508_speed[1]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed; target->Dribble_M3508_speed[1]=DribbleCfg->rev_spd;
u->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed; target->Dribble_M3508_speed[2]=DribbleCfg->rev_spd;
u->DribbleContext .DribbleState=DRIBBLE_PROCESS_UP; *DribbleState=DRIBBLE_PROCESS_UP;
} }
@ -485,21 +490,21 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
} }
if(common_speed_reverse_flag){ if(common_speed_reverse_flag){
if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){ if(DribbleCfg->light_ball_flag == 1){
u->DribbleContext .DribbleState=DRIBBLE_DONE; *DribbleState=DRIBBLE_DONE;
RELAY1_TOGGLE(0); //关闭气缸 RELAY1_TOGGLE(0); //关闭气缸
} }
} }
break ; break ;
case DRIBBLE_DONE: case DRIBBLE_DONE:
common_speed_reverse_flag=0; common_speed_reverse_flag=0;
for(int i=0;i<3;i++){ for(int i=0;i<3;i++){
u->motor_target.Dribble_M3508_speed[i]=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0 target->Dribble_M3508_speed[i]=DribbleCfg->init_spd ;//三摩擦速度归0
} }
/*标志位清零*/ /*标志位清零*/
u->DribbleContext.DribbleConfig.light_ball_flag=0; DribbleCfg->light_ball_flag=0;
u->DribbleContext.DribbleConfig.light_3508_flag=0; DribbleCfg->light_3508_flag=0;
break; break;
@ -515,7 +520,10 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
// CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置 // CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
// CoPITCH, //发射 // CoPITCH, //发射
// CoDONE, // CoDONE,
//int8_t Trans_Process (UP_t *u, CAN_Output_t *out)
//{
//
//}
int8_t Co_Process(UP_t *u, CAN_Output_t *out) int8_t Co_Process(UP_t *u, CAN_Output_t *out)
{ {
switch(u->CoContext .CoState ) switch(u->CoContext .CoState )
@ -524,12 +532,12 @@ int8_t Co_Process(UP_t *u, CAN_Output_t *out)
break ; break ;
case CoTRANSLATE_OUT: case CoTRANSLATE_OUT:
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主 if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle > 500)//速度为0认为卡主
{ {
u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，运球转移状态 u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，运球转移状态
u->PitchContext .PitchState = PITCH_START; u->Pitch_Cfg .PitchState = PITCH_START;
u->CoContext .CoState =CoDRIBBLE; u->CoContext .CoState =CoDRIBBLE;
} }
@ -537,17 +545,17 @@ int8_t Co_Process(UP_t *u, CAN_Output_t *out)
case CoDRIBBLE: case CoDRIBBLE:
Dribble_Process(u,out); //状态停在DRIBBLE_DONE Dribble_Process(u,out); //状态停在DRIBBLE_DONE
Pitch_Process(u,out); //状态停在PITCH_PULL_TRIGGER Pitch_Process(u,out); //状态停在PITCH_LAUNCHING
//状态停在对应位置时,平移去给发射送球 //状态停在对应位置时,平移去给发射送球
if(u->DribbleContext .DribbleState ==DRIBBLE_DONE && u->PitchContext.PitchState ==PITCH_LAUNCHING) if(u->DribbleCont .DribbleState ==DRIBBLE_DONE && u->Pitch_Cfg.PitchState ==PITCH_LAUNCHING)
{ {
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle; u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig .m3508_init_ang;
} }
if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleContext .DribbleConfig.m3508_init_angle,1.0f)) if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleCont .DribbleConfig.m3508_init_ang,1.0f))
{ {
u->motor_target.Dribble_M3508_speed[0]=1000; u->motor_target.Dribble_M3508_speed[0]=1000;
u->motor_target.Dribble_M3508_speed[1]=-1000; u->motor_target.Dribble_M3508_speed[1]=1000;
u->motor_target.Dribble_M3508_speed[2]=-1000; u->motor_target.Dribble_M3508_speed[2]=1000;
u->CoContext .CoState =CoTRANSLATE_IN; u->CoContext .CoState =CoTRANSLATE_IN;
} }
@ -565,26 +573,32 @@ int8_t Co_Process(UP_t *u, CAN_Output_t *out)
) { ) {
RELAY1_TOGGLE(1); //速度达到后打开气缸,送给发射 RELAY1_TOGGLE(1); //速度达到后打开气缸,送给发射
} }
if(u->DribbleContext .DribbleConfig.light_ball_flag == 0) if(u->DribbleCont .DribbleConfig.light_ball_flag == 0)
{ {
u->motor_target.Dribble_M3508_speed[0]=0; u->motor_target.Dribble_M3508_speed[0]=0;
u->motor_target.Dribble_M3508_speed[1]=0; u->motor_target.Dribble_M3508_speed[1]=0;
u->motor_target.Dribble_M3508_speed[2]=0; u->motor_target.Dribble_M3508_speed[2]=0;
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动
} }
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主,卡到最右端 if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle > 900)//速度为0认为卡主,卡到最右端
{ {
u->CoContext .CoState =CoPITCH; u->CoContext .CoState =CoPITCH;
} }
break ; break ;
case CoPITCH: case CoPITCH:
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
case CoPITCH_MID360:
u->motor_target .go_shoot=u->Pitch_Cfg.PitchConfig.go_init;
break ;
break ; break ;
} }

166
User/Module/up.h
View File

@ -27,13 +27,14 @@
*/ */
/*配合过程状态,co是合作的意思*/ /*配合过程状态*/
typedef enum { typedef enum {
CoPREPARE, // 准备阶段 CoPREPARE, // 准备阶段
CoTRANSLATE_OUT,//上方平移,去运球 CoTRANSLATE_OUT,//上方平移,去运球
CoDRIBBLE, //运球和蓄力阶段,再平移回去 CoDRIBBLE, //运球和蓄力阶段,再平移回去
CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置 CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
CoPITCH, //发射 CoPITCH, //发射
CoPITCH_MID360, //雷达发射
CoDONE, CoDONE,
@ -42,8 +43,8 @@ typedef enum {
typedef struct { typedef struct {
CoState_t CoState; CoState_t CoState;
uint8_t is_initialized ; uint8_t is_init ;
} CoContext_t; } CoCon_t;
@ -61,30 +62,32 @@ typedef enum {
/* 投球参数配置 */ /* 投球参数配置 */
typedef struct { typedef struct {
fp32 m2006_init_angle; // M2006初始角度-140 fp32 m2006_init; // M2006初始角度
fp32 m2006_trigger_angle; // M2006触发角度0,用来合并扳机 fp32 m2006_trig; // M2006触发角度0,用来合并扳机
fp32 go1_init_position; // GO电机触发位置,0,初始位置 fp32 go_init; // GO电机触发位置,初始位置，后续更改用于发射的位置
fp32 go1_release_threshold; // go上升去合并扳机，扳机位置 fp32 go_pull_pos; // go上升去合并扳机，扳机位置
fp32 go1_Receive_ball; //用在配合过程，用来接平移后的球
fp32 m2006_Screw_init;
fp32 Pitch_angle; fp32 Pitch_angle;
} PitchConfig_t; fp32 pull_speed;//go速度
fp32 go_release_pos;//go松开，发射位置
} PitchCfg_t;
//fp32 go_pull_pos;
// fp32 m2006_init; // M2006初始角度
// fp32 m2006_trig; // M2006触发角度
// fp32 go_init; // GO初始位置
// fp32 go_release; // GO释放阈值
// fp32 go_recv; // GO接球位置
// fp32 screw_init; // 螺杆初始值
// fp32 pitch_ang; // 投球角度
// fp32 pull_spd; // 拉动速度
/* 投球控制上下文 */ /* 投球控制上下文 */
typedef struct { typedef struct {
PitchState_t PitchState; PitchState_t PitchState;
PitchConfig_t PitchConfig; PitchCfg_t PitchConfig;
uint8_t is_initialized ;
} PitchContext_t;
uint8_t is_init ;
} Pitch_Cfg_t;
/*运球*/ /*运球*/
typedef enum { typedef enum {
@ -100,27 +103,27 @@ typedef enum {
typedef struct { typedef struct {
int8_t dribble_flag;//当移动三摩擦后为1，否则为0，防止发射撞到 int8_t dribble_flag;//当移动三摩擦后为1，否则为0，防止发射撞到
fp32 m3508_init_angle; // 平移前位置 fp32 m3508_init_ang; // 3508平移前位置
fp32 m3508_translate_angle; // 平移后位置 fp32 m3508_translate_angle; // 平移后位置
fp32 m3508_dribble_init_speed; fp32 init_spd;
fp32 m3508_dribble_speed; // 转动速度 fp32 spd; // 转动速度
fp32 m3508_dribble_Reverse_speed; fp32 rev_spd;
/*光电标志位，初始值均为0，触发为1*/ /*光电标志位，初始值均为0，触发为1*/
int light_3508_flag;//3508平移处的光电，0初始，1到位置 int light_3508_flag;//3508平移处的光电，0初始，1到位置
int light_ball_flag;//检测球的flag int light_ball_flag;//检测球的flag
} DribbleConfig_t; } DribbleCfg_t;
/* 状态机上下文 */ /* 状态机上下文 */
typedef struct { typedef struct {
DribbleState_t DribbleState; DribbleState_t DribbleState;
DribbleConfig_t DribbleConfig; DribbleCfg_t DribbleConfig;
uint8_t is_initialized; uint8_t is_init;
} DribbleContext_t; } DribbleCont_t;
typedef struct { typedef struct {
@ -151,57 +154,12 @@ typedef struct
pid_param_t Dribble_translate_M3508_angle_param;//平移用的角度环 pid_param_t Dribble_translate_M3508_angle_param;//平移用的角度环
DribbleConfig_t DribbleConfig_Config; DribbleCfg_t DribbleCfg;
PitchConfig_t PitchConfig_Config; PitchCfg_t PitchCfg;
GO_MotorCmd_t go_cmd; GO_MotorCmd_t go_cmd;
}UP_Param_t; }UP_Param_t;
typedef struct
{
fp32 ecd;
fp32 rpm;
uint8_t id;
fp32 orig_angle;
fp32 last_angle;
int32_t round_cnt;
int init_cnt;
fp32 total_angle;
}DJmotor_feedback_t;
typedef struct{ typedef struct{
uint8_t up_task_run;
const UP_Param_t *param;
CoContext_t CoContext;
/*运球过程*/
DribbleContext_t DribbleContext;
/*投篮过程*/
PitchContext_t PitchContext;
CMD_t *cmd;
struct{
struct{
fp32 VESC_5065_M1_rpm;
fp32 VESC_5065_M2_rpm;
}VESC;
GO_MotorData_t *go_data;//存放go电机数据
DJmotor_feedback_t DJmotor_feedback[6];
struct {
uint32_t ecd;
float angle;
}Encoder;
}motorfeedback;
struct{
fp32 VESC_5065_M1_rpm; fp32 VESC_5065_M1_rpm;
fp32 VESC_5065_M2_rpm; fp32 VESC_5065_M2_rpm;
@ -215,9 +173,9 @@ typedef struct{
fp32 Shoot_Pitch_angle; fp32 Shoot_Pitch_angle;
}motor_target; }up_motor_target_t;
struct{ typedef struct{
pid_type_def VESC_5065_M1; pid_type_def VESC_5065_M1;
@ -237,11 +195,55 @@ typedef struct{
pid_type_def Dribble_translate_M3508_angle;//平移用的角度环 pid_type_def Dribble_translate_M3508_angle;//平移用的角度环
}pid; }up_pid_t;
GO_MotorCmd_t go_cmd; typedef struct
{
fp32 ecd;
fp32 rpm;
uint8_t id;
fp32 orig_angle;
fp32 last_angle;
int32_t round_cnt;
int init_cnt;
fp32 total_angle;
}DJmotor_feedback_t;
typedef struct{
uint8_t up_task_run;
const UP_Param_t *param;
CoCon_t CoContext;
/*运球过程*/
DribbleCont_t DribbleCont;
/*投篮过程*/
Pitch_Cfg_t Pitch_Cfg;
CMD_t *cmd;
struct{
struct{
fp32 VESC_5065_M1_rpm;
fp32 VESC_5065_M2_rpm;
}VESC;
GO_MotorData_t *go_data;//存放go电机数据
DJmotor_feedback_t DJmotor_feedback[6];
struct {
uint32_t ecd;
float angle;
}Encoder;
}motorfeedback;
up_pid_t pid;
up_motor_target_t motor_target;
GO_MotorCmd_t go_cmd;
/*经PID计算后的实际发送给电机的实时输出值*/ /*经PID计算后的实际发送给电机的实时输出值*/
struct struct
@ -257,8 +259,6 @@ typedef struct{
fp32 vofa_send[8]; fp32 vofa_send[8];
} UP_t; } UP_t;

54
User/device/can_use.c
View File

@ -1,3 +1,4 @@
/* /*
CAN总线上的设备1到7 CAN总线上的设备1到7
将所有CAN总线上挂载的设备抽象成单个设备进行管理和控制 将所有CAN总线上挂载的设备抽象成单个设备进行管理和控制
@ -32,7 +33,7 @@
#define CAN_M3508_MAX_ABS_CUR (20) #define CAN_M3508_MAX_ABS_CUR (20)
#define CAN_M2006_MAX_ABS_CUR (10) #define CAN_M2006_MAX_ABS_CUR (10)
#define CAN_ENCODER_RESOLUTION (32768) //欧艾迪编码器分辨率 #define CAN_ENCODER_RESOLUTION (4096) //欧艾迪编码器分辨率
CAN_RawRx_t raw_rx1;//原始的can数据 CAN_RawRx_t raw_rx1;//原始的can数据
CAN_RawRx_t raw_rx2; CAN_RawRx_t raw_rx2;
@ -56,31 +57,20 @@ static void CAN_DJIMotor_Decode(CAN_MotorFeedback_t *feedback,
feedback->torque_current = feedback->torque_current =
raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES; raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES;
} }
static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,uint16_t id, static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,
const uint8_t *raw) { const uint8_t *raw) {
if (feedback == NULL || raw == NULL) return; if (feedback == NULL || raw == NULL) return;
switch (id) static uint16_t sick_rec[4];
{
case CAN_SICK_ID_1:
feedback->raw_dis[0] = (uint16_t)(raw[0] << 8| raw[1]);
break ; sick_rec[0] = (uint16_t)(raw[0] << 8 | raw[1]);
sick_rec[1] = (uint16_t)(raw[2] << 8 | raw[3]);
case CAN_SICK_ID_2: sick_rec[2] = (uint16_t)(raw[4] << 8 | raw[5]);
feedback->raw_dis[1] = (uint16_t)(raw[0] << 8| raw[1]); sick_rec[3] = (uint16_t)(raw[6] << 8 | raw[7]);
break ; feedback ->raw_dis [0]= sick_rec[0];
feedback ->raw_dis [1]= sick_rec[1];
case CAN_SICK_ID_3: feedback ->raw_dis [2]= sick_rec[2];
feedback->raw_dis[2] = (uint16_t)(raw[0] << 8| raw[1]); feedback ->raw_dis [3]= sick_rec[3];
break ;
case CAN_SICK_ID_4:
feedback->raw_dis[3] = (uint16_t)(raw[0] << 8| raw[1]);
break ;
}
} }
void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback, void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
const uint8_t *raw) { const uint8_t *raw) {
@ -93,7 +83,8 @@ void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
{ {
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=(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; break;
} }
} }
@ -383,19 +374,10 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
CAN_DJIMotor_Decode(&(can->motor.chassis_motor3508.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;
case CAN_SICK_ID_1: case CAN_SICK_ID:
CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_1, can_rx->rx_data); CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);
break;
case CAN_SICK_ID_2:
CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_2, can_rx->rx_data);
break;
case CAN_SICK_ID_3:
CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_3, can_rx->rx_data);
break;
case CAN_SICK_ID_4:
CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_4, can_rx->rx_data);
break; break;
default: default:
break; break;
} }

8
User/device/can_use.h
View File

@ -18,10 +18,8 @@ typedef enum {
CAN_UP_M3508_M5_ID = 0x205, /* 1 */ CAN_UP_M3508_M5_ID = 0x205, /* 1 */
CAN_UP_M3508_M6_ID = 0x206, /* 2 */ CAN_UP_M3508_M6_ID = 0x206, /* 2 */
CAN_SICK_ID_1=0x301, CAN_SICK_ID=0x305,
CAN_SICK_ID_2=0x302,
CAN_SICK_ID_3=0x303,
CAN_SICK_ID_4=0x304,
CAN_Encoder_ID=0x01, CAN_Encoder_ID=0x01,
@ -200,6 +198,6 @@ void CAN_Encoder_Control(CAN_t *can);
void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback, void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
const uint8_t *raw) ; const uint8_t *raw) ;
static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,uint16_t id, static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,
const uint8_t *raw) ; const uint8_t *raw) ;
#endif #endif

3
User/device/cmd.c
View File

@ -198,13 +198,14 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
else if(rc->LD.key_E ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360_Pitch; 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 if(rc->LD .key_E ==CMD_SW_MID ) cmd ->CMD_mode =AUTO_MID360;
else cmd ->CMD_mode =Normal ; else cmd ->CMD_mode =Normal ;
}
} }
else if(cmd ->CMD_CtrlType ==RCcontrol){ else if(cmd ->CMD_CtrlType ==RCcontrol){
/*手动下的*/ /*手动下的*/
if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode=Dribble ; if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode=Dribble ;
else if(rc->LD.key_G ==CMD_SW_UP) cmd ->CMD_mode =Pitch; else if(rc->LD.key_G ==CMD_SW_UP) cmd ->CMD_mode =Pitch;
else if(rc->LD .key_H==CMD_SW_UP) cmd ->CMD_mode =UP_Adjust; else if(rc->LD .key_H==CMD_SW_UP) cmd ->CMD_mode =UP_Adjust;
else if(rc->LD.key_C == CMD_SW_UP) cmd->CMD_mode =Chassis_Adjust;
else cmd ->CMD_mode =Normal; else cmd ->CMD_mode =Normal;
} }
} }

1
User/device/cmd.h
View File

@ -28,6 +28,7 @@ typedef enum{
AUTO_MID360_Adjust,//雷达调整 AUTO_MID360_Adjust,//雷达调整
UP_Adjust,//上层调整 UP_Adjust,//上层调整
Chassis_Adjust,
Dribble , //运球 Dribble , //运球
Pitch, //投篮,底盘锁定 Pitch, //投篮,底盘锁定

8
User/device/nuc.c
View File

@ -6,7 +6,8 @@ static volatile uint32_t drop_message = 0;
static osThreadId_t thread_alert; static osThreadId_t thread_alert;
uint8_t nucbuf[31]; static uint8_t nucbuf[31];
static char SendBuffer[19];
static void NUC_IdleCallback(void) { static void NUC_IdleCallback(void) {
@ -28,11 +29,8 @@ int8_t NUC_StartReceiving() {
return DEVICE_OK; return DEVICE_OK;
return DEVICE_ERR; return DEVICE_ERR;
} }
char SendBuffer[19];
int8_t NUC_StartSending(fp32 *data) { int8_t NUC_StartSending(fp32 *data) {
union union
{ {
float x[4]; float x[4];
@ -64,7 +62,7 @@ int8_t NUC_Restart(void) {
return DEVICE_OK; return DEVICE_OK;
} }
bool_t NUC_WaitDmaCplt(void) { bool_t NUC_WaitDmaCplt(void) {
return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) == return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,100) ==
SIGNAL_NUC_RAW_REDY); SIGNAL_NUC_RAW_REDY);
} }

78
User/device/vofa.c
View File

@ -5,37 +5,61 @@
//用来对需要的数据进行串口绘图 （未添加接收函数） //用来对需要的数据进行串口绘图 （未添加接收函数）
//float vofa_send[8];
////发送函数
//void vofa_tx_main(float *data)
//{
// float fdata[8]={0};
// uint8_t tail[4];
// tail[0]=0x00;
// tail[1]=0x00;
// tail[2]=0x80;
// tail[3]=0x7f;
// /*在下面添加发送的数据*/
// fdata[0] = data[0];
// fdata[1] = data[1];
// fdata[2] = data[2];
// fdata[3] = data[3];
// fdata[4] = data[4];
// fdata[5] = data[5];
// fdata[6] = data[6];
// fdata[7] = data[7];
//
//
// /*在下面使用对应的串口发送函数*/
//// 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);
//
//}
float vofa_send[8]; float vofa_send[8];
//发送函数 //发送函数
void vofa_tx_main(float *data) void vofa_tx_main(float *data)
{ {
float fdata[8]={0}; float fdata[8] = {0};
uint8_t tail[4]; uint8_t tail[4] = {0x00, 0x00, 0x80, 0x7f};
tail[0]=0x00;
tail[1]=0x00;
tail[2]=0x80;
tail[3]=0x7f;
/*在下面添加发送的数据*/
fdata[0] = data[0];
fdata[1] = data[1];
fdata[2] = data[2];
fdata[3] = data[3];
fdata[4] = data[4];
fdata[5] = data[5];
fdata[6] = data[6];
fdata[7] = data[7];
/*在下面使用对应的串口发送函数*/
// 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);
} for (int i = 0; i < 8; i++) {
fdata[i] = data[i];
}
uint8_t packet[8 * sizeof(float) + 4];
memcpy(packet, fdata, 8 * sizeof(float));
memcpy(packet + 8 * sizeof(float), tail, 4);
/*在下面使用对应的串口发送函数*/
// HAL_UART_Transmit_DMA(&huart6, packet, sizeof(packet));
// CDC_Transmit_FS( packet, sizeof(packet));
// osDelay(1);
}

1
User/task/can_task.c
View File

@ -47,7 +47,6 @@ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
//一问一答sick数据指令 //一问一答sick数据指令
// CAN_Sick_Control(&can);
CAN_Encoder_Control(&can); CAN_Encoder_Control(&can);
/*can设备数据存入队列*/ /*can设备数据存入队列*/

9
User/task/chassis_task.c
View File

@ -56,6 +56,8 @@ void Task_Chassis(void *argument)
{ {
#ifdef DEBUG #ifdef DEBUG
task_runtime.stack_water_mark.chassis = osThreadGetStackSpace(osThreadGetId()); task_runtime.stack_water_mark.chassis = osThreadGetStackSpace(osThreadGetId());
task_runtime.freq.chassis=TASK_FREQ_CHASSIS;
task_runtime.last_up_time.chassis=tick;
#endif #endif
/*imu数据获取*/ /*imu数据获取*/
@ -85,13 +87,14 @@ void Task_Chassis(void *argument)
osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);//清空队列 osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);//清空队列
osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor_CHASSIS3508, 0, 0); //发送数据 osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor_CHASSIS3508, 0, 0); //发送数据
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc_send);//清空队列
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc_send, &chassis.NUC_send, 0, 0); //发送数据
vofa_send[0] = chassis.vofa_send[0]; vofa_send[0] = chassis.vofa_send[0];
vofa_send[1] = chassis.vofa_send[1]; vofa_send[1] = chassis.vofa_send[1];
vofa_send[2] = chassis.vofa_send[2]; vofa_send[2] = chassis.vofa_send[2];
vofa_send[3] = chassis.vofa_send[3]; vofa_send[3] = chassis.vofa_send[3];
vofa_send[4] = chassis.vofa_send[4]; vofa_send[4] = chassis.vofa_send[4];
vofa_send[5] = chassis.vofa_send[5]; vofa_send[5] = chassis.vofa_send[5];
vofa_send[6] = chassis.vofa_send[6]; vofa_send[6] = chassis.vofa_send[6];
vofa_send[7] = chassis.vofa_send[7]; vofa_send[7] = chassis.vofa_send[7];

5
User/task/cmd_task.c
View File

@ -30,8 +30,9 @@ void Task_cmd(void *argument){
while(1){ while(1){
#ifdef DEBUG #ifdef DEBUG
/*记录任务所需要的栈空间*/ /*记录任务所需要的栈空间*/
task_runtime.stack_water_mark.cmd = task_runtime.stack_water_mark.cmd =osThreadGetStackSpace(osThreadGetId());
osThreadGetStackSpace(osThreadGetId()); task_runtime.last_up_time.cmd=tick;
task_runtime.freq.cmd=TASK_FREQ_CTRL_CMD;
#endif #endif
osKernelLock(); /*锁住RTOS内核调度*/ osKernelLock(); /*锁住RTOS内核调度*/

4
User/task/init.c
View File

@ -85,7 +85,9 @@ void Task_Init(void *argument) {
task_runtime.msgq.cmd.raw.nuc = task_runtime.msgq.cmd.raw.nuc =
osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL); osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL);
task_runtime.msgq.cmd.raw.nuc_send =
osMessageQueueNew(3u,sizeof(NUC_send_t), NULL);
osKernelUnlock(); osKernelUnlock();
osThreadTerminate(osThreadGetId()); /* 结束自身 */ osThreadTerminate(osThreadGetId()); /* 结束自身 */
} }

19
User/task/nuc_task.c
View File

@ -5,32 +5,38 @@
NUC_t nuc_raw; NUC_t nuc_raw;
CMD_NUC_t cmd_fromnuc; CMD_NUC_t cmd_fromnuc;
NUC_send_t NUC_send;
#else #else
static NUC_t nuc_raw; static NUC_t nuc_raw;
static CMD_NUC_t cmd_fromnuc; static CMD_NUC_t cmd_fromnuc;
NUC_send_t NUC_send;
#endif #endif
fp32 send[4]={11.0f,45.0,1.f,4.0f}; fp32 send[4]={11.0f,45.0,1.f,4.0f};
void Task_nuc(void *argument){ void Task_nuc(void *argument){
(void)argument; /**/ (void)argument; /**/
// osDelay(TASK_INIT_DELAY_NUC); // osDelay(TASK_INIT_DELAY_NUC);
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_NUC; const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_NUC;
NUC_Init(&nuc_raw); NUC_Init(&nuc_raw);
uint32_t tick = osKernelGetTickCount(); uint32_t tick = osKernelGetTickCount();
while (1) while (1)
{ {
#ifdef DEBUG #ifdef DEBUG
task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId()); task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());
task_runtime .freq.nuc = TASK_FREQ_NUC;
task_runtime.last_up_time.nuc= tick;
#endif #endif
NUC_StartReceiving(); NUC_StartReceiving();
// NUC_StartSending(NUC_send.send); NUC_StartSending(NUC_send.send);
if (NUC_WaitDmaCplt()){ // NUC_StartSending (send);
if (NUC_WaitDmaCplt()){
NUC_RawParse(&cmd_fromnuc); NUC_RawParse(&cmd_fromnuc);
} }
else{ else{
@ -38,7 +44,8 @@ void Task_nuc(void *argument){
} }
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc); osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc);
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0); osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0);
osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0);
tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/ tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/
osDelayUntil(tick); osDelayUntil(tick);

6
User/task/user_task.h
View File

@ -58,17 +58,15 @@ typedef struct {
struct { struct {
struct { struct {
osMessageQueueId_t rc; osMessageQueueId_t rc;
osMessageQueueId_t nuc; osMessageQueueId_t nuc; //
osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/ osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
osMessageQueueId_t nuc_send; //给nuc发
}raw; }raw;
/*控制分离*/ /*控制分离*/
osMessageQueueId_t UP_cmd_ctrl_t; osMessageQueueId_t UP_cmd_ctrl_t;
osMessageQueueId_t CHASSIS_cmd_ctrl_t; osMessageQueueId_t CHASSIS_cmd_ctrl_t;
osMessageQueueId_t status; osMessageQueueId_t status;
} cmd; } cmd;