只提交测试后的，遥控器投篮完成
This commit is contained in:
parent
9cd4a4b161
commit
5de502c448
@ -32,7 +32,6 @@
|
||||
/* USER CODE BEGIN Includes */
|
||||
#include "bsp_delay.h"
|
||||
#include "bsp_can.h"
|
||||
#include "Action.h"
|
||||
//#include "bsp_dwt.h"
|
||||
/* USER CODE END Includes */
|
||||
|
||||
|
||||
2
MDK-ARM/.vscode/keil-assistant.log
vendored
2
MDK-ARM/.vscode/keil-assistant.log
vendored
@ -475,3 +475,5 @@ Log at : 2025/2/23|14:39:36|GMT+0800
|
||||
|
||||
[info] Log at : 2025/3/11|21:09:29|GMT+0800
|
||||
|
||||
[info] Log at : 2025/3/28|21:47:12|GMT+0800
|
||||
|
||||
|
||||
@ -145,7 +145,7 @@
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>DLGUARM</Key>
|
||||
<Name></Name>
|
||||
<Name>(105=-1,-1,-1,-1,0)</Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
@ -195,6 +195,46 @@
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>GO_motor_info</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>8</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cbuf[36]</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>9</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>dr16,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>10</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>11</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rc_ctrl,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>12</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>up_cmd,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>13</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>angle</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>14</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>can_ZZ</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>15</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>flaggg,0x0A</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<WatchWindow2>
|
||||
<Ww>
|
||||
@ -1116,18 +1156,6 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\task\nuc_task.c</PathWithFileName>
|
||||
<FilenameWithoutPath>nuc_task.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>68</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\task\error_detect_task.c</PathWithFileName>
|
||||
<FilenameWithoutPath>error_detect_task.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1135,7 +1163,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>69</FileNumber>
|
||||
<FileNumber>68</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1147,7 +1175,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>70</FileNumber>
|
||||
<FileNumber>69</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1157,6 +1185,30 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>70</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\task\cmd_task.c</PathWithFileName>
|
||||
<FilenameWithoutPath>cmd_task.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>71</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\task\nuc_task.c</PathWithFileName>
|
||||
<FilenameWithoutPath>nuc_task.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
</Group>
|
||||
|
||||
<Group>
|
||||
@ -1167,7 +1219,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>71</FileNumber>
|
||||
<FileNumber>72</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1177,18 +1229,6 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>72</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\task\cmd_task.c</PathWithFileName>
|
||||
<FilenameWithoutPath>cmd_task.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>73</FileNumber>
|
||||
@ -1249,18 +1289,6 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>78</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\Algorithm\navi.c</PathWithFileName>
|
||||
<FilenameWithoutPath>navi.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
</Group>
|
||||
|
||||
<Group>
|
||||
@ -1271,7 +1299,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>79</FileNumber>
|
||||
<FileNumber>78</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1283,7 +1311,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>80</FileNumber>
|
||||
<FileNumber>79</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1295,7 +1323,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>81</FileNumber>
|
||||
<FileNumber>80</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1307,7 +1335,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>82</FileNumber>
|
||||
<FileNumber>81</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1319,7 +1347,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>83</FileNumber>
|
||||
<FileNumber>82</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1331,7 +1359,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>84</FileNumber>
|
||||
<FileNumber>83</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1343,7 +1371,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>85</FileNumber>
|
||||
<FileNumber>84</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1363,7 +1391,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>11</GroupNumber>
|
||||
<FileNumber>86</FileNumber>
|
||||
<FileNumber>85</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1375,7 +1403,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>11</GroupNumber>
|
||||
<FileNumber>87</FileNumber>
|
||||
<FileNumber>86</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1387,7 +1415,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>11</GroupNumber>
|
||||
<FileNumber>88</FileNumber>
|
||||
<FileNumber>87</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1407,7 +1435,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>12</GroupNumber>
|
||||
<FileNumber>89</FileNumber>
|
||||
<FileNumber>88</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1427,7 +1455,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>90</FileNumber>
|
||||
<FileNumber>89</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1439,7 +1467,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>91</FileNumber>
|
||||
<FileNumber>90</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1451,7 +1479,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>92</FileNumber>
|
||||
<FileNumber>91</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1463,7 +1491,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>93</FileNumber>
|
||||
<FileNumber>92</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
|
||||
@ -1208,11 +1208,6 @@
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\can_task.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>nuc_task.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\nuc_task.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>error_detect_task.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
@ -1228,6 +1223,67 @@
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\up_task.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>cmd_task.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\cmd_task.c</FilePath>
|
||||
<FileOption>
|
||||
<CommonProperty>
|
||||
<UseCPPCompiler>2</UseCPPCompiler>
|
||||
<RVCTCodeConst>0</RVCTCodeConst>
|
||||
<RVCTZI>0</RVCTZI>
|
||||
<RVCTOtherData>0</RVCTOtherData>
|
||||
<ModuleSelection>0</ModuleSelection>
|
||||
<IncludeInBuild>1</IncludeInBuild>
|
||||
<AlwaysBuild>2</AlwaysBuild>
|
||||
<GenerateAssemblyFile>2</GenerateAssemblyFile>
|
||||
<AssembleAssemblyFile>2</AssembleAssemblyFile>
|
||||
<PublicsOnly>2</PublicsOnly>
|
||||
<StopOnExitCode>11</StopOnExitCode>
|
||||
<CustomArgument></CustomArgument>
|
||||
<IncludeLibraryModules></IncludeLibraryModules>
|
||||
<ComprImg>1</ComprImg>
|
||||
</CommonProperty>
|
||||
<FileArmAds>
|
||||
<Cads>
|
||||
<interw>2</interw>
|
||||
<Optim>0</Optim>
|
||||
<oTime>2</oTime>
|
||||
<SplitLS>2</SplitLS>
|
||||
<OneElfS>2</OneElfS>
|
||||
<Strict>2</Strict>
|
||||
<EnumInt>2</EnumInt>
|
||||
<PlainCh>2</PlainCh>
|
||||
<Ropi>2</Ropi>
|
||||
<Rwpi>2</Rwpi>
|
||||
<wLevel>0</wLevel>
|
||||
<uThumb>2</uThumb>
|
||||
<uSurpInc>2</uSurpInc>
|
||||
<uC99>2</uC99>
|
||||
<uGnu>2</uGnu>
|
||||
<useXO>2</useXO>
|
||||
<v6Lang>0</v6Lang>
|
||||
<v6LangP>0</v6LangP>
|
||||
<vShortEn>2</vShortEn>
|
||||
<vShortWch>2</vShortWch>
|
||||
<v6Lto>2</v6Lto>
|
||||
<v6WtE>2</v6WtE>
|
||||
<v6Rtti>2</v6Rtti>
|
||||
<VariousControls>
|
||||
<MiscControls></MiscControls>
|
||||
<Define></Define>
|
||||
<Undefine></Undefine>
|
||||
<IncludePath></IncludePath>
|
||||
</VariousControls>
|
||||
</Cads>
|
||||
</FileArmAds>
|
||||
</FileOption>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>nuc_task.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\nuc_task.c</FilePath>
|
||||
</File>
|
||||
</Files>
|
||||
</Group>
|
||||
<Group>
|
||||
@ -1238,11 +1294,6 @@
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\Algorithm\filter.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>cmd_task.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\cmd_task.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>ahrs.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
@ -1268,11 +1319,6 @@
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\Algorithm\pid.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>navi.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\Algorithm\navi.c</FilePath>
|
||||
</File>
|
||||
</Files>
|
||||
</Group>
|
||||
<Group>
|
||||
|
||||
Binary file not shown.
@ -1,231 +0,0 @@
|
||||
#include "Chassis.h"
|
||||
#include "gpio.h"
|
||||
#include "Action.h"
|
||||
#include "user_math.h"
|
||||
#include "bsp_buzzer.h"
|
||||
#include "bsp_delay.h"
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/*机器人坐标系，向前x，右y，上yaw
|
||||
不同于nuc (前x，左y，上yaw) */
|
||||
/*
|
||||
x
|
||||
|
|
||||
--y
|
||||
|
||||
|
||||
*/
|
||||
|
||||
static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){
|
||||
if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */
|
||||
if (ctrl->C_cmd.type== c->ctrl && ctrl->C_cmd.mode== c->mode) return CHASSIS_OK; /*模式未改变直接返回*/
|
||||
//此处源代码处做了pid的reset 待添加
|
||||
c->ctrl =ctrl->C_cmd.type;
|
||||
c->mode =ctrl->C_cmd.mode;
|
||||
|
||||
return CHASSIS_OK;
|
||||
} //设置控制模式
|
||||
|
||||
|
||||
/*该函数用来更新can任务获得的电机反馈值*/
|
||||
|
||||
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
|
||||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||||
if (can == NULL) return CHASSIS_ERR_NULL;
|
||||
for (uint8_t i = 0; i < 4; i++)
|
||||
{
|
||||
c->motorfeedback.rotor_rpm3508[i] = can->motor.motor3508.as_array[i].rotor_speed;
|
||||
c->motorfeedback.rotor_current3508[i] = can->motor.motor3508.as_array[i].torque_current;
|
||||
}
|
||||
|
||||
|
||||
c->motorfeedback.rotor_pit6020angle = can->motor.pit6020.as_array[0].rotor_ecd;
|
||||
c->motorfeedback.rotor_pit6020rpm = can->motor.pit6020.as_array[0].rotor_speed;
|
||||
|
||||
c->motorfeedback.rotor_gimbal_yawangle = can->motor.chassis6020.as_array[0].rotor_ecd;
|
||||
c->motorfeedback.rotor_gimbal_yawrpm = can->motor.chassis6020.as_array[0].rotor_speed;
|
||||
|
||||
c->motorfeedback.rotor_gimbal_pitchangle = can->motor.chassis6020.as_array[1].rotor_ecd;
|
||||
c->motorfeedback.rotor_gimbal_pitchrpm = can->motor.chassis6020.as_array[1].rotor_speed;
|
||||
|
||||
c->sick_dis[0] = can->sickfed.raw_dis[0];
|
||||
c->sick_dis[1] = can->sickfed.raw_dis[1];
|
||||
c->sick_dis[2] = can->sickfed.raw_dis[2];
|
||||
c->sick_dis[3] = can->sickfed.raw_dis[3];
|
||||
|
||||
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_pitAngle6020),PID_POSITION,&(c->param->C6020pitAngle_param));//尝试位置控制角度
|
||||
|
||||
PID_init((&c->pid.chassis_pitOmega6020),PID_POSITION,&(c->param->C6020pitOmega_param));
|
||||
|
||||
PID_init((&c->pid.chassis_gimbal_yawAnglePID),PID_POSITION,&(c->param->Gimbal_yawAngle_param));//尝试位置控制角度
|
||||
|
||||
PID_init((&c->pid.chassis_gimbal_yawOmegaPID),PID_POSITION,&(c->param->Gimbal_yawOmega_param));
|
||||
|
||||
PID_init((&c->pid.chassis_gimbal_pitchAnglePID),PID_POSITION,&(c->param->Gimbal_pitchAngle_param));//尝试位置控制角度
|
||||
|
||||
PID_init((&c->pid.chassis_gimbal_pitchOmegaPID),PID_POSITION,&(c->param->Gimbal_pitchOmega_param));
|
||||
|
||||
PID_init(&(c->pid.chassis_NaviVxPID),PID_POSITION,&(c->param->NaviVx_param));
|
||||
|
||||
PID_init(&(c->pid.chassis_NaviVyPID),PID_POSITION,&(c->param->NaviVy_param));
|
||||
|
||||
PID_init(&(c->pid.chassis_NaviWzPID),PID_POSITION,&(c->param->NaviVw_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;
|
||||
}
|
||||
|
||||
|
||||
void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算
|
||||
{
|
||||
c->hopemotorout.OmniSpeedOut[3] = -Vx+Vy+Vw;//右前
|
||||
c->hopemotorout.OmniSpeedOut[0] = -Vx-Vy+Vw;//右后
|
||||
c->hopemotorout.OmniSpeedOut[1] = Vx-Vy+Vw;//左后
|
||||
c->hopemotorout.OmniSpeedOut[2] = Vx+Vy+Vw;//左前
|
||||
}
|
||||
|
||||
|
||||
//bool isArrive(fp32 real_pos,fp32 target_pos,fp32 mistake)
|
||||
//{
|
||||
// uint16_t xArrive = 0, yArrive = 0;
|
||||
// xArrive = abs_float_double(real_pos,target_pos) < mistake ? 1:0;
|
||||
// yArrive = abs_float_double(real_pos,target_pos) < mistake ? 1:0;
|
||||
// if(xArrive && yArrive) return true;
|
||||
// else return false;
|
||||
//}
|
||||
|
||||
|
||||
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->ctrl)
|
||||
{
|
||||
case RC:
|
||||
|
||||
/*
|
||||
在cmd里对数据进行处理 包括方向和油门
|
||||
6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同*/
|
||||
c->move_vec.Vw = ctrl->Vw*6000;
|
||||
c->move_vec.Vx = -ctrl->Vy*6000;
|
||||
c->move_vec.Vy = ctrl->Vx*6000;
|
||||
if(c->mode == RC_MODE1 ){
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
|
||||
case MID_NAVI:
|
||||
// //这套是全向轮的方向,一定要注意这里的xy方向
|
||||
c->move_vec.Vw =ctrl->C_navi.wz ;
|
||||
c->move_vec.Vx =ctrl->C_navi.vy ;
|
||||
c->move_vec.Vy =ctrl->C_navi.vx ;
|
||||
|
||||
c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw);
|
||||
c->move_vec.Vy =LowPassFilter2p_Apply(&(c->filled[2]),c->move_vec.Vy);
|
||||
c->move_vec.Vx =LowPassFilter2p_Apply(&(c->filled[3]),c->move_vec.Vx);
|
||||
|
||||
c->move_vec.Vw =PID_calc(&(c->pid.chassis_NaviWzPID),c->pos088.bmi088.gyro.z,c->move_vec.Vw);
|
||||
c->move_vec.Vx =PID_calc(&(c->pid.chassis_NaviVxPID),c->pos088.bmi088.accl.y,c->move_vec.Vx);
|
||||
c->move_vec.Vy =PID_calc(&(c->pid.chassis_NaviVyPID),c->pos088.bmi088.accl.x,c->move_vec.Vy);
|
||||
|
||||
if(ctrl->status[5] ==1)
|
||||
{
|
||||
c->move_vec.Vw = c->move_vec.Vw * 0.8f;
|
||||
c->move_vec.Vx = c->move_vec.Vx * 0.5f;
|
||||
c->move_vec.Vy = c->move_vec.Vy * 0.5f;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
}
|
||||
/*怎么用*/
|
||||
switch (c->mode)
|
||||
{
|
||||
case RELAXED:
|
||||
c->move_vec.Vx =0;
|
||||
c->move_vec.Vy =0;
|
||||
c->move_vec.Vw =0;
|
||||
break;
|
||||
case NORMAL:
|
||||
break;
|
||||
|
||||
case GYRO_STAY://陀螺仪yaw修正
|
||||
c->move_vec.Vw = c->move_vec.Vw +c->pos088.bmi088.gyro.z *2000;
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
//电机速度限幅
|
||||
|
||||
// abs_limit_fp(&c->move_vec.Vx,2000.0f);
|
||||
|
||||
// abs_limit_fp(&c->move_vec.Vy,2000.0f);
|
||||
|
||||
// abs_limit_fp(&c->move_vec.Vw,2000.0f);
|
||||
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->motor3508.as_array[i] = c->final_out.final_3508out[i];
|
||||
}
|
||||
|
||||
|
||||
// c->vofa_send[0]=c->pos088.bmi088.gyro.x;
|
||||
// c->vofa_send[1]=c->pos088.bmi088.gyro.y;
|
||||
// c->vofa_send[2]=c->pos088.bmi088.gyro.z;
|
||||
// c->vofa_send[3]=c->pos088.bmi088.accl.x;
|
||||
// c->vofa_send[4]=c->pos088.bmi088.accl.y;
|
||||
// c->vofa_send[5]=c->pos088.bmi088.accl.z;
|
||||
|
||||
return CHASSIS_OK;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@ -1,232 +0,0 @@
|
||||
#ifndef CHASSIS_H
|
||||
|
||||
/**
|
||||
* @brief 底盘控制类头文件
|
||||
*
|
||||
* 本头文件定义了底盘控制类的接口和数据结构，用于控制车辆的底盘。
|
||||
*
|
||||
* @param ChassisImu_t 底盘的 IMU 数据结构体
|
||||
* @param ops_t 操作数据结构体
|
||||
* @param Chassis_t 底盘数据结构体
|
||||
*
|
||||
* @brief 底盘控制类的功能包括：
|
||||
* 速度控制
|
||||
* 方向控制
|
||||
* PID 控制
|
||||
* 速度和方向的综合控制
|
||||
*
|
||||
* @attention PID 控制器的配置需要在 config 文件中设置
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#define CHASSIS_H
|
||||
|
||||
#include "struct_typedef.h"
|
||||
#include "pid.h"
|
||||
#include "bmi088.h"
|
||||
#include "map.h"
|
||||
#include "user_math.h"
|
||||
#include "ahrs.h"
|
||||
#include "can_use.h"
|
||||
#include "cmd.h"
|
||||
#include "filter.h"
|
||||
#include "Action.h"
|
||||
|
||||
#define CHASSIS_OK (0)
|
||||
#define CHASSIS_ERR (-1)
|
||||
#define CHASSIS_ERR_NULL (-2)
|
||||
#define CHASSIS_ERR_MODE (-3) /*CMD_ChassisMode_t */
|
||||
#define CHASSIS_ERR_TYPE (-4) /*Chassis_Type_t */
|
||||
|
||||
//m3508的电机转速转换为底盘的实际速度
|
||||
#define M3508_MOTOR_RPM_TO_VECTOR 0.0008809748903494517209f
|
||||
|
||||
|
||||
#define M6020_MOTOR_RPM_TO_VECTOR 0.003664f
|
||||
#define PI 3.1415926535f
|
||||
|
||||
typedef struct {
|
||||
|
||||
BMI088_t bmi088;
|
||||
|
||||
/*可通过该枚举类型来决定Imu的数据量纲*/
|
||||
enum {
|
||||
IMU_DEGREE,//角度值（0-360）
|
||||
IMU_RADIAN//弧度制（0-2pi)
|
||||
}angle_mode;
|
||||
|
||||
AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
|
||||
}ChassisImu_t;
|
||||
|
||||
/*底盘的类型*/
|
||||
typedef enum {
|
||||
CHASSIS_TYPE_MECANUM, /* 麦轮 */
|
||||
CHASSIS_TYPE_OMNI_CROSS, /* 全向轮*/
|
||||
CHASSIS_TYPE_AGV, /* AGV舵轮 */
|
||||
} Chassis_Type_e;
|
||||
|
||||
/*底盘的电机轮组*/
|
||||
typedef enum {
|
||||
DJI_M3508,
|
||||
DJI_G6020,
|
||||
AGV_Group,
|
||||
}Chassis_Motortype_e;
|
||||
|
||||
|
||||
/* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */
|
||||
typedef struct
|
||||
{
|
||||
Chassis_Type_e chassis_type; /* */
|
||||
Chassis_Motortype_e motor_type; /**/
|
||||
|
||||
|
||||
/*该部分决定PID的参数整定在config中修改*/
|
||||
pid_param_t M3508_param;
|
||||
pid_param_t AngleCor_param;
|
||||
pid_param_t OmegaCor_param;
|
||||
pid_param_t DisCamera_param;
|
||||
pid_param_t ImuCor_param;
|
||||
pid_param_t C6020pitAngle_param;
|
||||
pid_param_t C6020pitOmega_param;
|
||||
pid_param_t Gimbal_yawAngle_param;
|
||||
pid_param_t Gimbal_yawOmega_param;
|
||||
pid_param_t Gimbal_pitchAngle_param;
|
||||
pid_param_t Gimbal_pitchOmega_param;
|
||||
pid_param_t NaviVx_param;
|
||||
pid_param_t NaviVy_param;
|
||||
pid_param_t NaviVw_param;
|
||||
pid_param_t Sick_CaliYparam;
|
||||
pid_param_t Sick_CaliXparam;
|
||||
|
||||
|
||||
|
||||
}Chassis_Param_t;
|
||||
|
||||
|
||||
/*该结构体用于底盘的期望运动向量*/
|
||||
typedef struct
|
||||
{
|
||||
fp32 Vx;
|
||||
fp32 Vy;
|
||||
fp32 Vw;
|
||||
fp32 mul;//油门倍率
|
||||
}ChassisMove_Vec;
|
||||
|
||||
|
||||
/**
|
||||
* @brief
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
uint8_t chassis_task_run; //线程的运行
|
||||
|
||||
const Chassis_Param_t *param; //一些固定的参数
|
||||
|
||||
ChassisImu_t pos088; //088的实时姿态
|
||||
Action_POS_t Action_pos;
|
||||
CMD_Chassis_CtrlType_e ctrl;
|
||||
CMD_Chassis_mode_e mode;
|
||||
|
||||
ChassisMove_Vec move_vec; //由控制任务决定
|
||||
|
||||
struct{
|
||||
|
||||
fp32 rotor_rpm3508[4];
|
||||
fp32 rotor_current3508[4];
|
||||
|
||||
fp32 rotor_pit6020angle;
|
||||
fp32 rotor_pit6020rpm;
|
||||
|
||||
fp32 rotor_gimbal_yawangle;
|
||||
fp32 rotor_gimbal_yawrpm;
|
||||
|
||||
fp32 rotor_gimbal_pitchangle;
|
||||
fp32 rotor_gimbal_pitchrpm;
|
||||
|
||||
}motorfeedback;
|
||||
|
||||
|
||||
/*期望的底盘输出值*/
|
||||
struct{
|
||||
fp32 OmniSpeedOut[4];
|
||||
|
||||
}hopemotorout;
|
||||
|
||||
|
||||
/*经PID计算后的实际发送给电机的实时输出值*/
|
||||
struct
|
||||
{
|
||||
fp32 final_3508out[4];
|
||||
fp32 final_pitchout;
|
||||
fp32 final_gimbal_yawout;
|
||||
fp32 final_gimbal_pitchout;
|
||||
|
||||
}final_out;
|
||||
|
||||
struct{
|
||||
|
||||
pid_type_def chassis_3508VecPID[4];
|
||||
pid_type_def chassis_pitAngle6020;
|
||||
pid_type_def chassis_pitOmega6020;
|
||||
pid_type_def chassis_gimbal_yawAnglePID;
|
||||
pid_type_def chassis_gimbal_yawOmegaPID;
|
||||
pid_type_def chassis_gimbal_pitchAnglePID;
|
||||
pid_type_def chassis_gimbal_pitchOmegaPID;
|
||||
pid_type_def chassis_NaviVxPID;
|
||||
pid_type_def chassis_NaviVyPID;
|
||||
pid_type_def chassis_NaviWzPID;
|
||||
pid_type_def sick_CaliforYPID;
|
||||
pid_type_def sick_CaliforXPID;
|
||||
|
||||
pid_type_def Action_VxPID;
|
||||
pid_type_def Action_VyPID;
|
||||
pid_type_def Action_WzPID;
|
||||
}pid;
|
||||
|
||||
fp32 vofa_send[8];
|
||||
|
||||
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
|
||||
|
||||
|
||||
int32_t sick_dis[4]; //获取到的sick激光值
|
||||
|
||||
|
||||
|
||||
}Chassis_t;
|
||||
|
||||
/**
|
||||
* @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);
|
||||
|
||||
|
||||
/**
|
||||
* \brief
|
||||
|
||||
*/
|
||||
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/// @brief
|
||||
/// @param c
|
||||
void vesc_current_detection(Chassis_t *c);
|
||||
#endif
|
||||
@ -1,14 +1,12 @@
|
||||
#ifndef _CONFIG_H
|
||||
#define _CONFIG_H
|
||||
|
||||
#include "Chassis.h"
|
||||
#include "can_use.h"
|
||||
#include "ahrs.h"
|
||||
#include "map.h"
|
||||
#include "up.h"
|
||||
|
||||
typedef struct{
|
||||
Chassis_Param_t chassis; /**/
|
||||
UP_Param_t up;
|
||||
CAN_Params_t can;
|
||||
AHRS_Eulr_t mech_zero[4];
|
||||
|
||||
301
User/Module/up.c
301
User/Module/up.c
@ -4,102 +4,158 @@
|
||||
#include "bsp_buzzer.h"
|
||||
#include "bsp_delay.h"
|
||||
|
||||
#define GEAR_RATIO (36) // 2006减速比
|
||||
#define GEAR_RATIO_2006 (36) // 2006减速比
|
||||
#define GEAR_RATIO_3508 (19)
|
||||
|
||||
#define CAN_MOTOR_ENC_RES 8191 // 编码器分辨率
|
||||
#define MOTOR2006_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO)) //2006编码值转轴角度
|
||||
#define MOTOR2006_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_2006)) //2006编码值转轴角度
|
||||
#define MOTOR3508_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_3508)) //3508编码值转轴角度
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
|
||||
{
|
||||
u->param = param; /*初始化参数 */
|
||||
|
||||
/*go电机初始化*/
|
||||
GO_M8010_init();
|
||||
/*pid初始化*/
|
||||
PID_init (&u->pid.VESC_5065_M1 ,PID_POSITION ,&(u->param ->VESC_5065_M1_param ));
|
||||
PID_init (&u->pid.VESC_5065_M2 ,PID_POSITION ,&(u->param ->VESC_5065_M2_param ));
|
||||
|
||||
PID_init (&u->pid .M2006_angle ,PID_POSITION ,&(u->param->M2006_angle_param ));
|
||||
PID_init (&u->pid .M2006_speed ,PID_POSITION ,&(u->param->M2006_speed_param ));
|
||||
|
||||
PID_init (&u->pid .M3508_angle ,PID_POSITION ,&(u->param->M3508_angle_param ));
|
||||
PID_init (&u->pid .M3508_speed ,PID_POSITION ,&(u->param->M3508_speed_param ));
|
||||
|
||||
PID_init (&u->pid .GM6020_speed,PID_POSITION ,&(u->param ->UP_GM6020_speed_param ));
|
||||
PID_init (&u->pid .GM6020_angle,PID_POSITION ,&(u->param ->UP_GM6020_angle_param ));
|
||||
|
||||
u->M2006 .motor =M2006 ;
|
||||
u->M3508 .motor =M3508 ;
|
||||
|
||||
for(int i=0;i<3;i++){
|
||||
PID_init (&u->pid .M3508_speed[i] ,PID_POSITION ,&(u->param ->M3508_speed_param ));
|
||||
}
|
||||
|
||||
for(int i=0;i<2;i++){ //go初始位置设置为0
|
||||
u->GO_motor_info[i] = getGoPoint(i);
|
||||
GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), i,u->param->go_param .rev ,u->param->go_param .T ,u->param->go_param .W ,0,u->param->go_param .K_P ,u->param->go_param .K_W );
|
||||
}
|
||||
|
||||
/**/
|
||||
|
||||
u->state .Dribble_flag =Not_started_dri;
|
||||
u->state. Pitch_flag=Not_started_Pit;
|
||||
u->state .last_state = Not_started_dri;
|
||||
|
||||
}
|
||||
|
||||
/*can，上层状态更新*/
|
||||
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
|
||||
for(int i=0;i<2;i++){ //go初始位置设置为0
|
||||
u->motorfeedback .GO_motor_info [i] = getGoPoint(i);
|
||||
}
|
||||
|
||||
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can) {
|
||||
|
||||
u->motorfeedback .M2006_angle=can ->motor .motor3508 .as_array [0].rotor_ecd ;
|
||||
u->motorfeedback .M2006_rpm =can ->motor .motor3508 . as_array [0].rotor_speed ;
|
||||
u->motorfeedback .M2006 .motor =M2006;
|
||||
u->motorfeedback .M3508 .motor =M3508;
|
||||
|
||||
u->motorfeedback .VESC_5065_M1_rpm =can ->motor .chassis5065 .as_array [0].rotor_speed ;
|
||||
u->motorfeedback .VESC_5065_M2_rpm =can ->motor .chassis5065 .as_array [1].rotor_speed ;
|
||||
|
||||
u->motorfeedback .rotor_pit6020ecd =can ->motor .chassis6020.as_array [2].rotor_ecd ;
|
||||
u->motorfeedback .rotor_pit6020rpm =can ->motor .chassis6020.as_array [2].rotor_speed ;
|
||||
for(int i=0;i<3;i++){
|
||||
u->motorfeedback .M3508_speed[i] =can ->motor .motor3508 .as_array [i+1].rotor_speed ;
|
||||
}
|
||||
|
||||
for(int i=0;i<4;i++){
|
||||
u->motorfeedback .M3508_rpm[i] =can ->motor .motor3508 .as_array [i].rotor_speed ;
|
||||
u->motorfeedback .M3508_angle [i]=can ->motor .motor3508 .as_array [i].rotor_ecd ;
|
||||
}
|
||||
|
||||
int8_t cnt=0;
|
||||
u->cmd =c;
|
||||
|
||||
/*上层电机控制*/
|
||||
int8_t UP_angle(UP_t *u, fp32 target_angle) {
|
||||
// 获取当前编码器角度
|
||||
|
||||
// switch (u->)
|
||||
float angle = u->motorfeedback.M2006_angle;
|
||||
// 初始化阶段：前50次循环记录初始值
|
||||
if (u->M2006.init_cnt < 50) {
|
||||
u->M2006.orig_angle = angle; // 记录初始编码器值
|
||||
u->M2006.last_angle = angle;
|
||||
|
||||
u->M2006.init_cnt++; // 初始化计数器递增
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
float delta = angle - u->M2006.last_angle;
|
||||
if (delta > 4096) {
|
||||
u->M2006.round_cnt--; // 逆时针跨圈
|
||||
} else if (delta < -4096) {
|
||||
u->M2006.round_cnt++; // 顺时针跨圈
|
||||
|
||||
/*上层电机控制,使用can1的id1和2*/
|
||||
int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor) {
|
||||
// 获取当前编码器角度
|
||||
int8_t cnt=0;
|
||||
fp32 angle ,delta;
|
||||
|
||||
switch(motor)
|
||||
{
|
||||
case M2006 :
|
||||
angle = u->motorfeedback.M3508_angle[0];
|
||||
if (u->motorfeedback .M2006 .init_cnt < 50) {
|
||||
u->motorfeedback .M2006.orig_angle = angle; // 记录初始编码器值
|
||||
u->motorfeedback .M2006.last_angle = angle;
|
||||
|
||||
u->motorfeedback .M2006.init_cnt++; // 初始化计数器递增
|
||||
return 0;
|
||||
}
|
||||
|
||||
u->M2006.last_angle = angle;
|
||||
|
||||
delta = angle - u->motorfeedback .M2006.last_angle;
|
||||
delta = angle - u->motorfeedback .M2006.last_angle;
|
||||
if (delta > 4096) {
|
||||
u->motorfeedback .M2006.round_cnt--; // 逆时针跨圈
|
||||
} else if (delta < -4096) {
|
||||
u->motorfeedback .M2006.round_cnt++; // 顺时针跨圈
|
||||
}
|
||||
u->motorfeedback.M2006.last_angle = angle;
|
||||
// 计算总角度
|
||||
float total_angle = (u->M2006.round_cnt * 8191 + (angle - u->M2006.orig_angle)) * MOTOR2006_ECD_TO_ANGLE;
|
||||
u->M2006 .total_angle =total_angle;
|
||||
float total_angle = (u->motorfeedback.M2006 .round_cnt * 8191 + (angle - u->motorfeedback.M2006.orig_angle)) * MOTOR2006_ECD_TO_ANGLE;
|
||||
u->motorfeedback.M2006.total_angle =total_angle;
|
||||
|
||||
float delta_angle = PID_calc(&u->pid.M2006_angle, total_angle, target_angle);
|
||||
float delta_speed = PID_calc(&u->pid.M2006_speed, u->motorfeedback.M2006_rpm, delta_angle);
|
||||
float delta_speed = PID_calc(&u->pid.M2006_speed, u->motorfeedback.M3508_rpm [0], delta_angle);
|
||||
u->motor_target.M2006_angle = target_angle;
|
||||
u->final_out .final_2006out =delta_speed;
|
||||
u->final_out .final_3508out [0] =delta_speed;
|
||||
|
||||
break ;
|
||||
|
||||
case M3508 :
|
||||
|
||||
angle = u->motorfeedback.M3508_angle[1];
|
||||
if (u->motorfeedback .M3508 .init_cnt < 50) {
|
||||
u->motorfeedback .M3508.orig_angle = angle; // 记录初始编码器值
|
||||
u->motorfeedback .M3508.last_angle = angle;
|
||||
|
||||
u->motorfeedback .M3508.init_cnt++; // 初始化计数器递增
|
||||
return 0;
|
||||
}
|
||||
|
||||
int8_t UP_M3508_speed(UP_t *u,fp32 speed)
|
||||
{
|
||||
u->motor_target .M3508_speed =speed;
|
||||
for(int i=0;i<3;i++){
|
||||
u->final_out .final_3508out [i] =
|
||||
PID_calc (&(u->pid .M3508_speed[i] ),u->motorfeedback .M3508_speed [i],speed );
|
||||
delta = angle - u->motorfeedback .M3508.last_angle;
|
||||
delta = angle - u->motorfeedback .M3508.last_angle;
|
||||
if (delta > 4096) {
|
||||
u->motorfeedback .M3508.round_cnt--; // 逆时针跨圈
|
||||
} else if (delta < -4096) {
|
||||
u->motorfeedback .M3508.round_cnt++; // 顺时针跨圈
|
||||
}
|
||||
u->motorfeedback.M3508.last_angle = angle;
|
||||
// 计算总角度
|
||||
total_angle = (u->motorfeedback.M3508 .round_cnt * 8191 + (angle - u->motorfeedback.M3508.orig_angle)) * MOTOR2006_ECD_TO_ANGLE;
|
||||
u->motorfeedback.M3508.total_angle =total_angle;
|
||||
|
||||
delta_angle = PID_calc(&u->pid.M3508_angle , total_angle, target_angle);
|
||||
delta_speed = PID_calc(&u->pid.M3508_speed , u->motorfeedback.M3508_rpm [1], delta_angle);
|
||||
|
||||
u->motor_target.M3508_angle = target_angle;
|
||||
u->final_out .final_3508out[1] =delta_speed;
|
||||
|
||||
break ;
|
||||
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
//int8_t UP_M3508_speed(UP_t *u,fp32 speed)
|
||||
//{
|
||||
// u->motor_target .M3508_speed [] =speed;
|
||||
// for(int i=0;i<3;i++){
|
||||
// u->final_out .final_3508out [i] =
|
||||
// PID_calc (&(u->pid .M3508_speed[i] ),u->motorfeedback .M3508_speed [i],speed );
|
||||
// }
|
||||
//
|
||||
//}
|
||||
|
||||
|
||||
int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
|
||||
@ -107,16 +163,17 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
|
||||
u->motor_target .VESC_5065_M1_rpm =speed;
|
||||
u->motor_target .VESC_5065_M2_rpm =speed;
|
||||
|
||||
u->final_out .final_VESC_5065_M1out =u->motor_target .VESC_5065_M1_rpm;
|
||||
u->final_out .final_VESC_5065_M2out =-u->motor_target .VESC_5065_M2_rpm;
|
||||
u->final_out .final_VESC_5065_M1out =-u->motor_target .VESC_5065_M1_rpm;
|
||||
u->final_out .final_VESC_5065_M2out =u->motor_target .VESC_5065_M2_rpm;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
int8_t GM6020_control(UP_t *u,fp32 angle)
|
||||
{
|
||||
fp32 delat_speed;
|
||||
Clip(&angle,90,270);
|
||||
// Clip(&angle,90,270);
|
||||
|
||||
delat_speed =
|
||||
PID_calc (&(u->pid .GM6020_angle ),u->motorfeedback .rotor_pit6020ecd ,(angle /360*8191));
|
||||
@ -124,6 +181,7 @@ int8_t GM6020_control(UP_t *u,fp32 angle)
|
||||
PID_calc (&(u->pid .GM6020_speed ),u->motorfeedback.rotor_pit6020rpm ,delat_speed);
|
||||
u->motor_target .rotor_pit6020angle =angle ;
|
||||
}
|
||||
|
||||
/*go电机控制*/
|
||||
int8_t GO_SendData(UP_t *u,int id,float pos)
|
||||
{
|
||||
@ -133,35 +191,148 @@ int8_t GO_SendData(UP_t *u,int id,float pos)
|
||||
}
|
||||
|
||||
|
||||
|
||||
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
|
||||
{
|
||||
//电机控制 ，传进can
|
||||
out ->motor3508 .as_array [0]=u->final_out .final_2006out ;
|
||||
|
||||
for(int i=1;i<4;i++){
|
||||
out ->motor3508 .as_array[i]=u->final_out.final_3508out [i-1] ;
|
||||
UP_angle_control(u,u->motor_target .M2006_angle ,M2006);
|
||||
UP_angle_control(u,u->motor_target .M3508_angle ,M3508 );
|
||||
|
||||
|
||||
|
||||
GO_SendData(u,0 ,u->motor_target .go_shoot );
|
||||
GO_SendData(u,1 ,u->motor_target .go_spin);
|
||||
|
||||
|
||||
for(int i=0;i<4;i++){
|
||||
out ->motor3508 .as_array[i]=u->final_out.final_3508out [i] ;
|
||||
}
|
||||
|
||||
out ->chassis5065 .erpm [0]= u->final_out .final_VESC_5065_M1out ;
|
||||
out ->chassis5065 .erpm [1]=u->final_out .final_VESC_5065_M2out ;
|
||||
|
||||
out ->chassis5065 .erpm [1]= -u->final_out .final_VESC_5065_M2out ;
|
||||
out ->chassis6020 .as_array [2]=u->final_out .final_pitchout ;
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
int8_t UP_control(UP_t *u,CAN_Output_t *out)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
{
|
||||
// if(u ==NULL) return 0;
|
||||
// if(u ==NULL) return 0;
|
||||
// UP_M2006_angle(u,2500);
|
||||
switch (u->ctrl)
|
||||
|
||||
if(u ==NULL) return 0;
|
||||
if(u ==NULL) return 0;
|
||||
|
||||
switch (c->CMD_CtrlType )
|
||||
{
|
||||
case STATE_IDLE : break ;
|
||||
case STATE_PRE_DRIBBLE : break ;
|
||||
case STATE_POST_DRIBBLE : break ;
|
||||
case STATE_PRE_LAUNCH : break ;
|
||||
case STATE_POST_LAUNCH : break ;
|
||||
case UP_RCcontrol: //在手动模式下
|
||||
|
||||
switch (c->CMD_UP_mode )
|
||||
{
|
||||
|
||||
case Normal :
|
||||
|
||||
u->state .Pitch_flag =Not_started_Pit;
|
||||
u->state .last_state = Not_started_Pit;
|
||||
|
||||
u->motor_target .go_shoot =0;
|
||||
u->motor_target .M2006_angle =-140;
|
||||
|
||||
break;
|
||||
|
||||
case Pitch_pull :
|
||||
if(u->state .last_state == Not_started_Pit)
|
||||
{
|
||||
|
||||
u->motor_target .go_shoot =-300;
|
||||
u->motor_target .M2006_angle =-140;
|
||||
|
||||
if(u->motorfeedback .GO_motor_info[0]->Pos < (-4.8)) //到达位置后再扣扳机
|
||||
{
|
||||
u->motor_target .M2006_angle =0;
|
||||
if(u->motorfeedback .M2006.total_angle>-5)
|
||||
{//避免没勾上就拉
|
||||
u->motor_target .go_shoot =0;
|
||||
u->state .Pitch_flag = Launch_Ready ;
|
||||
u->state .last_state = Launch_Ready;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
break ;
|
||||
case Pitch_shoot :
|
||||
if (u->state .last_state == Launch_Ready)
|
||||
{
|
||||
|
||||
u->motor_target .M2006_angle =-140;
|
||||
u->state .Pitch_flag = Done_Pit ;
|
||||
u->state .last_state = Done_Pit;
|
||||
|
||||
}
|
||||
{
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
case MID_NAVI :
|
||||
{
|
||||
}
|
||||
break ;
|
||||
|
||||
case PICK_t :
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -5,26 +5,47 @@
|
||||
#include "struct_typedef.h"
|
||||
#include "pid.h"
|
||||
#include "bmi088.h"
|
||||
#include "map.h"
|
||||
#include "user_math.h"
|
||||
#include "ahrs.h"
|
||||
#include "can_use.h"
|
||||
#include "cmd.h"
|
||||
#include "filter.h"
|
||||
#include "Action.h"
|
||||
#include "chassis.h"
|
||||
#include "vofa.h"
|
||||
#include "GO_M8010_6_Driver.h"
|
||||
#include "bsp_usart.h"
|
||||
|
||||
|
||||
|
||||
typedef enum
|
||||
{
|
||||
Not_started_Pit=0,//未开始
|
||||
Launch_Ready=1, //准备发射
|
||||
Launch_complete=2,//发射完成
|
||||
Done_Pit=3 //已完成
|
||||
|
||||
}Pitch_flag_t;
|
||||
|
||||
typedef enum{
|
||||
STATE_IDLE, // 空闲状态
|
||||
STATE_PRE_DRIBBLE, // 运球前
|
||||
STATE_POST_DRIBBLE, // 运球后
|
||||
STATE_PRE_LAUNCH, // 发射前
|
||||
STATE_POST_LAUNCH // 发射后
|
||||
} OperationState;
|
||||
|
||||
Not_started_dri=0,//未开始
|
||||
No_ball =1, //抓上无球
|
||||
Have_ball_F=2, //刚开始有球
|
||||
Have_ball_S=3, //中途有球
|
||||
Done_dri =4 //已完成
|
||||
|
||||
}Dribble_flag_t;
|
||||
|
||||
/*运行控制中的控制*/
|
||||
typedef struct{
|
||||
|
||||
/*投球过程*/
|
||||
Pitch_flag_t Pitch_flag;
|
||||
/*运球过程*/
|
||||
Dribble_flag_t Dribble_flag;
|
||||
|
||||
int last_state;
|
||||
|
||||
} Oper_control_state_t;
|
||||
|
||||
typedef struct {
|
||||
|
||||
@ -52,17 +73,18 @@ typedef struct {
|
||||
float K_W;
|
||||
|
||||
}GO_param_t;
|
||||
|
||||
/*角度环控制电机类型*/
|
||||
typedef enum {
|
||||
M2006 = 1,
|
||||
M3508
|
||||
|
||||
} MotorType_t;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
/*该部分决定PID的参数整定在config中修改*/
|
||||
|
||||
pid_param_t VESC_5065_M1_param;
|
||||
pid_param_t VESC_5065_M2_param;
|
||||
|
||||
@ -73,6 +95,8 @@ typedef struct
|
||||
pid_param_t M2006_angle_param;
|
||||
|
||||
pid_param_t M3508_speed_param;
|
||||
pid_param_t M3508_angle_param;
|
||||
|
||||
GO_param_t go_param;
|
||||
|
||||
}UP_Param_t;
|
||||
@ -80,6 +104,7 @@ typedef struct
|
||||
typedef struct
|
||||
{
|
||||
MotorType_t motor;
|
||||
|
||||
float orig_angle;
|
||||
float last_angle;
|
||||
int32_t round_cnt;
|
||||
@ -93,25 +118,29 @@ typedef struct{
|
||||
|
||||
uint8_t up_task_run;
|
||||
const UP_Param_t *param;
|
||||
|
||||
UP_Imu_t pos088;
|
||||
|
||||
OperationState ctrl;
|
||||
motor_angle_data M2006;
|
||||
motor_angle_data M3508;
|
||||
GO_Motorfield *GO_motor_info[GO_NUM];//存放go电机数据
|
||||
/*控制及状态*/
|
||||
CMD_t *cmd;
|
||||
Oper_control_state_t state;//上层机构的运行状态
|
||||
|
||||
struct{
|
||||
|
||||
fp32 rotor_pit6020ecd;
|
||||
fp32 rotor_pit6020rpm;
|
||||
|
||||
fp32 VESC_5065_M1_rpm;
|
||||
fp32 VESC_5065_M2_rpm;
|
||||
|
||||
fp32 M2006_rpm;
|
||||
fp32 M2006_angle;
|
||||
motor_angle_data M2006;
|
||||
motor_angle_data M3508;
|
||||
|
||||
fp32 M3508_speed[3];
|
||||
GO_Motorfield *GO_motor_info[GO_NUM];//存放go电机数据
|
||||
|
||||
fp32 M3508_angle[4];
|
||||
fp32 M3508_rpm [4];
|
||||
|
||||
int flag;
|
||||
|
||||
}motorfeedback;
|
||||
|
||||
@ -125,7 +154,10 @@ typedef struct{
|
||||
fp32 VESC_5065_M2_rpm;
|
||||
|
||||
fp32 M2006_angle;
|
||||
fp32 M3508_speed;
|
||||
fp32 M3508_angle;
|
||||
|
||||
fp32 go_shoot;
|
||||
fp32 go_spin;
|
||||
|
||||
}motor_target;
|
||||
|
||||
@ -133,28 +165,29 @@ typedef struct{
|
||||
|
||||
pid_type_def GM6020_speed;
|
||||
pid_type_def GM6020_angle;
|
||||
|
||||
pid_type_def VESC_5065_M1;
|
||||
pid_type_def VESC_5065_M2;
|
||||
|
||||
pid_type_def M2006_angle;
|
||||
pid_type_def M2006_speed;
|
||||
pid_type_def M3508_speed[3];
|
||||
|
||||
pid_type_def M3508_angle;
|
||||
pid_type_def M3508_speed;
|
||||
|
||||
}pid;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/*经PID计算后的实际发送给电机的实时输出值*/
|
||||
struct
|
||||
{
|
||||
fp32 final_2006out;
|
||||
fp32 final_3508out[3];
|
||||
fp32 final_3508out[4];
|
||||
fp32 final_pitchout;
|
||||
fp32 final_VESC_5065_M1out;
|
||||
fp32 final_VESC_5065_M2out;
|
||||
|
||||
|
||||
}final_out;
|
||||
|
||||
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
|
||||
@ -162,17 +195,18 @@ typedef struct{
|
||||
fp32 vofa_send[8];
|
||||
|
||||
|
||||
|
||||
} UP_t;
|
||||
|
||||
|
||||
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq);
|
||||
|
||||
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can) ;
|
||||
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) ;
|
||||
int8_t GM6020_control(UP_t *u,fp32 angle);
|
||||
int8_t VESC_M5065_Control(UP_t *u,fp32 speed);
|
||||
int8_t GO_SendData(UP_t *u,int id,float pos);
|
||||
int8_t UP_angle(UP_t *u, fp32 target_angle) ;
|
||||
int8_t UP_control(UP_t *u,CAN_Output_t *out);
|
||||
int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor);
|
||||
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c);
|
||||
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out);
|
||||
int8_t UP_M2006_angle(UP_t *u,fp32 target_angle);
|
||||
int8_t UP_M3508_speed(UP_t *u,fp32 speed);
|
||||
|
||||
@ -1,203 +0,0 @@
|
||||
|
||||
/**
|
||||
* 东北大学ACTION码盘驱动
|
||||
*
|
||||
* 本驱动采用ops_9定位系统，负责收发码盘的原始数据，并在任务中解析出相应的位置坐标。
|
||||
* 解析后的位置坐标将被交给导航处理模块，生成期望的运动向量。
|
||||
*
|
||||
* 使用手册：
|
||||
* 请参阅相关文档了解如何使用本驱动。
|
||||
*
|
||||
* 注意：
|
||||
* 本驱动仅适用于东北大学ACTION码盘。
|
||||
|
||||
*删去Action_HandleOffline错误处理函数中对结构体清0的函数
|
||||
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "tim.h"
|
||||
#include "Action.h"
|
||||
#include <string.h>
|
||||
|
||||
static osThreadId_t thread_alert;
|
||||
|
||||
|
||||
uint8_t rxbuf[RS232_FRAME_LENGTH];
|
||||
|
||||
static void Ops10msTimerCallback(void *arg){
|
||||
(void)arg;
|
||||
osThreadFlagsSet(thread_alert,SIGNAL_OPSTIMER_REDY);
|
||||
}
|
||||
|
||||
/* Private function -------------------------------------------------------- */
|
||||
static void ACTION_IdleCallback(void) {
|
||||
osThreadFlagsSet(thread_alert,SIGNAL_ACTION_RAW_REDY);
|
||||
}
|
||||
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
int8_t ACTIONRECV_Init(Action_POS_t *pos){
|
||||
if(pos == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
pos->Action_ready =0;//码盘校准标志位初始化
|
||||
|
||||
if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
|
||||
pos->action_timer_Id =
|
||||
osTimerNew(Ops10msTimerCallback, osTimerPeriodic,NULL,NULL);
|
||||
|
||||
osTimerStart(pos->action_timer_Id,10);//此处ticks 理解为1个tick为1ms 此处为每10ms触发一次回调函数
|
||||
|
||||
BSP_UART_RegisterCallback(BSP_UART_ACTION,BSP_UART_IDLE_LINE_CB,
|
||||
ACTION_IdleCallback);
|
||||
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
int8_t ACTION_StartReceiving() {
|
||||
if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_ACTION),
|
||||
(uint8_t *)rxbuf,
|
||||
sizeof(rxbuf)) == HAL_OK)
|
||||
return DEVICE_OK;
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
bool_t ACTION_WaitDmaCplt(void) {
|
||||
return(osThreadFlagsWait(SIGNAL_ACTION_RAW_REDY, osFlagsWaitAll,400) ==
|
||||
SIGNAL_ACTION_RAW_REDY);
|
||||
}
|
||||
|
||||
|
||||
/* *
|
||||
|
||||
|
||||
*/
|
||||
int8_t ACTION_Parse(Action_POS_t *pos)
|
||||
{
|
||||
if (pos == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
static union
|
||||
{
|
||||
fp32 pos_data[3];
|
||||
char rxbuff[12];
|
||||
} pos_rxbuf;
|
||||
|
||||
|
||||
if (rxbuf[0] == 0x0D && rxbuf[1] == 0x0A)
|
||||
{
|
||||
if (rxbuf[26] == 0x0A && rxbuf[27] == 0x0D)
|
||||
{
|
||||
|
||||
pos_rxbuf.rxbuff[0]=rxbuf[2];
|
||||
pos_rxbuf.rxbuff[1]=rxbuf[3];
|
||||
pos_rxbuf.rxbuff[2]=rxbuf[4];
|
||||
pos_rxbuf.rxbuff[3]=rxbuf[5];
|
||||
|
||||
|
||||
pos_rxbuf.rxbuff[4]=rxbuf[14];
|
||||
pos_rxbuf.rxbuff[5]=rxbuf[15];
|
||||
pos_rxbuf.rxbuff[6]=rxbuf[16];
|
||||
pos_rxbuf.rxbuff[7]=rxbuf[17];
|
||||
|
||||
|
||||
pos_rxbuf.rxbuff[8]=rxbuf[18];
|
||||
pos_rxbuf.rxbuff[9]=rxbuf[19];
|
||||
pos_rxbuf.rxbuff[10]=rxbuf[20];
|
||||
pos_rxbuf.rxbuff[11]=rxbuf[21];
|
||||
|
||||
|
||||
|
||||
// 数据解析
|
||||
pos->pos_yaw = pos_rxbuf.pos_data[0];
|
||||
// 按照安装方向决定正负号
|
||||
pos->pos_x = pos_rxbuf.pos_data[1];
|
||||
pos->pos_y = pos_rxbuf.pos_data[2];
|
||||
|
||||
pos ->Action_ready =1;//码盘校准完成
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
//该函数用来计算速度(利用获取的位置来计算相应的速度)
|
||||
//此处获取的是真实的位置(mm)后每10ms做一次微分处理
|
||||
int8_t ACTION_DataRefresh(Action_POS_t *pos){
|
||||
if (pos == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
if (osThreadFlagsGet() & SIGNAL_OPSTIMER_REDY){
|
||||
osThreadFlagsClear(SIGNAL_OPSTIMER_REDY);
|
||||
//计算每毫秒的速度 该函数每10ms调用
|
||||
pos->pos_Vx = (pos->pos_x - pos->pos_lastX) / 10;
|
||||
pos->pos_Vy = (pos->pos_y - pos->pos_lastY) / 10;
|
||||
|
||||
pos->pos_lastX = pos->pos_x;
|
||||
pos->pos_lastY = pos->pos_y;
|
||||
}
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int8_t Action_HandleOffline(Action_POS_t *pos) {
|
||||
if (pos == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
(void)pos;
|
||||
|
||||
// memset(pos, 0, sizeof(*pos));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* 将字符串拼接 */
|
||||
void Strcat(char str1[], char str2[], uint8_t num)
|
||||
{
|
||||
int i = 0, j = 0;
|
||||
|
||||
while (str1[i] != '\0')
|
||||
i++;
|
||||
|
||||
for (j = 0; j < num; j++)
|
||||
{
|
||||
str1[i++] = str2[j];
|
||||
}
|
||||
}
|
||||
|
||||
/* 该部分函数用于码盘的重定位系统 */
|
||||
/* ----------------------------- */
|
||||
|
||||
/* 手动标定，用于校正 */
|
||||
void ACT_Calibration(void)
|
||||
{
|
||||
HAL_UART_Transmit(&huart1, (uint8_t *)"ACTR", 4, 100);
|
||||
}
|
||||
|
||||
/* 清零 */
|
||||
/* 将当前位置设置为 (0, 0) */
|
||||
void ACT_ZeroClear(void)
|
||||
{
|
||||
HAL_UART_Transmit(&huart1, (uint8_t *)"ACT0", 4, 100);
|
||||
}
|
||||
|
||||
/* 更新 XY 坐标 */
|
||||
/* 将当前位置更新为指定的 X 和 Y */
|
||||
void ACT_UpdateXY(float pos_x, float pos_y)
|
||||
{
|
||||
char update_xy[12] = "ACTD"; // 命令前缀为 "ACTD"
|
||||
static union
|
||||
{
|
||||
float XY[2]; // 两个浮点数表示 X 和 Y 坐标
|
||||
char data[8]; // 将浮点数视为字节数据
|
||||
} set;
|
||||
|
||||
set.XY[0] = pos_x;
|
||||
set.XY[1] = pos_y;
|
||||
|
||||
Strcat(update_xy, set.data, 8);
|
||||
|
||||
HAL_UART_Transmit(&huart1, (uint8_t *)update_xy, sizeof(update_xy), 100);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -1,59 +0,0 @@
|
||||
#ifndef _ACTION_H_
|
||||
#define _ACTION_H_
|
||||
|
||||
#define RS232_FRAME_LENGTH 28u
|
||||
#define REF_LEN_RX_BUFF 0xFF
|
||||
|
||||
#include <cmsis_os2.h>
|
||||
#include "device.h"
|
||||
#include "bsp_usart.h"
|
||||
#include "cmd.h"
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
osTimerId_t action_timer_Id;
|
||||
/*原始数据*/
|
||||
fp32 pos_x;
|
||||
fp32 pos_y;
|
||||
fp32 pos_yaw; // 航向角速度
|
||||
/*处理数据*/
|
||||
fp32 pos_Vx;
|
||||
fp32 pos_Vy;
|
||||
fp32 pos_lastX;
|
||||
fp32 pos_lastY;
|
||||
|
||||
int Action_ready;//码盘校准标志位
|
||||
|
||||
|
||||
}Action_POS_t;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
int8_t ACTIONRECV_Init(Action_POS_t*pos);
|
||||
|
||||
int8_t ACTION_StartReceiving(void);
|
||||
|
||||
bool_t ACTION_WaitDmaCplt(void);
|
||||
|
||||
int8_t ACTION_Parse(Action_POS_t*pos);
|
||||
|
||||
int8_t ACTION_DataRefresh(Action_POS_t*pos);
|
||||
|
||||
int8_t Action_HandleOffline(Action_POS_t *pos);
|
||||
|
||||
void ACT_UpdateXY(float pos_x,float pos_y);
|
||||
|
||||
void ACT_ZeroClear(void);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@ -8,6 +8,7 @@ extern "C"{
|
||||
#include "usart.h"
|
||||
#include "string.h"
|
||||
#include <math.h>
|
||||
#include "user_math.h"
|
||||
#define GO_NUM 2
|
||||
/**
|
||||
* @brief
|
||||
@ -63,6 +64,7 @@ typedef struct {
|
||||
float T;
|
||||
float W;
|
||||
float Pos;
|
||||
|
||||
int footForce;
|
||||
uint8_t buffer[17];
|
||||
uint8_t Rec_buffer[16];
|
||||
|
||||
@ -13,74 +13,27 @@
|
||||
|
||||
/* Private function -------------------------------------------------------- */
|
||||
/*Export function --------------------------------------------------------------*/
|
||||
int8_t CMD_Init(CMD_t *cmd){
|
||||
/*若主结构体为空 自动返回错误 */
|
||||
if(cmd == NULL) return-1;
|
||||
/**/
|
||||
cmd->C_cmd.type =RC;
|
||||
cmd->C_cmd.mode =NORMAL;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
|
||||
|
||||
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
|
||||
|
||||
cmd->Vx = rc->ch_r_x;
|
||||
cmd->Vy = -rc->ch_r_y;
|
||||
cmd->Vw = rc->ch_l_x;
|
||||
|
||||
cmd->poscamear = rc->ch_l_y;
|
||||
|
||||
cmd->key_ctrl_l = rc->sw_l;
|
||||
cmd->key_ctrl_r = rc->sw_r ;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief rc失控时机器人恢复放松模式
|
||||
*
|
||||
* @param cmd 主结构体
|
||||
*/
|
||||
static void CMD_RcLostLogic(CMD_t *cmd){
|
||||
/* 机器人底盘运行模式恢复至放松模式 */
|
||||
cmd->C_cmd.mode = RELAXED;
|
||||
}
|
||||
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){
|
||||
if (cmd == NULL) return -1;
|
||||
if (rc == NULL) return -1;
|
||||
/*c当rc丢控时，恢复机器人至默认状态 */
|
||||
if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
|
||||
CMD_RcLostLogic(cmd);
|
||||
} else {
|
||||
CMD_RcLogic(rc, cmd);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*nuc数据统一到cmd*/
|
||||
int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
|
||||
if(cmd == NULL) return -1;
|
||||
if(n == NULL) return -1;
|
||||
|
||||
cmd->cmd_status = n->status_fromnuc;
|
||||
cmd->raw_status = n->ctrl_status;
|
||||
cmd->NAVI_t .cmd_status = n->status_fromnuc;
|
||||
cmd->NAVI_t .raw_status = n->ctrl_status;
|
||||
|
||||
for (int i = 0; i < 7; ++i)
|
||||
{ // 从最低位到最高位遍历
|
||||
cmd->status[i] = ((cmd->raw_status) & (1 << i)) ? 1 : 0;
|
||||
cmd->NAVI_t .status[i] = ((cmd->NAVI_t .raw_status) & (1 << i)) ? 1 : 0;
|
||||
}
|
||||
switch(cmd->cmd_status){
|
||||
switch(cmd->NAVI_t .cmd_status){
|
||||
|
||||
case PICK :
|
||||
cmd ->NAVI_t .pick .posx =n->pick .posx ;
|
||||
cmd ->NAVI_t .pick .posy =n->pick .posy ;
|
||||
cmd ->NAVI_t .pick .posw =n->pick .posw ;
|
||||
|
||||
case MID:
|
||||
cmd->C_navi.vx = n->navi.vx;
|
||||
cmd->C_navi.vy = n->navi.vy;
|
||||
cmd->C_navi.wz = n->navi.wz;
|
||||
break;
|
||||
|
||||
|
||||
@ -88,108 +41,40 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
遥控器模式重新分配
|
||||
|
||||
这一部分没有设置具体的模式名，后期根据需要修改
|
||||
遥控器模式(RC):
|
||||
左按键 --- 右按键
|
||||
mode1
|
||||
up no_mode
|
||||
mode2
|
||||
|
||||
|
||||
mode3
|
||||
down no_mode
|
||||
mode4
|
||||
|
||||
mid auto_navi(0x09)雷达导航
|
||||
|
||||
*/
|
||||
int8_t CMD_CtrlSet(CMD_t *cmd) {
|
||||
/*遥控器，上下层通用，按键控制，统一到cmd*/
|
||||
int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) {
|
||||
|
||||
if(cmd == NULL) return -1;
|
||||
if(cmd->key_ctrl_l == CMD_SW_UP)//当左拨杆打到最上面时 强制使用遥控器控制
|
||||
{
|
||||
/*遥控器模式下，右按键三种状态分配*/
|
||||
|
||||
if(cmd->key_ctrl_r==CMD_SW_UP)
|
||||
{
|
||||
cmd->C_cmd.type = RC;
|
||||
cmd->C_cmd.mode = RC_MODE1;
|
||||
if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
|
||||
cmd->CMD_CtrlType =RELAXED;
|
||||
}
|
||||
if(cmd->key_ctrl_r==CMD_SW_MID)
|
||||
else if(rc->sw_l==CMD_SW_UP)
|
||||
{
|
||||
cmd->C_cmd.type = RC;
|
||||
cmd->C_cmd.mode = RC_NO_MODE;
|
||||
}
|
||||
if(cmd->key_ctrl_r==CMD_SW_DOWN)//遥控器控制模式下，右拨杆上时启动
|
||||
{
|
||||
cmd->C_cmd.type = RC;
|
||||
cmd->C_cmd.mode =RC_MODE2;
|
||||
cmd ->CMD_CtrlType =UP_RCcontrol;
|
||||
if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Normal; //左上，右上，投篮
|
||||
if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Pitch_pull; //左上，右中，无模式
|
||||
if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Pitch_shoot; //左上，右上，投篮
|
||||
}
|
||||
|
||||
}
|
||||
else if(cmd->key_ctrl_l ==CMD_SW_DOWN)
|
||||
else if(rc->sw_l==CMD_SW_MID)
|
||||
{
|
||||
if(cmd->key_ctrl_r==CMD_SW_UP)
|
||||
{
|
||||
cmd->C_cmd.type = RC;
|
||||
cmd->C_cmd.mode = RC_MODE3;
|
||||
}
|
||||
if(cmd->key_ctrl_r==CMD_SW_MID)
|
||||
{
|
||||
cmd->C_cmd.type = RC;
|
||||
cmd->C_cmd.mode = RC_NO_MODE;
|
||||
}
|
||||
if(cmd->key_ctrl_r==CMD_SW_DOWN)//遥控器控制模式下，右拨杆上时启动
|
||||
{
|
||||
cmd->C_cmd.type = RC;
|
||||
cmd->C_cmd.mode = RC_MODE4;
|
||||
}
|
||||
|
||||
if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_CtrlType =MID_NAVI;; //左中，右上，雷达
|
||||
if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左中，右中，无模式
|
||||
if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Dribble; //左中，右上，运球
|
||||
|
||||
}
|
||||
else //左按键打到中间，自动模式
|
||||
else if(rc->sw_l==CMD_SW_DOWN)
|
||||
{
|
||||
if( cmd->key_ctrl_l==CMD_SW_MID ){
|
||||
cmd->C_cmd.type = MID_NAVI;
|
||||
cmd->C_cmd.mode = RC_NO_MODE;
|
||||
|
||||
switch(cmd->cmd_status)
|
||||
{
|
||||
case MID://雷达，视觉导航
|
||||
cmd->C_cmd.type = MID_NAVI;
|
||||
break;
|
||||
cmd ->CMD_CtrlType =PICK_t;
|
||||
if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Normal; //左下，右上，投篮
|
||||
if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左下，右中，无模式
|
||||
if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Normal; //左下，右上，投篮
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
//接收码盘导航的返回数据，传入cmd_t结构体
|
||||
int8_t CMD_ParseAction(CMD_t *cmd,CMD_ACTION_t *act)
|
||||
{
|
||||
if(cmd == NULL) return -1;
|
||||
if(act == NULL) return -1;
|
||||
|
||||
if(cmd->key_ctrl_l ==CMD_SW_MID )
|
||||
{
|
||||
if(cmd->key_ctrl_r == CMD_SW_UP)
|
||||
act->flag =1;
|
||||
|
||||
if(cmd->key_ctrl_r == CMD_SW_DOWN )
|
||||
act->flag =-1;
|
||||
|
||||
}
|
||||
|
||||
cmd->C_navi .vx =act ->out .Vx ;
|
||||
cmd ->C_navi .vy =act ->out .Vy ;
|
||||
cmd ->C_navi .wz =act ->out .Vw ;
|
||||
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -1,19 +1,3 @@
|
||||
/*
|
||||
|
||||
该任务用于接收来自各个不同的控制方式所期望的控制指令 将其集中统一化后分发给各个模块
|
||||
|
||||
*/
|
||||
|
||||
|
||||
/*
|
||||
按键控制逻辑
|
||||
RC模式，左按键打到最上，右按键启用，中间无状态，上下各代表模式1、模式2
|
||||
雷达导航，左按键打到中间，右按键禁用
|
||||
左按键打到下面，保留，未启用
|
||||
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
#ifndef _CMD_H
|
||||
@ -23,38 +7,45 @@
|
||||
|
||||
|
||||
#define MID (0x09)
|
||||
#define PICK (0x06)
|
||||
|
||||
typedef enum{
|
||||
RC,//遥控器控制，左按键上
|
||||
UP_RCcontrol,//遥控器控制，左按键上，控制上层
|
||||
MID_NAVI,//雷达导航
|
||||
}CMD_Chassis_CtrlType_e;
|
||||
PICK_t,
|
||||
RELAXED,//异常模式
|
||||
|
||||
}CMD_CtrlType_t;
|
||||
|
||||
typedef enum{
|
||||
RELAXED,//异常模式
|
||||
NORMAL,
|
||||
GYRO_STAY,
|
||||
|
||||
RC_MODE1,
|
||||
RC_NO_MODE,
|
||||
RC_MODE2,
|
||||
|
||||
RC_MODE3,
|
||||
RC_MODE4,
|
||||
Normal, //无模式
|
||||
Dribble , //运球
|
||||
Pitch_pull , //投篮
|
||||
Pitch_shoot , //投篮
|
||||
|
||||
AUTO_NAVI
|
||||
}CMD_Chassis_mode_e;
|
||||
/*该结构体负责接收和解析地盘模块所需要的控制指令*/
|
||||
}CMD_UP_mode_t;
|
||||
typedef struct {
|
||||
|
||||
CMD_Chassis_CtrlType_e type;
|
||||
|
||||
CMD_Chassis_mode_e mode;
|
||||
|
||||
|
||||
}CMD_Chassis_Ctrl_t;
|
||||
|
||||
|
||||
|
||||
uint8_t status_fromnuc;
|
||||
uint8_t ctrl_status; //取其中每一个二进制位用作通信
|
||||
struct
|
||||
{
|
||||
fp32 vx;
|
||||
fp32 vy;
|
||||
fp32 wz;
|
||||
}navi;
|
||||
struct
|
||||
{
|
||||
fp32 posx;
|
||||
fp32 posy;
|
||||
fp32 posw;
|
||||
}pick;
|
||||
struct
|
||||
{
|
||||
fp32 angle;
|
||||
}sick_cali;
|
||||
} CMD_NUC_t;
|
||||
/* 拨杆位置 */
|
||||
typedef enum {
|
||||
CMD_SW_ERR = 0,
|
||||
@ -82,102 +73,38 @@ typedef struct {
|
||||
|
||||
} __attribute__((packed))CMD_RC_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
fp32 Vx;
|
||||
fp32 Vy;
|
||||
fp32 Vw;
|
||||
}OpsHopeVector_t;
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
fp32 x;
|
||||
fp32 y;
|
||||
fp32 z;
|
||||
int cnt_point;//计数点
|
||||
}ops_point;
|
||||
|
||||
|
||||
typedef struct {
|
||||
|
||||
OpsHopeVector_t out;
|
||||
|
||||
int flag;//传递flag触发标志位
|
||||
fp32 key_ctrl_l;
|
||||
fp32 key_ctrl_r;
|
||||
|
||||
} CMD_ACTION_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t status_fromnuc;
|
||||
uint8_t ctrl_status; //取其中每一个二进制位用作通信
|
||||
struct
|
||||
{
|
||||
fp32 vx;
|
||||
fp32 vy;
|
||||
fp32 wz;
|
||||
}navi;
|
||||
/*视觉*/
|
||||
struct {
|
||||
uint8_t cmd_status;
|
||||
uint8_t raw_status;
|
||||
uint8_t status[7];
|
||||
struct
|
||||
{
|
||||
fp32 posx;
|
||||
fp32 posy;
|
||||
fp32 posw;
|
||||
}pick;
|
||||
struct
|
||||
{
|
||||
fp32 angle;
|
||||
}sick_cali;
|
||||
} CMD_NUC_t;
|
||||
|
||||
typedef struct{
|
||||
fp32 posy;
|
||||
fp32 posx;
|
||||
fp32 posw;
|
||||
}CMD_FOR_PICK;
|
||||
}NAVI_t;
|
||||
|
||||
|
||||
typedef struct {
|
||||
fp32 vx;
|
||||
fp32 vy;
|
||||
fp32 wz;
|
||||
CMD_UP_mode_t CMD_UP_mode;
|
||||
CMD_CtrlType_t CMD_CtrlType;
|
||||
|
||||
|
||||
}CMD_FOR_NAVI;
|
||||
|
||||
typedef struct {
|
||||
uint8_t cmd_status;
|
||||
uint8_t raw_status;
|
||||
|
||||
uint8_t status[7];
|
||||
|
||||
fp32 Vx;
|
||||
fp32 Vy;
|
||||
fp32 Vw;
|
||||
|
||||
fp32 poscamear;
|
||||
|
||||
fp32 key_ctrl_l;
|
||||
fp32 key_ctrl_r;
|
||||
|
||||
fp32 forsick_wz;
|
||||
|
||||
CMD_Chassis_Ctrl_t C_cmd;
|
||||
CMD_FOR_NAVI C_navi;
|
||||
|
||||
} CMD_t;
|
||||
|
||||
|
||||
int8_t CMD_Init(CMD_t *cmd);
|
||||
|
||||
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc);
|
||||
|
||||
int8_t CMD_ParseAction(CMD_t *cmd,CMD_ACTION_t *act);
|
||||
|
||||
/*nuc数据统一到cmd*/
|
||||
int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n);
|
||||
|
||||
|
||||
int8_t CMD_SwitchStatus(CMD_t *cmd);
|
||||
|
||||
int8_t CMD_CtrlSet(CMD_t *cmd);
|
||||
int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) ;
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@ -1,75 +0,0 @@
|
||||
/*
|
||||
码盘解析任务
|
||||
|
||||
处理码盘测量的实际距离
|
||||
|
||||
将需要的数据共享给其他的线程
|
||||
|
||||
*/
|
||||
#include "action_task.h"
|
||||
#include "Action.h"
|
||||
#include "user_task.h"
|
||||
|
||||
|
||||
#ifdef DEBUG
|
||||
|
||||
Action_POS_t pos;
|
||||
#else
|
||||
|
||||
static Action_POS pos;
|
||||
|
||||
#endif
|
||||
|
||||
void Task_action(void *argument)
|
||||
{
|
||||
(void)argument;
|
||||
// osDelay(TASK_INIT_DELAY_ACTION);
|
||||
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_ACTION;
|
||||
|
||||
|
||||
ACTIONRECV_Init(&pos);
|
||||
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 获取当前控制任务运行频率的tick*/
|
||||
|
||||
while (1)
|
||||
{
|
||||
#ifdef DEBUG
|
||||
/* 记录任务使用的的栈空闄*/
|
||||
task_runtime.stack_water_mark.action =
|
||||
osThreadGetStackSpace(osThreadGetId());
|
||||
#endif
|
||||
ACTION_StartReceiving();
|
||||
ACTION_DataRefresh(&pos);
|
||||
|
||||
|
||||
|
||||
/* 使用下面的错误处理函数遇到一些问题
|
||||
每10ms置标志位， ACTION_WaitDmaCplt 判断此标志位，通过后会执行速度计算
|
||||
根据计算，任务运行频率大于中断产生频率，每一次运行判断标志位不通过，会导致结构体内数据清0的操作
|
||||
目前的处理方法：注释掉Action_HandleOffline函数中，对结构体数据清零的函数memset
|
||||
|
||||
|
||||
*/
|
||||
|
||||
if(ACTION_WaitDmaCplt()){
|
||||
|
||||
ACTION_Parse(&pos);
|
||||
|
||||
}
|
||||
else{
|
||||
Action_HandleOffline(&pos);
|
||||
}
|
||||
|
||||
|
||||
//将解算后的码盘位置值放入消息队列供其他任务使用
|
||||
osMessageQueueReset(task_runtime.msgq.cmd.raw.Action);
|
||||
osMessageQueuePut(task_runtime.msgq.cmd.raw.Action,(&pos),0,0);
|
||||
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻*/
|
||||
osDelayUntil(tick);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1 +0,0 @@
|
||||
|
||||
@ -1,94 +0,0 @@
|
||||
/*
|
||||
底盘控制任务
|
||||
*/
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
|
||||
#include "Chassis.h"
|
||||
#include "user_task.h"
|
||||
#include "can_use.h"
|
||||
#include <cmsis_os2.h>
|
||||
#include "up.h"
|
||||
#include "vofa.h"
|
||||
|
||||
static CAN_t can;
|
||||
|
||||
|
||||
|
||||
#ifdef DEBUG
|
||||
|
||||
CAN_Output_t out;
|
||||
|
||||
Chassis_t chassis ;
|
||||
|
||||
CMD_t ctrl;
|
||||
|
||||
UP_t upp;
|
||||
#else
|
||||
|
||||
static CAN_Output_t out;
|
||||
|
||||
static Chassis_t chassis;
|
||||
|
||||
static Chassis_Ctrl_t ctrl;
|
||||
|
||||
static UP_t upp;
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* \brief 底盘任务
|
||||
*
|
||||
* \param argument
|
||||
*/
|
||||
void Task_Chassis(void *argument)
|
||||
{
|
||||
|
||||
(void)argument; /* 未使用argument，消除警告*/
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_UP;
|
||||
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
//
|
||||
//
|
||||
//// HAL_GPIO_WritePin(FlagForUpper_GPIO_Port,FlagForUpper_Pin,GPIO_PIN_RESET); //拉低电平 避免未清除
|
||||
//
|
||||
while(1)
|
||||
{
|
||||
#ifdef DEBUG
|
||||
task_runtime.stack_water_mark.chassis = osThreadGetStackSpace(osThreadGetId());
|
||||
#endif
|
||||
|
||||
// /*imu数据获取*/
|
||||
// osMessageQueueGet(task_runtime.msgq.imu.eulr, &chassis.pos088.imu_eulr, NULL, 0);
|
||||
//
|
||||
// osMessageQueueGet(task_runtime.msgq.imu.gyro, &chassis.pos088.bmi088.gyro,NULL, 0);
|
||||
//
|
||||
// osMessageQueueGet(task_runtime.msgq.imu.accl, &chassis.pos088.bmi088.accl,NULL, 0);
|
||||
// /*can上设备数据获取*/
|
||||
// osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
|
||||
//
|
||||
// osMessageQueueGet(task_runtime.msgq.cmd.raw.Action, &chassis.Action_pos , NULL, 0);
|
||||
|
||||
/*底盘控制信息获取,目前dji遥控器*/
|
||||
osMessageQueueGet(task_runtime.msgq.cmd.chassis,&ctrl, NULL, 0);
|
||||
|
||||
/*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/
|
||||
// osKernelLock();
|
||||
|
||||
/*更新电机反馈 */
|
||||
Chassis_UpdateFeedback(&chassis, &can);
|
||||
|
||||
/*底盘控制*/
|
||||
Chassis_Control(&chassis,&ctrl,&out);
|
||||
|
||||
/*解锁*/
|
||||
// osKernelUnlock();
|
||||
|
||||
tick += delay_tick;
|
||||
osDelayUntil(tick);
|
||||
}
|
||||
|
||||
}
|
||||
@ -1,23 +0,0 @@
|
||||
#ifndef CHASSIS_TASK_H
|
||||
|
||||
|
||||
#define CHASSIS_TASK_H
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
extern void Task_Chassis(void *argument);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@ -4,16 +4,14 @@
|
||||
#ifdef DEBUG
|
||||
|
||||
CMD_t cmd;
|
||||
CMD_RC_t rc_ctrl;
|
||||
CMD_NUC_t Nuc;
|
||||
CMD_ACTION_t cmd_ops_out;
|
||||
CMD_RC_t rc_ctrl;
|
||||
|
||||
#else
|
||||
|
||||
static CMD_t cmd;
|
||||
static CMD_RC_t rc_ctrl;
|
||||
static CMD_NUC_t Nuc;
|
||||
static CMD_ACTION_t cmd_ops_out;
|
||||
static CMD_RC_t rc_ctrl;
|
||||
|
||||
#endif
|
||||
|
||||
@ -26,7 +24,6 @@ void Task_cmd(void *argument){
|
||||
/* 计算到下一次调度任务所需要的tick数 */
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_CMD;
|
||||
/**/
|
||||
CMD_Init(&cmd);
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /*控制任务运行频率的计时 */
|
||||
while(1){
|
||||
@ -40,25 +37,17 @@ void Task_cmd(void *argument){
|
||||
|
||||
/*将各任务接收到的原始数据解析为通用的控制命令*/
|
||||
|
||||
|
||||
|
||||
/*注意，不能将nuc和码盘导航一块使用*/
|
||||
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器
|
||||
CMD_ParseRC(&cmd, &rc_ctrl);
|
||||
|
||||
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK) //nuc
|
||||
CMD_ParseNuc(&cmd,&Nuc);
|
||||
|
||||
CMD_CtrlSet(&cmd);
|
||||
|
||||
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器
|
||||
CMD_ParseRc(&cmd, &rc_ctrl);
|
||||
|
||||
if(osMessageQueueGet(task_runtime.msgq.cmd.raw .ops_9_out , &cmd_ops_out, 0, 0) == osOK)//码盘
|
||||
CMD_ParseAction (&cmd ,&cmd_ops_out);
|
||||
osKernelUnlock(); /* 同上 解锁RTOS内核 */
|
||||
|
||||
/*将需要与其他任务共享的数据放到消息队列里 此处主要分享给底盘 后续会添加和上层机构的通信 */
|
||||
osMessageQueueReset(task_runtime.msgq.cmd.chassis);
|
||||
osMessageQueuePut(task_runtime.msgq.cmd.chassis,&cmd,0,0);
|
||||
osMessageQueueReset(task_runtime.msgq.cmd.up_ctrl);
|
||||
|
||||
osMessageQueuePut(task_runtime.msgq.cmd.up_ctrl,&cmd,0,0);
|
||||
|
||||
tick += delay_tick; /*计算下一个唤醒时刻*/
|
||||
osDelayUntil(tick); /*绝对延时 等待下一个唤醒时刻 */
|
||||
|
||||
@ -11,7 +11,6 @@
|
||||
#include "task\user_task.h"
|
||||
#include "can_use.h"
|
||||
#include "cmd.h"
|
||||
#include "Action.h"
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
@ -86,6 +85,8 @@ void Task_Init(void *argument) {
|
||||
osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
|
||||
task_runtime.msgq.cmd.raw.nuc =
|
||||
osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL);
|
||||
task_runtime.msgq.cmd.up_ctrl =
|
||||
osMessageQueueNew(3u,sizeof(CMD_t), NULL);
|
||||
|
||||
osKernelUnlock();
|
||||
osThreadTerminate(osThreadGetId()); /* 结束自身 */
|
||||
|
||||
@ -1,68 +0,0 @@
|
||||
#include "user_task.h"
|
||||
#include "navi.h"
|
||||
#include "map.h"
|
||||
|
||||
#ifdef DEBUG
|
||||
ops_t o;
|
||||
Action_POS_t pos_get;
|
||||
CMD_ACTION_t ops_out;//经过导航算法解算后输出的期望控制值
|
||||
|
||||
#else
|
||||
|
||||
static ops_t o;
|
||||
static Action_POS_t pos_get;
|
||||
static CMD_ACTION_t ops_out;
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
void Task_Navi(void *argument){
|
||||
|
||||
(void)argument;
|
||||
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() /TASK_FREQ_NAVI;
|
||||
|
||||
Action_init(&o,&(task_runtime.config.chassis_config->ops),&pos_get);
|
||||
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 获取当前控制任务运行频率的tick*/
|
||||
|
||||
while (1)
|
||||
{
|
||||
#ifdef DEBUG
|
||||
/* 记录任务使用的的栈空间 */
|
||||
task_runtime.stack_water_mark.action =
|
||||
osThreadGetStackSpace(osThreadGetId());
|
||||
#endif
|
||||
osMessageQueueGet(task_runtime.msgq.imu.gyro,&o.ops_gyro,NULL,0);
|
||||
|
||||
osMessageQueueGet(task_runtime.msgq.imu.eulr,&o.ops_imu_pos,NULL,0);
|
||||
|
||||
osMessageQueueGet(task_runtime.msgq.cmd .raw .Action ,&pos_get,NULL,0);
|
||||
|
||||
go_path(&o,&ops_out);
|
||||
|
||||
//将解算后的导航期望运动值放入消息队列供其他任务使用
|
||||
osMessageQueueReset(task_runtime.msgq.cmd.raw.ops_9_out);
|
||||
osMessageQueuePut(task_runtime.msgq.cmd.raw.ops_9_out,(&ops_out),0,0);
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻*/
|
||||
osDelayUntil(tick);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
|
||||
|
||||
|
||||
@ -1 +0,0 @@
|
||||
#include "navi.h"
|
||||
@ -6,6 +6,8 @@
|
||||
NUC_t nuc_raw;
|
||||
CMD_NUC_t cmd_fromnuc;
|
||||
#else
|
||||
static NUC_t nuc_raw;
|
||||
static CMD_NUC_t cmd_fromnuc;
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@ -19,6 +19,7 @@ CAN_Output_t UP_CAN_out;
|
||||
|
||||
UP_t UP ;
|
||||
|
||||
CMD_t up_cmd;
|
||||
|
||||
|
||||
#else
|
||||
@ -27,6 +28,7 @@ static CAN_Output_t up_can_out;
|
||||
|
||||
static UP_t UP;
|
||||
|
||||
static CMD_t up_cmd;
|
||||
|
||||
#endif
|
||||
float aaa=0;
|
||||
@ -53,20 +55,20 @@ void Task_up(void *argument)
|
||||
#ifdef DEBUG
|
||||
task_runtime.stack_water_mark.up = osThreadGetStackSpace(osThreadGetId());
|
||||
#endif
|
||||
UP_UpdateFeedback(&UP, &can) ;
|
||||
UP_UpdateFeedback(&UP, &can,&up_cmd) ;
|
||||
// GM6020_control(&UP, 100) ;
|
||||
UP_angle(&UP, bbb);
|
||||
// UP_M3508_speed(&UP, 500);
|
||||
|
||||
|
||||
|
||||
// UP_angle_control(&UP,0,M2006);
|
||||
//
|
||||
//
|
||||
// VESC_M5065_Control(&UP, 20000);
|
||||
|
||||
|
||||
|
||||
GO_SendData(&UP, 1,CCC);
|
||||
GO_SendData(&UP, 0,aaa);
|
||||
|
||||
//
|
||||
// GO_SendData(&UP, 1,CCC);
|
||||
// GO_SendData(&UP, 0,aaa);
|
||||
UP_control(&UP,&UP_CAN_out,&up_cmd);
|
||||
ALL_Motor_Control(&UP,&UP_CAN_out);
|
||||
|
||||
osDelay(1);
|
||||
@ -81,6 +83,7 @@ void Task_up(void *argument)
|
||||
/*can上设备数据获取*/
|
||||
osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
|
||||
|
||||
osMessageQueueGet(task_runtime.msgq.cmd .up_ctrl , &up_cmd, NULL, 0);
|
||||
|
||||
/*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/
|
||||
osKernelLock();
|
||||
|
||||
@ -42,16 +42,12 @@ typedef struct {
|
||||
/* 任务 */
|
||||
struct {
|
||||
osThreadId_t atti_esti;
|
||||
// osThreadId_t chassis;
|
||||
osThreadId_t dr16;
|
||||
// osThreadId_t action_ops;
|
||||
osThreadId_t up;
|
||||
osThreadId_t nuc;
|
||||
|
||||
// osThreadId_t ai;
|
||||
osThreadId_t can;
|
||||
osThreadId_t cmd;
|
||||
osThreadId_t nuc;
|
||||
// osThreadId_t navi;
|
||||
osThreadId_t error_detect;
|
||||
} thread;
|
||||
|
||||
@ -67,13 +63,11 @@ typedef struct {
|
||||
struct {
|
||||
osMessageQueueId_t rc;
|
||||
osMessageQueueId_t nuc;
|
||||
osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
|
||||
osMessageQueueId_t Action;
|
||||
osMessageQueueId_t ops_9_out;
|
||||
|
||||
osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
|
||||
|
||||
}raw;
|
||||
osMessageQueueId_t chassis;
|
||||
osMessageQueueId_t up_ctrl;
|
||||
osMessageQueueId_t status;
|
||||
} cmd;
|
||||
/* can任务放入、读取，电机或电容的输入输出 */
|
||||
@ -108,40 +102,29 @@ typedef struct {
|
||||
UBaseType_t can;
|
||||
UBaseType_t atti_esti;
|
||||
UBaseType_t dr16;
|
||||
UBaseType_t action;
|
||||
UBaseType_t cmd;
|
||||
UBaseType_t nuc;
|
||||
UBaseType_t navi;
|
||||
UBaseType_t error_detect;
|
||||
UBaseType_t nuc;
|
||||
|
||||
UBaseType_t up;
|
||||
|
||||
} stack_water_mark; /* stack使用 */
|
||||
|
||||
struct {
|
||||
// float chassis;
|
||||
float can;
|
||||
float atti_esti;
|
||||
float r12ds;
|
||||
float cmd;
|
||||
float nuc;
|
||||
// float action;
|
||||
// float navi;
|
||||
float error_detect;
|
||||
|
||||
float up;
|
||||
} freq; /* 任务运行频率 */
|
||||
|
||||
struct {
|
||||
// float chassis;
|
||||
float can;
|
||||
float atti_esti;
|
||||
// float ai;
|
||||
float r12ds;
|
||||
float cmd;
|
||||
float nuc;
|
||||
// float navi;
|
||||
// float action;
|
||||
float error_detect;
|
||||
float up;
|
||||
|
||||
@ -156,21 +139,15 @@ extern const osThreadAttr_t attr_init;
|
||||
|
||||
extern const osThreadAttr_t attr_atti_esti;
|
||||
|
||||
//extern const osThreadAttr_t attr_chassis;
|
||||
|
||||
extern const osThreadAttr_t attr_can;
|
||||
|
||||
extern const osThreadAttr_t attr_cmd;
|
||||
|
||||
//extern const osThreadAttr_t attr_ops_9pos;
|
||||
|
||||
extern const osThreadAttr_t attr_nuc;
|
||||
|
||||
extern const osThreadAttr_t attr_error_detect;
|
||||
|
||||
extern const osThreadAttr_t attr_dr16;
|
||||
|
||||
//extern const osThreadAttr_t attr_navi;
|
||||
extern const osThreadAttr_t attr_nuc;
|
||||
|
||||
extern const osThreadAttr_t attr_up;
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
@ -178,20 +155,14 @@ void Task_Init(void *argument);
|
||||
|
||||
void Task_AttiEsti(void *argument);
|
||||
|
||||
//void Task_Chassis(void *argument);
|
||||
|
||||
void Task_can(void *argument);
|
||||
|
||||
void Task_cmd(void *argument);
|
||||
|
||||
void Task_nuc(void *argument);
|
||||
|
||||
//void Task_action(void *argument);
|
||||
|
||||
//void Task_Navi(void *argument);
|
||||
|
||||
void Task_up(void *argument);
|
||||
|
||||
void Task_nuc(void *argument);
|
||||
|
||||
void Task_error_detect(void *argument);
|
||||
|
||||
void Task_dr16(void *argument);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user