添加变形舵
This commit is contained in:
parent
c3ff5b2d2d
commit
95fc143f17
11687
MDK-ARM/JLinkLog.txt
11687
MDK-ARM/JLinkLog.txt
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
@ -103,7 +103,7 @@
|
||||
<bEvRecOn>1</bEvRecOn>
|
||||
<bSchkAxf>0</bSchkAxf>
|
||||
<bTchkAxf>0</bTchkAxf>
|
||||
<nTsel>3</nTsel>
|
||||
<nTsel>4</nTsel>
|
||||
<sDll></sDll>
|
||||
<sDllPa></sDllPa>
|
||||
<sDlgDll></sDlgDll>
|
||||
@ -114,9 +114,13 @@
|
||||
<tDlgDll></tDlgDll>
|
||||
<tDlgPa></tDlgPa>
|
||||
<tIfile></tIfile>
|
||||
<pMon>BIN\CMSIS_AGDI.dll</pMon>
|
||||
<pMon>Segger\JL2CM3.dll</pMon>
|
||||
</DebugOpt>
|
||||
<TargetDriverDllRegistry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>DLGUARM</Key>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>JL2CM3</Key>
|
||||
@ -205,6 +209,21 @@
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>gimbal</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>10</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd_for_chassis</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>11</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>chassis</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>12</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd_chassis</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
<THDelay>0</THDelay>
|
||||
@ -932,7 +951,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>component</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1284,7 +1303,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>module</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1375,18 +1394,6 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\task\ET16s.c</PathWithFileName>
|
||||
<FilenameWithoutPath>ET16s.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>88</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\task\dr16.c</PathWithFileName>
|
||||
<FilenameWithoutPath>dr16.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1394,7 +1401,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>89</FileNumber>
|
||||
<FileNumber>88</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1406,7 +1413,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>90</FileNumber>
|
||||
<FileNumber>89</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1418,7 +1425,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>91</FileNumber>
|
||||
<FileNumber>90</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1430,7 +1437,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>92</FileNumber>
|
||||
<FileNumber>91</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1440,6 +1447,18 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>92</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\task\ET16s.c</PathWithFileName>
|
||||
<FilenameWithoutPath>ET16s.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>93</FileNumber>
|
||||
@ -1459,8 +1478,8 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\task\vofa.c</PathWithFileName>
|
||||
<FilenameWithoutPath>vofa.c</FilenameWithoutPath>
|
||||
<PathWithFileName>..\User\task\init.c</PathWithFileName>
|
||||
<FilenameWithoutPath>init.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
@ -1471,8 +1490,8 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\task\init.c</PathWithFileName>
|
||||
<FilenameWithoutPath>init.c</FilenameWithoutPath>
|
||||
<PathWithFileName>..\User\task\vofa.c</PathWithFileName>
|
||||
<FilenameWithoutPath>vofa.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
@ -1492,7 +1511,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>cmd</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
|
||||
@ -1948,11 +1948,6 @@
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\cmd.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>ET16s.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\ET16s.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>dr16.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
@ -1978,21 +1973,26 @@
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\shoot_ctrl.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>ET16s.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\ET16s.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>step_motor.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\step_motor.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>vofa.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\vofa.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>init.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\init.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>vofa.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\vofa.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>user_task.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
|
||||
Binary file not shown.
@ -17,7 +17,7 @@ Library Manager: ArmAr.exe V6.16
|
||||
Hex Converter: FromElf.exe V6.16
|
||||
CPU DLL: SARMCM3.DLL V5.34.0.0
|
||||
Dialog DLL: DCM.DLL V1.17.3.0
|
||||
Target DLL: CMSIS_AGDI.dll V1.32.13.0
|
||||
Target DLL: Segger\JL2CM3.dll V2.99.38.0
|
||||
Dialog DLL: TCM.DLL V1.48.0.0
|
||||
|
||||
<h2>Project:</h2>
|
||||
@ -32,14 +32,14 @@ Note: source file '..\User\bsp\gpio.c' - object file renamed from 'Steering Whee
|
||||
Note: source file '..\User\bsp\i2c.c' - object file renamed from 'Steering Wheel_Infatry\i2c.o' to 'Steering Wheel_Infatry\i2c_1.o'.
|
||||
Note: source file '..\User\bsp\spi.c' - object file renamed from 'Steering Wheel_Infatry\spi.o' to 'Steering Wheel_Infatry\spi_1.o'.
|
||||
Note: source file '..\User\task\ai.c' - object file renamed from 'Steering Wheel_Infatry\ai.o' to 'Steering Wheel_Infatry\ai_1.o'.
|
||||
Note: source file '..\User\task\ET16s.c' - object file renamed from 'Steering Wheel_Infatry\ET16s.o' to 'Steering Wheel_Infatry\et16s_1.o'.
|
||||
Note: source file '..\User\task\dr16.c' - object file renamed from 'Steering Wheel_Infatry\dr16.o' to 'Steering Wheel_Infatry\dr16_1.o'.
|
||||
Note: source file '..\User\task\ET16s.c' - object file renamed from 'Steering Wheel_Infatry\ET16s.o' to 'Steering Wheel_Infatry\et16s_1.o'.
|
||||
Note: source file '..\User\task\step_motor.c' - object file renamed from 'Steering Wheel_Infatry\step_motor.o' to 'Steering Wheel_Infatry\step_motor_1.o'.
|
||||
Note: source file '..\User\task\vofa.c' - object file renamed from 'Steering Wheel_Infatry\vofa.o' to 'Steering Wheel_Infatry\vofa_1.o'.
|
||||
Note: source file '..\User\module\cmd\cmd.c' - object file renamed from 'Steering Wheel_Infatry\cmd.o' to 'Steering Wheel_Infatry\cmd_1.o'.
|
||||
compiling init.c...
|
||||
compiling chassis.c...
|
||||
linking...
|
||||
Program Size: Code=67008 RO-data=1720 RW-data=1224 ZI-data=119848
|
||||
Program Size: Code=67504 RO-data=1720 RW-data=1224 ZI-data=119880
|
||||
FromELF: creating hex file...
|
||||
"Steering Wheel_Infatry\Steering Wheel_Infatry.axf" - 0 Error(s), 0 Warning(s).
|
||||
|
||||
@ -65,7 +65,7 @@ Package Vendor: Keil
|
||||
|
||||
* Component: ARM::CMSIS:CORE:5.4.0
|
||||
Include file: CMSIS\Core\Include\tz_context.h
|
||||
Build Time Elapsed: 00:00:03
|
||||
Build Time Elapsed: 00:00:02
|
||||
</pre>
|
||||
</body>
|
||||
</html>
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -85,15 +85,15 @@
|
||||
"steering wheel_infatry\chassis.o"
|
||||
"steering wheel_infatry\ai_1.o"
|
||||
"steering wheel_infatry\cmd.o"
|
||||
"steering wheel_infatry\et16s_1.o"
|
||||
"steering wheel_infatry\dr16_1.o"
|
||||
"steering wheel_infatry\atti_esti.o"
|
||||
"steering wheel_infatry\gimbal_ctrl.o"
|
||||
"steering wheel_infatry\chassis_ctrl.o"
|
||||
"steering wheel_infatry\shoot_ctrl.o"
|
||||
"steering wheel_infatry\et16s_1.o"
|
||||
"steering wheel_infatry\step_motor_1.o"
|
||||
"steering wheel_infatry\vofa_1.o"
|
||||
"steering wheel_infatry\init.o"
|
||||
"steering wheel_infatry\vofa_1.o"
|
||||
"steering wheel_infatry\user_task.o"
|
||||
"steering wheel_infatry\cmd_1.o"
|
||||
"steering wheel_infatry\cmd_adapter.o"
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -65,6 +65,21 @@ fp32 vofa_send[8]; //vofa输出数据
|
||||
#define CHASSIS_ROTOR_WZ_MIN 0.6f /* 小陀螺旋转位移下界 */
|
||||
#define CHASSIS_ROTOR_WZ_MAX 0.8f /* 小陀螺旋转位移上界 */
|
||||
|
||||
float motor_add_anagle(float current_angle){
|
||||
static int cirle;
|
||||
static float prev_angle;
|
||||
float delta = current_angle - prev_angle;
|
||||
prev_angle=current_angle;
|
||||
if(delta>M_PI){
|
||||
cirle-=1;
|
||||
}else if(delta<-M_PI){
|
||||
cirle+=1;
|
||||
}
|
||||
current_angle+=cirle*M_2PI;
|
||||
|
||||
return current_angle;
|
||||
}
|
||||
|
||||
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_mode_t mode ,uint32_t now)
|
||||
{
|
||||
if (c == NULL)
|
||||
@ -112,8 +127,8 @@ int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
|
||||
c->mode = CHASSIS_MODE_RELAX; // 默认模式为停止锁死底盘
|
||||
c->mech_zero = c->param->mech_zero;/*云台6020的机械中点*/
|
||||
c->travel = c->param->travel ;/*云台6020的机械行程*/
|
||||
c->chassis2006_setangle = c->param->chassis2006_setangle; // 变形舵轮2006目标角度 大概是48圈
|
||||
c->chassis3508_setangle = c->param->chassis3508_setangle; // 变形舵轮3508目标角度 大概是60度左右
|
||||
// c->chassis2006_setangle = c->param->chassis2006_setangle; // 变形舵轮2006目标角度 大概是48圈
|
||||
// c->chassis3508_setangle = c->param->chassis3508_setangle; // 变形舵轮3508目标角度 大概是60度左右
|
||||
/*初始化can*/
|
||||
BSP_CAN_Init();
|
||||
/*注册3508电机*/
|
||||
@ -165,7 +180,7 @@ int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
|
||||
LowPassFilter2p_Init(&c->filled[7], target_freq, 20.0f); // 6020-1
|
||||
LowPassFilter2p_Init(&c->filled[8], target_freq, 20.0f); // 6020-2
|
||||
LowPassFilter2p_Init(&c->filled[9], target_freq, 20.0f); // 6020-3
|
||||
LowPassFilter2p_Init(&c->filled[10], target_freq, 20.0f); // 6020-4
|
||||
LowPassFilter2p_Init(&c->filled[10],target_freq, 20.0f); // 6020-4
|
||||
|
||||
LowPassFilter2p_Init(&c->filled[11], target_freq, 10.0f); // 2006角度
|
||||
|
||||
@ -240,6 +255,7 @@ void Chassis_speed_calculate(Chassis_t *c, Chassis_CMD_t *c_cmd)
|
||||
c->hopemotorout.rotor6020_jiesuan_1[3] = atan2((c->move_vec.Vy + c->move_vec.Vw * cos(radians)),
|
||||
(c->move_vec.Vx - c->move_vec.Vw * sin(radians))) *
|
||||
(180.0f / M_PI);
|
||||
c->chassis2006_setangle=c->param->chassis2006_setangle;
|
||||
break;
|
||||
|
||||
case CHASSIS_MODE_BREAK:
|
||||
@ -258,7 +274,9 @@ void Chassis_speed_calculate(Chassis_t *c, Chassis_CMD_t *c_cmd)
|
||||
c->hopemotorout.rotor6020_jiesuan_1[1] = 45;
|
||||
c->hopemotorout.rotor6020_jiesuan_1[2] = 315;
|
||||
c->hopemotorout.rotor6020_jiesuan_1[3] = 45;
|
||||
|
||||
|
||||
|
||||
c->chassis2006_setangle=0;
|
||||
break;
|
||||
|
||||
case HUIHANG_BIANXING_DUOLUN:
|
||||
@ -266,14 +284,12 @@ void Chassis_speed_calculate(Chassis_t *c, Chassis_CMD_t *c_cmd)
|
||||
c->hopemotorout.rotor6020_jiesuan_1[1] = 90;
|
||||
c->hopemotorout.rotor6020_jiesuan_1[2] = 270;
|
||||
c->hopemotorout.rotor6020_jiesuan_1[3] = 270;
|
||||
|
||||
c->chassis2006_setangle = c->param->chassis2006_setangle;
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
c->hopemotorout.rotor3508_jiesuan_1[i] = 0.0f;
|
||||
}
|
||||
float motor_2006omega = PID_Calc(&c->pid.chassis_2006_anglepid,c->chassis2006_setangle,c->motorfeedback.motor2006_angle,0.0f,c->dt);
|
||||
c->final_out.final_2006out=PID_Calc(&c->pid.chassis_2006_OmegaPid,motor_2006omega,c->motorfeedback.motor2006_rpm,0.0f,c->dt);
|
||||
|
||||
break;
|
||||
// c->hopemotorout.rotor3508_jiesuan_1[0] = PID_Calc(&c->pid.chassis_3508_anglepid[0],c->chassis3508_setangle,c->motorfeedback.rotor_angle3508[0],0.0f,c->dt);
|
||||
// c->hopemotorout.rotor3508_jiesuan_1[1] = PID_Calc(&c->pid.chassis_3508_anglepid[1],c->chassis3508_setangle,c->motorfeedback.rotor_angle3508[1],0.0f,c->dt);
|
||||
// c->hopemotorout.rotor3508_jiesuan_1[2] = PID_Calc(&c->pid.chassis_3508_anglepid[2],-c->chassis3508_setangle,c->motorfeedback.rotor_angle3508[2],0.0f,c->dt);
|
||||
@ -283,7 +299,11 @@ void Chassis_speed_calculate(Chassis_t *c, Chassis_CMD_t *c_cmd)
|
||||
// c->out.rotor3508_out[1]=PID_Calc(&c->pid.chassis_3508_OmegaPid[1],c->hopemotorout.rotor3508_jiesuan_1[1],c->motorfeedback.rotor_rpm3508[1],0.0f,c->dt);
|
||||
// c->out.rotor3508_out[2]=PID_Calc(&c->pid.chassis_3508_OmegaPid[2],c->hopemotorout.rotor3508_jiesuan_1[2],c->motorfeedback.rotor_rpm3508[2],0.0f,c->dt);
|
||||
// c->out.rotor3508_out[3]=PID_Calc(&c->pid.chassis_3508_OmegaPid[3],c->hopemotorout.rotor3508_jiesuan_1[3],c->motorfeedback.rotor_rpm3508[3],0.0f,c->dt);
|
||||
}
|
||||
|
||||
}
|
||||
float motor_2006omega = PID_Calc(&c->pid.chassis_2006_anglepid,c->chassis2006_setangle,c->motorfeedback.motor2006_add_angle,0.0f,c->dt);
|
||||
c->final_out.final_2006out=PID_Calc(&c->pid.chassis_2006_OmegaPid,motor_2006omega,c->motorfeedback.motor2006_omega,0.0f,c->dt);
|
||||
|
||||
}
|
||||
// 角度归化到0°——360°
|
||||
|
||||
@ -358,6 +378,8 @@ int8_t Chassis_update(Chassis_t *c)
|
||||
c->motorfeedback.motor2006_angle = MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_2006_param)->motor)) ;
|
||||
c->motorfeedback.motor2006_rpm = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_2006_param)->motor)) ;
|
||||
|
||||
c->motorfeedback.motor2006_add_angle= motor_add_anagle(c->motorfeedback.motor2006_angle);
|
||||
c->motorfeedback.motor2006_omega=c->motorfeedback.motor2006_rpm*M_2PI/60;
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
|
||||
@ -397,6 +419,7 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
|
||||
c->move_vec.Vx = 0.0f;
|
||||
c->move_vec.Vy = 0.0f;
|
||||
c->move_vec.Vw = 0.0f;
|
||||
c->final_out.final_2006out=0.0f;
|
||||
break;
|
||||
|
||||
case RC:
|
||||
@ -467,15 +490,15 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
|
||||
/*电机输出设定和发送*/
|
||||
void Chassis_Setoutput(Chassis_t *c)
|
||||
{
|
||||
// for (int i = 0; i < 4; i++)
|
||||
// {
|
||||
// MOTOR_RM_SetOutput(&(c->param->motor_3508_param[i]), c->out.rotor3508_out[i]);
|
||||
// MOTOR_RM_SetOutput(&(c->param->motor_6020_param[i]), c->out.rotor6020_out[i]);
|
||||
// }
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
MOTOR_RM_SetOutput(&(c->param->motor_3508_param[i]), c->out.rotor3508_out[i]);
|
||||
MOTOR_RM_SetOutput(&(c->param->motor_6020_param[i]), c->out.rotor6020_out[i]);
|
||||
}
|
||||
MOTOR_RM_SetOutput(&(c->param->motor_2006_param), c->out.motor2006_out);
|
||||
// MOTOR_RM_SetOutput(&(c->param->motor_3508_param[0]), 1.0f);
|
||||
// MOTOR_RM_Ctrl(&(c->param->motor_3508_param[0]));
|
||||
// MOTOR_RM_Ctrl(&(c->param->motor_6020_param[0]));
|
||||
MOTOR_RM_SetOutput(&(c->param->motor_3508_param[0]), 1.0f);
|
||||
MOTOR_RM_Ctrl(&(c->param->motor_3508_param[0]));
|
||||
MOTOR_RM_Ctrl(&(c->param->motor_6020_param[0]));
|
||||
MOTOR_RM_Ctrl(&(c->param->motor_2006_param));
|
||||
|
||||
}
|
||||
|
||||
@ -159,11 +159,12 @@ typedef struct
|
||||
fp32 rotor_current6020[4];
|
||||
fp32 rotor_temp6020[4];
|
||||
fp32 motor2006_rpm;
|
||||
fp32 motor2006_omega;
|
||||
fp32 motor2006_current;
|
||||
fp32 motor2006_temp;
|
||||
fp32 motor2006_angle;
|
||||
|
||||
float gimbal_yaw_encoder;
|
||||
fp32 motor2006_add_angle;
|
||||
fp32 gimbal_yaw_encoder;
|
||||
} motorfeedback;
|
||||
|
||||
struct
|
||||
|
||||
@ -19,16 +19,39 @@
|
||||
Config_RobotParam_t robot_config = {
|
||||
.chassis={
|
||||
|
||||
.motor_3508_param[0]={BSP_CAN_2,0x201,MOTOR_M3508,false,false},
|
||||
.motor_3508_param[0]={BSP_CAN_1,0x201,MOTOR_M3508,false,false},
|
||||
.motor_3508_param[1]={BSP_CAN_1,0x202,MOTOR_M3508,false,false},
|
||||
.motor_3508_param[2]={BSP_CAN_1,0x203,MOTOR_M3508,false,false},
|
||||
.motor_3508_param[3]={BSP_CAN_1,0x204,MOTOR_M3508,false,false},
|
||||
|
||||
.motor_6020_param[0]={BSP_CAN_1,0x205,MOTOR_GM6020,false,false},
|
||||
.motor_6020_param[1]={BSP_CAN_1,0x206,MOTOR_GM6020,false,false},
|
||||
.motor_6020_param[2]={BSP_CAN_1,0x207,MOTOR_GM6020,false,false},
|
||||
.motor_6020_param[3]={BSP_CAN_1,0x208,MOTOR_GM6020,false,false},
|
||||
|
||||
.motor_2006_param ={BSP_CAN_1,0x205,MOTOR_M2006,false,true},
|
||||
.motor_6020_param[0]={BSP_CAN_1,0x206,MOTOR_GM6020,false,false},
|
||||
.motor_6020_param[1]={BSP_CAN_1,0x207,MOTOR_GM6020,false,false},
|
||||
.motor_6020_param[2]={BSP_CAN_1,0x208,MOTOR_GM6020,false,false},
|
||||
.motor_6020_param[3]={BSP_CAN_1,0x209,MOTOR_GM6020,false,false},
|
||||
.chassis2006_setangle=45,
|
||||
.chassis_2006_angle_param={
|
||||
.k=1.0f,
|
||||
.p=1.0f,
|
||||
.i=0.2f,
|
||||
.d=0.3f,
|
||||
.i_limit=3.0f,
|
||||
.out_limit=35.0f,
|
||||
.d_cutoff_freq= -1.0f,
|
||||
.range=-M_2PI,
|
||||
},
|
||||
.chassis_2006_Omega_param={
|
||||
.k=1.0f,
|
||||
.p=0.3f,
|
||||
.i=0.0f,
|
||||
.d=0.001f,
|
||||
.i_limit=1.0f,
|
||||
.out_limit=1.0f,
|
||||
.d_cutoff_freq= -1.0f,
|
||||
.range=-1.0f
|
||||
|
||||
},
|
||||
|
||||
|
||||
.C6020Omega_param={
|
||||
.k=1.0f,
|
||||
.p=0.5f,
|
||||
@ -153,31 +176,9 @@ Config_RobotParam_t robot_config = {
|
||||
.gyro = 1000.0f,
|
||||
},
|
||||
|
||||
.pid = {
|
||||
/* 大yaw参数 */
|
||||
.major_yaw_omega={
|
||||
.k = 0.0f,
|
||||
.p = 0.3f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f
|
||||
},
|
||||
.major_yaw_angle={
|
||||
.k = 2.0f,
|
||||
.p = 40.5f,//36.5
|
||||
.i = 0.0f,
|
||||
.d = 0.12f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 7.0f,
|
||||
.d_cutoff_freq = 20.0f,
|
||||
.range = M_2PI
|
||||
},
|
||||
|
||||
/*欧拉角控制参数*/
|
||||
.pid = {
|
||||
|
||||
/*欧拉角控制参数*/
|
||||
.yaw_omega = {
|
||||
.k = 0.30f,
|
||||
.p = 0.4f,
|
||||
@ -242,7 +243,7 @@ Config_RobotParam_t robot_config = {
|
||||
.fric = {
|
||||
{
|
||||
.param = {
|
||||
.can = BSP_CAN_1,
|
||||
.can = BSP_CAN_2,
|
||||
.id = 0x201,
|
||||
.module = MOTOR_M3508,
|
||||
.reverse = false,
|
||||
@ -252,7 +253,7 @@ Config_RobotParam_t robot_config = {
|
||||
},
|
||||
{
|
||||
.param = {
|
||||
.can = BSP_CAN_1,
|
||||
.can = BSP_CAN_2,
|
||||
.id = 0x202,
|
||||
.module = MOTOR_M3508,
|
||||
.reverse = true,
|
||||
@ -263,7 +264,7 @@ Config_RobotParam_t robot_config = {
|
||||
|
||||
},
|
||||
.trig = {
|
||||
.can = BSP_CAN_1,
|
||||
.can = BSP_CAN_2,
|
||||
.id = 0x205,
|
||||
.module = MOTOR_M2006,
|
||||
.reverse = false,
|
||||
@ -339,8 +340,8 @@ Config_RobotParam_t robot_config = {
|
||||
},
|
||||
.rc_mode_map = {
|
||||
.sw_left_up = CHASSIS_MODE_RELAX,
|
||||
.sw_left_mid = CHASSIS_MODE_FOLLOW_GIMBAL,
|
||||
.sw_left_down = CHASSIS_MODE_ROTOR,
|
||||
.sw_left_mid = HUIHANG_BIANXING_DUOLUN,
|
||||
.sw_left_down = CHASSIS_MODE_FOLLOW_GIMBAL,
|
||||
|
||||
.gimbal_sw_up = GIMBAL_MODE_RELAX,
|
||||
.gimbal_sw_mid = GIMBAL_MODE_ABSOLUTE,
|
||||
|
||||
@ -39,19 +39,19 @@ void Task_chassis_ctrl(void *argument) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
/*接受cmd任务数据*/
|
||||
// if(osMessageQueueGet(task_runtime.msgq.cmd.chassis, &cmd_chassis, NULL, 0)==osOK)
|
||||
// {
|
||||
// Chassis_update(&chassis);
|
||||
// Chassis_Control(&chassis, &cmd_chassis);
|
||||
// }else
|
||||
//{
|
||||
// 如果没有收到命令，可以执行一个安全停止的逻辑
|
||||
// 或者什么都不做，让底盘保持上一帧的状态（取决于你的设计）
|
||||
// 一个安全的选择是让底盘停止
|
||||
Chassis_CMD_t safe_cmd = {.mode = STOP, .Vx = 0, .Vy = 0, .Vw = 0};
|
||||
Chassis_Control(&chassis, &safe_cmd,tick);
|
||||
//}
|
||||
Chassis_Setoutput(&chassis);
|
||||
if(osMessageQueueGet(task_runtime.msgq.chassis.cmd, &cmd_chassis, NULL, 0)==osOK);
|
||||
|
||||
Chassis_update(&chassis);
|
||||
Chassis_Control(&chassis, &cmd_chassis,tick);
|
||||
Chassis_Setoutput(&chassis);
|
||||
////{
|
||||
// // 如果没有收到命令，可以执行一个安全停止的逻辑
|
||||
// // 或者什么都不做，让底盘保持上一帧的状态（取决于你的设计）
|
||||
// // 一个安全的选择是让底盘停止
|
||||
// Chassis_CMD_t safe_cmd = {.mode = STOP, .Vx = 0, .Vy = 0, .Vw = 0};
|
||||
// Chassis_Control(&chassis, &safe_cmd,tick);
|
||||
////}
|
||||
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user