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

修改整体，无报错

Browse Source
This commit is contained in:
ZHAISHUI04 2025-04-22 22:49:28 +08:00
parent b1c7ca2858
commit e2bc674c7d
32 changed files with 1432 additions and 875 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

177
MDK-ARM/R2.uvoptx
View File

@ -117,6 +117,26 @@
<pMon>BIN\CMSIS_AGDI.dll</pMon>
</DebugOpt>
<TargetDriverDllRegistry>
<SetRegEntry>
<Number>0</Number>
<Key>ARMRTXEVENTFLAGS</Key>
<Name>-L70 -Z18 -C0 -M0 -T1</Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
<Key>DLGTARM</Key>
<Name>(1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0)</Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
<Key>ARMDBGFLAGS</Key>
<Name></Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
<Key>DLGUARM</Key>
<Name></Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
<Key>CMSIS_AGDI</Key>
@ -129,18 +149,35 @@
</SetRegEntry>
</TargetDriverDllRegistry>
<Breakpoint/>
<WatchWindow1>
<Ww>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>chassis</ItemText>
</Ww>
<Ww>
<count>1</count>
<WinNumber>1</WinNumber>
<ItemText>ctrl</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>dr16,0x0A</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>
</Tracepoint>
<DebugFlag>
<trace>0</trace>
<periodic>0</periodic>
<aLwin>0</aLwin>
<periodic>1</periodic>
<aLwin>1</aLwin>
<aCover>0</aCover>
<aSer1>0</aSer1>
<aSer2>0</aSer2>
<aPa>0</aPa>
<viewmode>0</viewmode>
<viewmode>1</viewmode>
<vrSel>0</vrSel>
<aSym>0</aSym>
<aTbox>0</aTbox>
@ -775,7 +812,7 @@
<Group>
<GroupName>User/Module</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -803,17 +840,29 @@
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>6</GroupNumber>
<FileNumber>49</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\Module\up.c</PathWithFileName>
<FilenameWithoutPath>up.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
<GroupName>User/bsp</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>49</FileNumber>
<FileNumber>50</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -825,7 +874,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>50</FileNumber>
<FileNumber>51</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -837,7 +886,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>51</FileNumber>
<FileNumber>52</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -849,7 +898,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>52</FileNumber>
<FileNumber>53</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -861,7 +910,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>53</FileNumber>
<FileNumber>54</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -873,7 +922,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>54</FileNumber>
<FileNumber>55</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -885,7 +934,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>55</FileNumber>
<FileNumber>56</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -897,7 +946,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>56</FileNumber>
<FileNumber>57</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -909,7 +958,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>57</FileNumber>
<FileNumber>58</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -921,7 +970,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>58</FileNumber>
<FileNumber>59</FileNumber>
<FileType>5</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -933,7 +982,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>59</FileNumber>
<FileNumber>60</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -945,7 +994,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>60</FileNumber>
<FileNumber>61</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -957,7 +1006,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>61</FileNumber>
<FileNumber>62</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -971,13 +1020,13 @@
<Group>
<GroupName>User/Task</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>62</FileNumber>
<FileNumber>63</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -989,7 +1038,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>63</FileNumber>
<FileNumber>64</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1001,7 +1050,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>64</FileNumber>
<FileNumber>65</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1013,7 +1062,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>65</FileNumber>
<FileNumber>66</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1025,7 +1074,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>66</FileNumber>
<FileNumber>67</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1037,7 +1086,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>67</FileNumber>
<FileNumber>68</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1049,7 +1098,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>68</FileNumber>
<FileNumber>69</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1061,7 +1110,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>69</FileNumber>
<FileNumber>70</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1073,7 +1122,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>70</FileNumber>
<FileNumber>71</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1085,7 +1134,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>71</FileNumber>
<FileNumber>72</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1095,17 +1144,29 @@
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>73</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\task\up_task.c</PathWithFileName>
<FilenameWithoutPath>up_task.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
<GroupName>User/Algorithm</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>72</FileNumber>
<FileNumber>74</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1117,7 +1178,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>73</FileNumber>
<FileNumber>75</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1129,7 +1190,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>74</FileNumber>
<FileNumber>76</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1141,7 +1202,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>75</FileNumber>
<FileNumber>77</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1153,7 +1214,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>76</FileNumber>
<FileNumber>78</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1165,7 +1226,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>77</FileNumber>
<FileNumber>79</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1179,13 +1240,13 @@
<Group>
<GroupName>User/Device</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>78</FileNumber>
<FileNumber>80</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1197,7 +1258,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>79</FileNumber>
<FileNumber>81</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1209,7 +1270,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>80</FileNumber>
<FileNumber>82</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1221,7 +1282,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>81</FileNumber>
<FileNumber>83</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1233,7 +1294,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>82</FileNumber>
<FileNumber>84</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1245,7 +1306,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>83</FileNumber>
<FileNumber>85</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1257,7 +1318,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>84</FileNumber>
<FileNumber>86</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1267,6 +1328,18 @@
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>87</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\device\GO_M8010_6_Driver.c</PathWithFileName>
<FilenameWithoutPath>GO_M8010_6_Driver.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
@ -1277,7 +1350,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>85</FileNumber>
<FileNumber>88</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1289,7 +1362,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>86</FileNumber>
<FileNumber>89</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1301,7 +1374,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>87</FileNumber>
<FileNumber>90</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1321,7 +1394,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>12</GroupNumber>
<FileNumber>88</FileNumber>
<FileNumber>91</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1341,7 +1414,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>89</FileNumber>
<FileNumber>92</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1353,7 +1426,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>90</FileNumber>
<FileNumber>93</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1365,7 +1438,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>91</FileNumber>
<FileNumber>94</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1377,7 +1450,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>92</FileNumber>
<FileNumber>95</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>

15
MDK-ARM/R2.uvprojx
View File

@ -1108,6 +1108,11 @@
<FileType>1</FileType>
<FilePath>..\User\Module\config.c</FilePath>
</File>
<File>
<FileName>up.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\Module\up.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -1233,6 +1238,11 @@
<FileType>1</FileType>
<FilePath>..\User\task\r12ds_task.c</FilePath>
</File>
<File>
<FileName>up_task.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\up_task.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -1308,6 +1318,11 @@
<FileType>1</FileType>
<FilePath>..\User\device\r12ds.c</FilePath>
</File>
<File>
<FileName>GO_M8010_6_Driver.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\GO_M8010_6_Driver.c</FilePath>
</File>
</Files>
</Group>
<Group>

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

Binary file not shown.

35
User/Module/Chassis.c
View File

@ -21,10 +21,10 @@
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; /*模式未改变直接返回*/
if (ctrl->CMD_CtrlType== c->ctrl && ctrl->CMD_mode == c->mode) return CHASSIS_OK; /*模式未改变直接返回*/
//此处源代码处做了pid的reset 待添加
c->ctrl =ctrl->C_cmd.type;
c->mode =ctrl->C_cmd.mode;
c->ctrl =ctrl->CMD_CtrlType ;
c->mode =ctrl->CMD_mode ;
return CHASSIS_OK;
} //设置控制模式
@ -42,13 +42,13 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
}
c->motorfeedback.rotor_pit6020angle = can->motor.pit6020.as_array[0].rotor_angle;
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_angle;
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_angle;
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];
@ -126,7 +126,7 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
switch (c->ctrl)
{
case RC: //手动控制
case RCcontrol: //手动控制
/*
在cmd里对数据进行处理 包括方向和油门
@ -137,11 +137,6 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
break;
case UP_CONTROL://上层在控制时，底盘速度为0
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
break;
case AUTO : //在自动模式下
@ -150,9 +145,9 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
case AUTO_NAVI: //自动雷达
// //这套是全向轮的方向,一定要注意这里的xy方向
c->move_vec.Vw =ctrl->C_navi.wz*1000 ;
c->move_vec.Vy =-ctrl->C_navi.vy*1000 ;
c->move_vec.Vx =-ctrl->C_navi.vx*1000 ;
c->move_vec.Vw =ctrl->cmd_MID360 .posw *1000 ;
c->move_vec.Vy =-ctrl->cmd_MID360.posy *1000 ;
c->move_vec.Vx =-ctrl->cmd_MID360.posx *1000 ;
// 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);
@ -182,16 +177,16 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
c->move_vec.Vx =ctrl->Vx*6000 ;
c->move_vec.Vy =ctrl->Vy *6000;
c->move_vec .Vw = -ctrl->C_pick .posx;
c->move_vec .Vw = -ctrl->cmd_pick .posx;
if(abs_value(ctrl ->C_pick .posx )>20)
if(abs_value(ctrl ->cmd_pick .posx )>20)
{
c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_HIGN),(c->move_vec .Vw) ,0);
}
else if(abs_value(ctrl ->C_pick .posx )<0.1)
else if(abs_value(ctrl ->cmd_pick .posx )<0.1)
{
c->move_vec.Vw =0;
}
@ -202,7 +197,7 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
c->vofa_send[0]=c->move_vec.Vw;
c->vofa_send[1]=-ctrl->C_pick .posx;
c->vofa_send[1]=-ctrl->cmd_pick .posx;
@ -229,7 +224,7 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
{
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];
out->motor_CHASSIS3508 .as_array[i] = c->final_out.final_3508out[i];
}

4
User/Module/Chassis.h
View File

@ -118,8 +118,8 @@ typedef struct{
const Chassis_Param_t *param; //一些固定的参数
ChassisImu_t pos088; //088的实时姿态
CMD_Chassis_CtrlType_e ctrl;
CMD_Chassis_mode_e mode;
CMD_CtrlType_t ctrl;
CMD_mode_t mode;
ChassisMove_Vec move_vec; //由控制任务决定

169
User/Module/config.c
View File

@ -8,19 +8,84 @@
#define DEG_TO_RAD(x) ((x) * (3.141592653 / 180.0)) //角度转弧度
#ifdef DEBUG
ConfigParam_t param_chassis ={
ConfigParam_t param ={
#else
static const ConfigParam_t param_chassis ={
static const ConfigParam_t param ={
#endif
.up={
.chassis = {/**/
/*上层pid参数*/
.Shoot_M2006_angle_param = {
.p = 25.0f,
.i = 0.0f,
.d = 1.5f,
.i_limit = 1000.0f,
.out_limit = 3000.0f,
},
.Shoot_M2006_speed_param = {
.p = 5.0f,
.i = 0.3f,
.d = 0.0f,
.i_limit = 2000.0f,
.out_limit = 3000.0f,
},
.Pitch_M2006_angle_param = {
.p = 25.0f,
.i = 0.0f,
.d = 1.5f,
.i_limit = 1000.0f,
.out_limit = 3000.0f,
},
.Pitch_M2006_speed_param={
.p = 5.0f,
.i = 0.3f,
.d = 0.0f,
.i_limit = 2000.0f,
.out_limit = 3000.0f,
},
.go_cmd={
.id =0,
.mode = 1,
.K_P = 1.0f,
.K_W = 0.05,
.Pos = 0,
.W = 0,
.T = 0,
},
/*上层其他参数*/
/*运球*/
.DribbleConfig_Config = {
.m3508_init_angle = 50,
.m3508_high_angle = 1200,
.go2_init_angle = 0,
.go2_flip_angle = -250,
.flip_timing = 200,
.go2_release_threshold = -550.0f,
},
/*投球*/
.PitchConfig_Config = {
.m2006_init_angle =-150,
.m2006_trigger_angle =0,
.go1_init_position = 0,
.go1_release_threshold =0,
.m2006_Screw_init=0
},
},
.chassis = {/**/
.M3508_param = {
@ -47,93 +112,31 @@ static const ConfigParam_t param_chassis ={
},
// .M3508_param = {
// .p = 10.0f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit =10000.0f,
// },
////高速那一套
.NaviVx_param ={
.p = 1.15f,
.i = 0.0f,
.d = 0.15f,
.i_limit = 0.0f,
.out_limit =5000.0f,
},
.NaviVy_param ={
.p = 1.15f,
.i = 0.0f,
.d = 0.15f,
.i_limit = 0.0f,
.out_limit =5000.0f,
},
.NaviVw_param ={
.p = 1.15f,
.i = 0.0f,
.d = 0.15f,
.i_limit = 0.0f,
.out_limit =5000.0f,
},
//
//低速那一套
// .NaviVx_param ={
// .p = 1.04f,
// .i = 0.0f,
// .d = 0.15f,
// .i_limit = 0.0f,
// .out_limit =5000.0f,
// },
// .NaviVy_param ={
// .p = 1.1f,
// .i = 0.0f,
// .d = 0.15f,
// .i_limit = 0.0f,
// .out_limit =5000.0f,
// },
// .NaviVw_param ={
// .p = 1.5f,
// .i = 0.0f,
// .d = 0.15f,
// .i_limit = 0.0f,
// .out_limit =5000.0f,
// },
.Sick_CaliYparam ={
.p =2.5f,
.i =0.001f,
.d =0.1f,
.i_limit =15.0f,
.out_limit =500.0f,
},
.Sick_CaliXparam ={
.p =2.5f,
.i =0.001f,
.d =0.1f,
.i_limit =15.0f,
.out_limit =500.0f,
}
},
.can = {
.pitch6020 = BSP_CAN_1,
.motor3508 = BSP_CAN_1,
.chassis6020 = BSP_CAN_2,
.motor_CHASSIS3508 = BSP_CAN_1,
.motor_UP3508 = BSP_CAN_2,
// .chassis6020 = BSP_CAN_1,//禁用
.chassis5065 = BSP_CAN_1,
.sick = BSP_CAN_2,
},
};
const ConfigParam_t *Config_ChassisGet(void)
{
return &param_chassis;
return &param;
}
///*获取导航地图*/
//void set_ops_path(ConfigParam_t *config, const point_t *path, int8_t path_num) {
// config->ops.path = path;
// config->ops.path_num = path_num;
//}
/**
* \brief 从Flash读取配置信息
*
@ -142,11 +145,11 @@ const ConfigParam_t *Config_ChassisGet(void)
void Config_Get(Config_t *cfg) {
BSP_Flash_ReadBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg));
// /* 防止第一次烧写后访问NULL指针 */
cfg->chassis_config = &param_chassis;
if (cfg->chassis_config == NULL) cfg->chassis_config = &param_chassis;
cfg->config = &param;
if (cfg->config == NULL) cfg->config = &param;
/* 防止擦除后全为1 */
if ((uint32_t)(cfg->chassis_config) == UINT32_MAX)
cfg->chassis_config = &param_chassis;
if ((uint32_t)(cfg->config) == UINT32_MAX)
cfg->config = &param;
if (memcmp(&cfg->cali_088.gyro_offset.x, "\xFF\xFF\xFF\xFF", 4) == 0) {
cfg->cali_088.gyro_offset.x = 0.0f;

10
User/Module/config.h
View File

@ -1,14 +1,16 @@
#ifndef _CONFIG_H
#define _CONFIG_H
#include "Chassis.h"
#include "can_use.h"
#include "ahrs.h"
#include "map.h"
#include "up.h"
#include "chassis.h"
typedef struct{
Chassis_Param_t chassis; /**/
UP_Param_t up;
Chassis_Param_t chassis;
CAN_Params_t can;
AHRS_Eulr_t mech_zero[4];
@ -16,7 +18,7 @@ AHRS_Eulr_t mech_zero[4];
typedef struct{
const ConfigParam_t *chassis_config;
const ConfigParam_t *config;
BMI088_Cali_t cali_088;

253
User/Module/up.c Normal file
View File

@ -0,0 +1,253 @@
#include "up.h"
#include "gpio.h"
#include "user_math.h"
#include "bsp_buzzer.h"
#include "bsp_delay.h"
#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)) //2006编码值转轴角度
#define MOTOR3508_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_3508)) //3508编码值转轴角度
// 定义继电器控制
#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
#define RELAY2_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
{
u->param = param; /*初始化参数 */
/*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 .Shoot_M2006angle ,PID_POSITION ,&(u->param->Shoot_M2006_angle_param ));
PID_init (&u->pid .Shoot_M2006speed ,PID_POSITION ,&(u->param->Shoot_M2006_speed_param ));
PID_init (&u->pid .Pitch_M2006_angle ,PID_POSITION ,&(u->param->Pitch_M2006_angle_param ));
PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param ));
u->go_cmd =u->param ->go_cmd ;
// 初始化上层状态机
if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球
u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
u->DribbleContext .DribbleState = Dribble_PREPARE;
u->DribbleContext .is_initialized = 1;
}
if (!u->PitchContext .is_initialized) {
u->PitchContext .PitchConfig = u->param ->PitchConfig_Config ;//赋值
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
u->PitchContext .is_initialized = 1;
}
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
}
/*can，上层状态更新*/
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
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 ;
for(int i=0;i<4;i++){
u->motorfeedback .DJmotor_feedback[i].rpm =can ->motor .motor3508 .as_array [i].rotor_speed ;
u->motorfeedback .DJmotor_feedback[i].ecd =can ->motor .motor3508 .as_array [i].rotor_ecd ;
DJ_processdata(&u->motorfeedback .DJmotor_feedback [i], MOTOR2006_ECD_TO_ANGLE);
}
u->cmd =c;
return 0;
}
int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle)
{
int8_t cnt=0;
fp32 angle ,delta;
angle = f->ecd;
if (f->init_cnt < 50) {
f->orig_angle= angle;
f->last_angle = angle;
f->init_cnt++;
return 0;
}
delta = angle - f->last_angle;
if (delta > 4096) {
f->round_cnt--;
} else if (delta < -4096) {
f->round_cnt++;
}
f->last_angle = angle;
f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle;
}
/*
这里id范围为1-4，
*/
int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle)
{
fp32 delta_angle,delta_speed;
delta_angle = PID_calc(Angle_pid , f->total_angle , target_angle);
delta_speed = PID_calc(Speed_pid , f->rpm , delta_angle);
u->final_out .DJfinal_out [id-1]=delta_speed;
return 0;
}
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;
return 0;
}
/*go电机控制*/
int8_t GO_SendData(UP_t *u, float pos, float limit)
{
static int is_calibration=0;
static fp32 error=0; //误差量
GO_MotorData_t *GO_calibration;//校准前，原始数据
GO_calibration = get_GO_measure_point() ;
if(is_calibration==0)
{
is_calibration=HAL_GPIO_ReadPin (GPIOE ,GPIO_PIN_9 );
u->go_cmd.Pos = -50; //上电之后跑
error= GO_calibration->Pos ;
}
u->motorfeedback .go_data = GO_calibration;
u->motorfeedback .go_data ->Pos= error ;
u->go_cmd.Pos = pos;
// 读取参数
float tff = u->go_cmd.T; // 前馈力矩
float kp = u->go_cmd.K_P; // 位置刚度
float kd = u->go_cmd.K_W; // 速度阻尼
float q_desired = u->go_cmd.Pos; // 期望位置（rad）
float q_current = u->motorfeedback.go_data->Pos; // 当前角度位置（rad）
float dq_desired = u->go_cmd.W; // 期望角速度（rad/s）
float dq_current = u->motorfeedback.go_data->W; // 当前角速度（rad/s）
// 计算输出力矩 tau
float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);
/*限制最大输入来限制最大输出*/
if (pos - q_current > limit) {
u->go_cmd.Pos = q_current + limit; // 限制位置
}else if (pos - q_current < -limit) {
u->go_cmd.Pos = q_current - limit; // 限制位置
}else {
u->go_cmd.Pos = pos; // 允许位置
}
// 发送数据
GO_M8010_send_data(&u->go_cmd);
return 0;
}
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
{
//电机控制 ，传进can
DJ_Angle_Control(u,1,&u->motorfeedback .DJmotor_feedback[0] ,
&u->pid .Shoot_M2006angle ,
&u->pid .Shoot_M2006speed ,
u->motor_target .Shoot_M2006_angle );
DJ_Angle_Control(u,2,&u->motorfeedback .DJmotor_feedback[1] ,
&u->pid .Pitch_M2006_angle ,
&u->pid .Pitch_M2006_speed ,
u->motor_target .Pitch_M2006_angle );
GO_SendData(u,u->motor_target .go_shoot,1 );
for(int i=0;i<4;i++){
out ->motor_UP3508.as_array[i]=u->final_out.DJfinal_out [i] ;
}
out ->chassis5065 .erpm [0]= u->final_out .final_VESC_5065_M1out ;
out ->chassis5065 .erpm [1]= -u->final_out .final_VESC_5065_M2out ;
return 0;
}
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
{
if(u ==NULL) return 0;
static int is_pitch=1;
switch (c->CMD_CtrlType )
{
case RCcontrol: //在手动模式下
switch (c-> CMD_mode )
{
case Normal :
/*投篮*/
if(is_pitch){
u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ;
is_pitch=0;
}
//
break;
case Pitch :
if (u->PitchContext .PitchState ==PITCH_PREPARE) //首次启动
{
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
}
break ;
return 0;
}
}
}

233
User/Module/up.h Normal file
View File

@ -0,0 +1,233 @@
#ifndef UP_H
#define UP_H
#include "struct_typedef.h"
#include "pid.h"
#include "bmi088.h"
#include "user_math.h"
#include "ahrs.h"
#include "can_use.h"
#include "cmd.h"
#include "filter.h"
#include "vofa.h"
#include "GO_M8010_6_Driver.h"
#include "bsp_usart.h"
/*
状态机
状态结构体，触发标志位结构体
配置层，-->config.c
|
中间层，-->up.h,统一UP_t
|
运行函数，switch(状态) 运行相应函数
*/
/* 投球状态定义 */
typedef enum {
PITCH_PREPARE, // 准备阶段
PITCH_START, //启动，拉扳机
PITCH_PULL_TRIGGER,
PITCH_LAUNCHING, // 发射中
PITCH_COMPLETE // 完成
} PitchState_t;
/* 投球参数配置 */
typedef struct {
fp32 m2006_init_angle; // M2006初始角度-140
fp32 m2006_trigger_angle; // M2006触发角度0,用来合并扳机
fp32 go1_init_position; // GO电机触发位置,0,初始位置
fp32 go1_release_threshold; // go上升去合并扳机，扳机位置
fp32 m2006_Screw_init;
} PitchConfig_t;
/* 投球控制上下文 */
typedef struct {
PitchState_t PitchState;
PitchConfig_t PitchConfig;
uint8_t is_initialized ;
} PitchContext_t;
/*运球*/
typedef enum {
Dribble_PREPARE,
STATE_GRAB_BALL, // 夹球升起阶段
STATE_RELEASE_BALL, // 释放球体
STATE_CATCH_PREP, // 接球准备
STATE_CATCH_BALL, // 接球动作
STATE_TRANSFER, //转移球
STATE_CATCH_DONE //完成
} DribbleState_t;
/* 参数配置结构体 */
typedef struct {
fp32 m3508_init_angle; // 3508初始角度
fp32 m3508_high_angle; // 3508升起角度
fp32 go2_init_angle; // GO2初始角度
fp32 go2_flip_angle; // GO2翻转角度
fp32 flip_timing; // 翻转触发时机
fp32 go2_release_threshold; // 释放球
} DribbleConfig_t;
/* 状态机上下文 */
typedef struct {
DribbleState_t DribbleState;
DribbleConfig_t DribbleConfig;
uint8_t is_initialized;
} DribbleContext_t;
typedef struct {
BMI088_t bmi088;
AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
}UP_Imu_t;
typedef struct
{
pid_param_t VESC_5065_M1_param;
pid_param_t VESC_5065_M2_param;
pid_param_t Shoot_M2006_speed_param;
pid_param_t Shoot_M2006_angle_param;
pid_param_t Pitch_M2006_speed_param;
pid_param_t Pitch_M2006_angle_param;
DribbleConfig_t DribbleConfig_Config;
PitchConfig_t PitchConfig_Config;
GO_MotorCmd_t go_cmd;
}UP_Param_t;
typedef struct
{
fp32 ecd;
fp32 rpm;
fp32 orig_angle;
fp32 last_angle;
int32_t round_cnt;
uint16_t init_cnt;
fp32 total_angle;
}DJmotor_feedback_t;
typedef struct{
uint8_t up_task_run;
const UP_Param_t *param;
/*运球过程*/
DribbleContext_t DribbleContext;
/*投篮过程*/
PitchContext_t PitchContext;
UP_Imu_t pos088;
CMD_t *cmd;
struct{
fp32 VESC_5065_M1_rpm;
fp32 VESC_5065_M2_rpm;
GO_MotorData_t *go_data;//存放go电机数据
DJmotor_feedback_t DJmotor_feedback[4];
}motorfeedback;
struct{
fp32 VESC_5065_M1_rpm;
fp32 VESC_5065_M2_rpm;
fp32 Shoot_M2006_angle;
fp32 Pitch_M2006_angle;
fp32 go_shoot;
}motor_target;
struct{
pid_type_def VESC_5065_M1;
pid_type_def VESC_5065_M2;
pid_type_def Shoot_M2006angle;
pid_type_def Shoot_M2006speed;
pid_type_def Pitch_M2006_angle;
pid_type_def Pitch_M2006_speed;
}pid;
GO_MotorCmd_t go_cmd;
/*经PID计算后的实际发送给电机的实时输出值*/
struct
{
fp32 DJfinal_out[4];
fp32 final_VESC_5065_M1out;
fp32 final_VESC_5065_M2out;
}final_out;
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
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, CMD_t *c) ;
int8_t VESC_M5065_Control(UP_t *u,fp32 speed);
int8_t GO_SendData(UP_t *u,float pos,float limit);
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 DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle);
int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle);
#endif

11
User/bsp/bsp_usart.c
View File

@ -9,9 +9,9 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
if (huart->Instance == USART3)
return BSP_UART_REMOTE;
else if (huart->Instance == USART1)
return BSP_UART_VOFA;
else if (huart->Instance == USART6)
return BSP_UART_NUC;
else if (huart->Instance == USART6)
return BSP_UART_RS485;
/*
else if (huart->Instance == USARTX)
return BSP_UART_XXX;
@ -94,10 +94,10 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
switch (uart) {
case BSP_UART_REMOTE:
return &huart3;
case BSP_UART_VOFA:
return &huart1;
case BSP_UART_NUC:
case BSP_UART_RS485:
return &huart6;
case BSP_UART_NUC:
return &huart1;
/*
case BSP_UART_XXX:
return &huartX;
@ -108,7 +108,6 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
}
int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type,
void (*callback)(void)) {
if (callback == NULL) return BSP_ERR_NULL;

2
User/bsp/bsp_usart.h
View File

@ -12,7 +12,7 @@
typedef enum {
BSP_UART_REMOTE,
BSP_UART_VOFA,
BSP_UART_RS485,
BSP_UART_NUC,
/* BSP_UART_XXX, */
BSP_UART_NUM,

108
User/device/GO_M8010_6_Driver.c Normal file
View File

@ -0,0 +1,108 @@
/**
*go控制程序
*/
#include "GO_M8010_6_Driver.h"
#include "usart.h"
#include "bsp_usart.h"
#include <cmsis_os2.h>
#include "crc16.h"
#include <string.h>
#define SATURATE(_IN, _MIN, _MAX) \
{ \
if ((_IN) <= (_MIN)) \
(_IN) = (_MIN); \
else if ((_IN) >= (_MAX)) \
(_IN) = (_MAX); \
}
GO_MotorCmd_t cmd_gogogo ;
RIS_ControlData_t motor_send_data;
GO_MotorData_t data = {0}; // 确保 data 定义在此处
uint8_t rx_buffer[sizeof(data.motor_recv_data)] = {0}; // 确保 rx_buffer 定义在此处
/// @brief 将发送给电机的浮点参数转换为定点类型参数
/// @param motor_s 要转换的电机指令结构体
void modify_data(GO_MotorCmd_t *motor_s)
{
motor_send_data.head[0] = 0xFE;
motor_send_data.head[1] = 0xEE;
SATURATE(motor_s->id, 0, 15);
SATURATE(motor_s->mode, 0, 7);
SATURATE(motor_s->K_P, 0.0f, 25.599f);
SATURATE(motor_s->K_W, 0.0f, 25.599f);
SATURATE(motor_s->T, -127.99f, 127.99f);
SATURATE(motor_s->W, -804.00f, 804.00f);
SATURATE(motor_s->Pos, -411774.0f, 411774.0f);
motor_send_data.mode.id = motor_s->id;
motor_send_data.mode.status = motor_s->mode;
motor_send_data.comd.k_pos = motor_s->K_P / 25.6f * 32768.0f;
motor_send_data.comd.k_spd = motor_s->K_W / 25.6f * 32768.0f;
motor_send_data.comd.pos_des = motor_s->Pos / 6.28318f * 32768.0f;
motor_send_data.comd.spd_des = motor_s->W / 6.28318f * 256.0f;
motor_send_data.comd.tor_des = motor_s->T * 256.0f;
motor_send_data.CRC16 = CRC16_Calc( (uint8_t *)&motor_send_data, sizeof(RIS_ControlData_t) - sizeof(motor_send_data.CRC16),0);
}
/// @brief 将接收到的定点类型原始数据转换为浮点参数类型
/// @param motor_r 要转换的电机反馈结构体
void extract_data(GO_MotorData_t *motor_r)
{
if (motor_r->motor_recv_data.head[0] != 0xFD || motor_r->motor_recv_data.head[1] != 0xEE)
{
motor_r->correct = 0;
return;
}
motor_r->calc_crc = CRC16_Calc((uint8_t *)&motor_r->motor_recv_data, sizeof(RIS_MotorData_t) - sizeof(motor_r->motor_recv_data.CRC16),0);
if (motor_r->motor_recv_data.CRC16 != motor_r->calc_crc)
{
memset(&motor_r->motor_recv_data, 0, sizeof(RIS_MotorData_t));
motor_r->correct = 0;
motor_r->bad_msg++;
return;
}
else
{
motor_r->motor_id = motor_r->motor_recv_data.mode.id;
motor_r->mode = motor_r->motor_recv_data.mode.status;
motor_r->Temp = motor_r->motor_recv_data.fbk.temp;
motor_r->MError = motor_r->motor_recv_data.fbk.MError;
motor_r->W = ((float)motor_r->motor_recv_data.fbk.speed / 256.0f) * 6.28318f;
motor_r->T = ((float)motor_r->motor_recv_data.fbk.torque) / 256.0f;
motor_r->Pos = 6.28318f * ((float)motor_r->motor_recv_data.fbk.pos) / 32768.0f;
motor_r->footForce = motor_r->motor_recv_data.fbk.force;
motor_r->correct = 1;
return;
}
}
void GO_M8010_send_data(GO_MotorCmd_t *cmd)
{
modify_data(cmd);
HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_RS485), (uint8_t *)(&motor_send_data), sizeof(motor_send_data));
osDelay (1);
HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_RS485), rx_buffer, sizeof(rx_buffer));
}
void USART6_RxCompleteCallback(void )
{
UART_HandleTypeDef *huart6 = BSP_UART_GetHandle(BSP_UART_RS485);
uint16_t crc = CRC16_Calc(rx_buffer,sizeof(rx_buffer)-2,0x0000);
memcpy(&data.motor_recv_data, rx_buffer, sizeof(data.motor_recv_data)); // Copy received data
extract_data(&data);
}
const GO_MotorData_t *get_GO_measure_point(void)
{
return &data;
}

116
User/device/GO_M8010_6_Driver.h Normal file
View File

@ -0,0 +1,116 @@
#ifndef __GO_M8010_6_H
#define __GO_M8010_6_H
#include "struct_typedef.h"
#pragma pack(1)
/**
* @brief 电机模式控制信息
*
*/
typedef struct
{
uint8_t id : 4; // 电机ID: 0,1...,13,14 15表示向所有电机广播数据(此时无返回)
uint8_t status : 3; // 工作模式: 0.锁定 1.FOC闭环 2.编码器校准 3.保留
uint8_t reserve : 1; // 保留位
} RIS_Mode_t; // 控制模式 1Byte
/**
* @brief 电机状态控制信息
*
*/
typedef struct
{
int16_t tor_des; // 期望关节输出扭矩 unit: N.m (q8)
int16_t spd_des; // 期望关节输出速度 unit: rad/s (q8)
int32_t pos_des; // 期望关节输出位置 unit: rad (q15)
int16_t k_pos; // 期望关节刚度系数 unit: -1.0-1.0 (q15)
int16_t k_spd; // 期望关节阻尼系数 unit: -1.0-1.0 (q15)
} RIS_Comd_t; // 控制参数 12Byte
/**
* @brief 电机状态反馈信息
*
*/
typedef struct
{
int16_t torque; // 实际关节输出扭矩 unit: N.m (q8)
int16_t speed; // 实际关节输出速度 unit: rad/s (q8)
int32_t pos; // 实际关节输出位置 unit: rad (q15)
int8_t temp; // 电机温度: -128~127°C
uint8_t MError : 3; // 电机错误标识: 0.正常 1.过热 2.过流 3.过压 4.编码器故障 5-7.保留
uint16_t force : 12; // 足端气压传感器数据 12bit (0-4095)
uint8_t none : 1; // 保留位
} RIS_Fbk_t; // 状态数据 11Byte
/**
* @brief 控制数据包格式
*
*/
typedef struct
{
uint8_t head[2]; // 包头 2Byte
RIS_Mode_t mode; // 电机控制模式 1Byte
RIS_Comd_t comd; // 电机期望数据 12Byte
uint16_t CRC16; // CRC 2Byte
} RIS_ControlData_t; // 主机控制命令 17Byte
/**
* @brief 电机反馈数据包格式
*
*/
typedef struct
{
uint8_t head[2]; // 包头 2Byte
RIS_Mode_t mode; // 电机控制模式 1Byte
RIS_Fbk_t fbk; // 电机反馈数据 11Byte
uint16_t CRC16; // CRC 2Byte
} RIS_MotorData_t; // 电机返回数据 16Byte
#pragma pack()
/// @brief 电机指令结构体
typedef struct
{
unsigned short id; // 电机ID，15代表广播数据包
unsigned short mode; // 0:空闲 1:FOC控制 2:电机标定
float T; // 期望关节的输出力矩(电机本身的力矩)(Nm)
float W; // 期望关节速度(电机本身的速度)(rad/s)
float Pos; // 期望关节位置(rad)
float K_P; // 关节刚度系数(0-25.599)
float K_W; // 关节速度系数(0-25.599)
} GO_MotorCmd_t;
/// @brief 电机反馈结构体
typedef struct
{
unsigned char motor_id; // 电机ID
unsigned char mode; // 0:空闲 1:FOC控制 2:电机标定
int Temp; // 温度
int MError; // 错误码
float T; // 当前实际电机输出力矩(电机本身的力矩)(Nm)
float W; // 当前实际电机速度(电机本身的速度)(rad/s)
float Pos; // 当前电机位置(rad)
int correct; // 接收数据是否完整(1完整，0不完整)
int footForce; // 足端力传感器原始数值
uint16_t calc_crc;
uint32_t timeout; // 通讯超时 数量
uint32_t bad_msg; // CRC校验错误 数量
RIS_MotorData_t motor_recv_data; // 电机接收数据结构体
} GO_MotorData_t;
void modify_data(GO_MotorCmd_t *motor_s);
void extract_data(GO_MotorData_t *motor_r);
const GO_MotorData_t *get_GO_measure_point(void);
void GO_M8010_send_data( GO_MotorCmd_t *cmd);
void USART6_RxCompleteCallback(void );
#endif /*__GO_M8010_6_H */

115
User/device/can_use.c
View File

@ -12,10 +12,12 @@
#define CAN_MOTOR_CUR_RES (16384)
#define CAN_GM6020_CTRL_ID_BASE (0x1ff)
//#define CAN_GM6020_CTRL_ID_BASE (0x1ff) //禁用
#define CAN_PITCH6020_CTRL_ID (0x2ff)
#define CAN_G3508_CTRL_ALL_ID (0x200)
#define CAN_G3508_CTRL_ALL_CHASSIS_ID (0x200)
#define CAN_G3508_CTRL_ALL_UP_ID (0x1ff)
#define CAN_VESC_CTRL_ID_BASE (0x300)
@ -41,12 +43,12 @@ static CAN_t *gcan;
static void CAN_DJIMotor_Decode(CAN_MotorFeedback_t *feedback,
const uint8_t *raw) {
if (feedback == NULL || raw == NULL) return;
uint16_t raw_angle = (uint16_t)((raw[0] << 8) | raw[1]);
uint16_t raw_ecd = (uint16_t)((raw[0] << 8) | raw[1]);
int16_t raw_current = (int16_t)((raw[4] << 8) | raw[5]);
feedback->rotor_speed = (int16_t)((raw[2] << 8) | raw[3]);
feedback->temp = raw[6];
feedback->rotor_angle = raw_angle / (float)CAN_MOTOR_ENC_RES * 360.0f;
feedback->rotor_ecd = raw_ecd / (float)CAN_MOTOR_ENC_RES * 360.0f;
feedback->torque_current =
raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES;
}
@ -144,21 +146,49 @@ int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group, CAN_Output_t *output,
CAN_t *can) {
if (output == NULL) return DEVICE_ERR_NULL;
int16_t motor1, motor2, motor3, motor4, motor5 ;
int16_t motor1, motor2, motor3, motor4, motor5, motor6, motor7;
switch (group) {
case CAN_MOTOR_CHASSIS6020:
motor1 =
(int16_t)(output->chassis6020.named.m1);
motor2 =
(int16_t)(output->chassis6020.named.m2);
motor3 =
(int16_t)(output->chassis6020.named.m3);
motor4 =
(int16_t)(output->chassis6020.named.m4);
// case CAN_MOTOR_CHASSIS6020: //(总线0x1ff的6020禁用，怕和3508冲突)
// motor1 =
// (int16_t)(output->chassis6020.named.m1);
// motor2 =
// (int16_t)(output->chassis6020.named.m2);
// motor3 =
// (int16_t)(output->chassis6020.named.m3);
// motor4 =
// (int16_t)(output->chassis6020.named.m4);
raw_tx.tx_header.StdId = CAN_GM6020_CTRL_ID_BASE;
// raw_tx.tx_header.StdId = CAN_GM6020_CTRL_ID_BASE;
// raw_tx.tx_header.IDE = CAN_ID_STD;
// raw_tx.tx_header.RTR = CAN_RTR_DATA;
// raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
// raw_tx.tx_data[0] = (uint8_t)((motor1 >> 8) & 0xFF);
// raw_tx.tx_data[1] = (uint8_t)(motor1 & 0xFF);
// raw_tx.tx_data[2] = (uint8_t)((motor2 >> 8) & 0xFF);
// raw_tx.tx_data[3] = (uint8_t)(motor2 & 0xFF);
// raw_tx.tx_data[4] = (uint8_t)((motor3 >> 8) & 0xFF);
// raw_tx.tx_data[5] = (uint8_t)(motor3 & 0xFF);
// raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
// raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);
// HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->chassis6020),
// &raw_tx.tx_header, raw_tx.tx_data,
// &(can->mailbox.chassis));
// break;
case CAN_MOTOR_CHASSIS3508:
motor1 =
(int16_t)(output->motor_CHASSIS3508.named.m1);
motor2 =
(int16_t)(output->motor_CHASSIS3508.named.m2);
motor3 =
(int16_t)(output->motor_CHASSIS3508.named.m3);
motor4 =
(int16_t)(output->motor_CHASSIS3508.named.m4);
raw_tx.tx_header.StdId =CAN_G3508_CTRL_ALL_CHASSIS_ID;
raw_tx.tx_header.IDE = CAN_ID_STD;
raw_tx.tx_header.RTR = CAN_RTR_DATA;
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
@ -172,21 +202,21 @@ int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group, CAN_Output_t *output,
raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->chassis6020),
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->motor_CHASSIS3508),
&raw_tx.tx_header, raw_tx.tx_data,
&(can->mailbox.chassis));
break;
case CAN_MOTOR_3508:
break;
case CAN_MOTOR_UP3508:
motor1 =
(int16_t)(output->motor3508.named.m1);
(int16_t)(output->motor_UP3508.named.m1);
motor2 =
(int16_t)(output->motor3508.named.m2);
(int16_t)(output->motor_UP3508.named.m2);
motor3 =
(int16_t)(output->motor3508.named.m3);
(int16_t)(output->motor_UP3508.named.m3);
motor4 =
(int16_t)(output->motor3508.named.m4);
(int16_t)(output->motor_UP3508.named.m4);
raw_tx.tx_header.StdId =CAN_G3508_CTRL_ALL_ID;
raw_tx.tx_header.StdId =CAN_G3508_CTRL_ALL_UP_ID;
raw_tx.tx_header.IDE = CAN_ID_STD;
raw_tx.tx_header.RTR = CAN_RTR_DATA;
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
@ -200,13 +230,17 @@ int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group, CAN_Output_t *output,
raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->motor3508),
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->motor_UP3508),
&raw_tx.tx_header, raw_tx.tx_data,
&(can->mailbox.chassis));
&(can->mailbox.up));
break;
case CAN_MOTOR_PITCH6020:
motor5 =
(int16_t)(output->pitch6020.named.m1);
motor6 =
(int16_t)(output->pitch6020.named.m2);
motor7 =
(int16_t)(output->pitch6020.named.m3);
raw_tx.tx_header.StdId = CAN_PITCH6020_CTRL_ID;
raw_tx.tx_header.IDE = CAN_ID_STD;
raw_tx.tx_header.RTR = CAN_RTR_DATA;
@ -214,10 +248,10 @@ int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group, CAN_Output_t *output,
raw_tx.tx_data[0] = (uint8_t)((motor5 >> 8) & 0xFF);
raw_tx.tx_data[1] = (uint8_t)(motor5 & 0xFF);
raw_tx.tx_data[2] = 0;
raw_tx.tx_data[3] = 0;
raw_tx.tx_data[4] = 0;
raw_tx.tx_data[5] = 0;
raw_tx.tx_data[2] = (uint8_t)((motor6 >> 8) & 0xFF);
raw_tx.tx_data[3] = (uint8_t)(motor6 & 0xFF);
raw_tx.tx_data[4] = (uint8_t)((motor7 >> 8) & 0xFF);
raw_tx.tx_data[5] = (uint8_t)(motor7 & 0xFF);
raw_tx.tx_data[6] = 0;
raw_tx.tx_data[7] = 0;
@ -256,7 +290,7 @@ int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_
int Byte[4];
Vesc_ByteGet raw[4];
switch (group) {
case CAN_MOTOR_CHASSIS5065:
case CAN_MOTOR_5065:
//将期望的四个电机输出值分别对应到四个联合体 为了下面的拆分字节
raw[0].as_array = output->chassis5065.named.m1;
raw[1].as_array = output->chassis5065.named.m2;
@ -322,12 +356,12 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
CAN_DJIMotor_Decode(&(can->motor.chassis6020.as_array[index]), can_rx->rx_data);
break;
case CAN_M3508_M1_ID:
case CAN_M3508_M2_ID:
case CAN_M3508_M3_ID:
case CAN_M3508_M4_ID:
case CAN_CHASSIS_M3508_M1_ID:
case CAN_CHASSIS_M3508_M2_ID:
case CAN_CHASSIS_M3508_M3_ID:
case CAN_CHASSIS_M3508_M4_ID:
// 存储消息到对应的电机结构体中
index = can_rx->rx_header.StdId - CAN_M3508_M1_ID;
index = can_rx->rx_header.StdId - CAN_CHASSIS_M3508_M1_ID;
can->recive_flag |= 1 << (index);
CAN_DJIMotor_Decode(&(can->motor.motor3508.as_array[index]), can_rx->rx_data);
detect_hook(CHASSIS3508M1_TOE + index);
@ -348,6 +382,17 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
CAN_DJIMotor_Decode(&(can->motor.pit6020.as_array[index]), can_rx->rx_data);
detect_hook(PITCH6020_TOE + index);
break;
case CAN_UP_M3508_M1_ID:
case CAN_UP_M3508_M2_ID:
case CAN_UP_M3508_M3_ID:
case CAN_UP_M3508_M4_ID:
// 存储消息到对应的电机结构体中
index = can_rx->rx_header.StdId - CAN_UP_M3508_M1_ID;
can->recive_flag |= 1 << (index);
CAN_DJIMotor_Decode(&(can->motor.motor3508.as_array[index]), can_rx->rx_data);
detect_hook(CHASSIS3508M1_TOE + index);
break;
case CAN_SICK_ID:
// 存储消息到sickfed结构体中
CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);

34
User/device/can_use.h
View File

@ -5,16 +5,22 @@
#include "bsp_can.h"
typedef enum {
CAN_M3508_M1_ID = 0x201, /* 1 */
CAN_M3508_M2_ID = 0x202, /* 2 */
CAN_M3508_M3_ID = 0x203, /* 3 */
CAN_M3508_M4_ID = 0x204, /* 4 */
CAN_CHASSIS_M3508_M1_ID = 0x201, /* 1 */
CAN_CHASSIS_M3508_M2_ID = 0x202, /* 2 */
CAN_CHASSIS_M3508_M3_ID = 0x203, /* 3 */
CAN_CHASSIS_M3508_M4_ID = 0x204, /* 4 */
/**/
CAN_G6020_AgvM1 =0x205,
CAN_G6020_AgvM2 =0x206,
CAN_G6020_AgvM3 =0x207,
CAN_G6020_AgvM4 =0x208,
CAN_UP_M3508_M1_ID = 0x205, /* 1 */
CAN_UP_M3508_M2_ID = 0x206, /* 2 */
CAN_UP_M3508_M3_ID = 0x207, /* 3 */
CAN_UP_M3508_M4_ID = 0x208, /* 4 */
CAN_SICK_ID=0x301,
CAN_G6020_Pitch =0x209,
@ -38,10 +44,10 @@ typedef enum {
typedef enum {
CAN_MOTOR_CHASSIS6020 = 0,
CAN_MOTOR_PITCH6020,
CAN_MOTOR_CHASSIS5065,
CAN_MOTOR_3508,
CAN_MOTOR_5065,
CAN_MOTOR_CHASSIS3508,
CAN_MOTOR_UP3508,
CAN_MOTOR_GROUT_NUM,
} CAN_MotorGroup_e;
@ -77,9 +83,10 @@ typedef union{
}Vesc_ByteGet;
typedef struct {
CAN_DJIOutput_t chassis6020;
// CAN_DJIOutput_t chassis6020;
CAN_VescOutput_t chassis5065;
CAN_DJIOutput_t motor3508;
CAN_DJIOutput_t motor_CHASSIS3508;
CAN_DJIOutput_t motor_UP3508;
CAN_DJIOutput_t pitch6020;
} CAN_Output_t;
@ -98,9 +105,10 @@ typedef struct {
} CAN_RawTx_t;
typedef struct {
BSP_CAN_t chassis6020;
// BSP_CAN_t chassis6020;//禁用
BSP_CAN_t chassis5065;
BSP_CAN_t motor3508;
BSP_CAN_t motor_CHASSIS3508;
BSP_CAN_t motor_UP3508;
BSP_CAN_t pitch6020;
BSP_CAN_t sick;
} CAN_Params_t;
@ -108,7 +116,7 @@ typedef struct {
/* 电机反馈信息 */
typedef struct {
float rotor_angle;
float rotor_ecd;
float rotor_speed;
float torque_current;
float temp;
@ -157,6 +165,8 @@ typedef struct {
const CAN_Params_t *param;
struct {
uint32_t chassis;
uint32_t up;
} mailbox;
osMessageQueueId_t msgq_raw;

175
User/device/cmd.c
View File

@ -14,52 +14,42 @@
/* Private function -------------------------------------------------------- */
/*Export function --------------------------------------------------------------*/
#ifdef dr16_t
#else
#endif
int8_t CMD_Init(CMD_t *cmd){
/*若主结构体为空 自动返回错误 */
if(cmd == NULL) return-1;
/**/
cmd->C_cmd.type =RC;
cmd->C_cmd.mode =NORMAL;
cmd->CMD_CtrlType =RCcontrol;
cmd->CMD_mode =Normal;
return 0;
}
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
// RC_dr16 ,
// RC_r12ds
//
#ifdef r12ds_t
/*乐迪反馈值转换(-1 -- 1 )范围*/
if(rc->ch_x<0) cmd->Vx =map_fp32(rc->ch_x,-337,0,-1,0);
else cmd->Vx =map_fp32(rc->ch_x,0,310,0,1);
// if(rc->ch_x<0) cmd->Vx =map_fp32(rc->ch_x,-337,0,-1,0);
// else cmd->Vx =map_fp32(rc->ch_x,0,310,0,1);
//
//
// if(rc->ch_y<0) cmd->Vy =map_fp32(rc->ch_y,-260,0,-1,0);
// else cmd->Vy =map_fp32(rc->ch_y,0,312,0,1);
// if(rc->ch_w<0) cmd->Vw =map_fp32(rc->ch_w,-308,0,-1,0);
// else cmd->Vw =map_fp32(rc->ch_w,0,291,0,1);
if(rc->ch_y<0) cmd->Vy =map_fp32(rc->ch_y,-260,0,-1,0);
else cmd->Vy =map_fp32(rc->ch_y,0,312,0,1);
if(rc->ch_w<0) cmd->Vw =map_fp32(rc->ch_w,-308,0,-1,0);
else cmd->Vw =map_fp32(rc->ch_w,0,291,0,1);
cmd->key_ctrl_l = rc->key [0];
cmd->key_ctrl_r = rc->key [1];
#elif defined(dr16_t)
// cmd->key_ctrl_l = rc->key [0];
// cmd->key_ctrl_r = rc->key [1];
//
cmd->Vx = rc->ch_r_x;
@ -71,13 +61,6 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
cmd->key_ctrl_l = rc->sw_l;
cmd->key_ctrl_r = rc->sw_r ;
#endif
}
@ -90,19 +73,18 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
*/
static void CMD_RcLostLogic(CMD_t *cmd){
/* 机器人底盘运行模式恢复至放松模式 */
cmd->C_cmd.mode = RELAXED;
cmd->CMD_CtrlType = RELAXED;
}
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){
if (cmd == NULL) return -1;
if (rc == NULL) return -1;
#ifdef dr16_t
/*c当rc丢控时，恢复机器人至默认状态 */
if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
CMD_RcLostLogic(cmd);
} else {
CMD_RcLogic(rc, cmd);
}
#endif
return 0;
}
@ -122,15 +104,15 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
switch(cmd->cmd_status){
case MID:
cmd->C_navi.vx = n->navi.vx;
cmd->C_navi.vy = n->navi.vy;
cmd->C_navi.wz = n->navi.wz;
cmd->cmd_MID360.posx = n->navi.vx;
cmd->cmd_MID360.posy = n->navi.vy;
cmd->cmd_MID360.posw = n->navi.wz;
break;
case PICK :
cmd ->C_pick .posx =n->pick .posx ;
cmd ->C_pick .posy =n->pick .posy ;
cmd ->C_pick .posw =n->pick .posw ;
cmd ->cmd_pick .posx =n->pick .posx ;
cmd ->cmd_pick .posy =n->pick .posy ;
cmd ->cmd_pick .posw =n->pick .posw ;
break;
@ -147,81 +129,68 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
这一部分没有设置具体的模式名，后期根据需要修改
遥控器模式(RC):
左按键 --- 右按键
mode1
up no_mode
mode1
up no_mode
mode2
mode3
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 = UP_CONTROL;
cmd->C_cmd.mode = RC_MODE1;
}
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 = UP_CONTROL;
cmd->C_cmd.mode =RC_MODE2;
}
cmd->Vx = rc->ch_r_x;
cmd->Vy = rc->ch_r_y;
cmd->Vw = rc->ch_l_x;
}
else if(cmd->key_ctrl_l ==CMD_SW_DOWN)
{
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;
}
}
else //左按键打到中间，自动模式
{
if( cmd->key_ctrl_l==CMD_SW_MID ){
cmd->C_cmd.type = AUTO;
cmd->C_cmd.mode = RC_NO_MODE ; //雷达导航
}
if(cmd->key_ctrl_r==CMD_SW_UP)
{
cmd->C_cmd.type = AUTO;
cmd->C_cmd.mode = AUTO_NAVI;
}
if(cmd->key_ctrl_r==CMD_SW_DOWN)//遥控器控制模式下，右拨杆上时启动
{
cmd->C_cmd.type = AUTO;
cmd->C_cmd.mode = AUTO_PICK;
}
cmd->poscamear = rc->ch_l_y;
cmd->key_ctrl_l = rc->sw_l;
cmd->key_ctrl_r = rc->sw_r ;
if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
cmd->CMD_CtrlType =RELAXED;
}
return 0;
else if(rc->sw_l==CMD_SW_UP)
{
cmd ->CMD_CtrlType =RCcontrol;
if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_mode =Pitch; //左上，右上，投篮，设置好的
if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式
if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Pitch_HAND; //左上，右上，手动调整
}
else if(rc->sw_l==CMD_SW_MID)
{
if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_CtrlType =MID_NAVI;; //左中，右上，雷达
if(rc->sw_r ==CMD_SW_MID)
{
cmd ->CMD_CtrlType =RCcontrol;
cmd ->CMD_mode =Normal; //左中，右中，无模式
}
if(rc->sw_r ==CMD_SW_DOWN)
{
cmd ->CMD_mode =Normal; //左中，右下，无模式
cmd ->CMD_CtrlType =RCcontrol;
}
}
else if(rc->sw_l==CMD_SW_DOWN)
{
cmd ->CMD_CtrlType =RCcontrol;
if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，投篮
if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左下，右中，无模式
if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式
}
return 0;
}

205
User/device/cmd.h
View File

@ -1,21 +1,3 @@
/*
该任务用于接收来自各个不同的控制方式所期望的控制指令 将其集中统一化后分发给各个模块
*/
/*
按键控制逻辑
RC模式，左按键打到最上，右按键启用，中间无状态，上下各代表模式1、模式2
雷达导航，左按键打到中间，右按键禁用
左按键打到下面，保留，未启用
*/
#ifndef _CMD_H
#define _CMD_H
#include "struct_typedef.h"
@ -25,95 +7,29 @@
#define MID (0x09)
#define PICK (0x06)
/*选择遥控器，else为r12ds
更改遥控器时，建议取消相应的task
*/
#define dr16_t
//#define r12ds_t
typedef enum{
RC,//遥控器控制，左按键上
RCcontrol,//遥控器控制，左按键上，控制上层
MID_NAVI,//雷达导航
PICK_t,
AUTO,
UP_CONTROL
}CMD_Chassis_CtrlType_e;
RELAXED,//异常模式
}CMD_CtrlType_t;
typedef enum{
RELAXED,//异常模式
NORMAL,
GYRO_STAY,
RC_MODE1,
RC_NO_MODE,
RC_MODE2,
RC_MODE3,
RC_MODE4,
AUTO_NAVI,
AUTO_PICK
}CMD_Chassis_mode_e;
/*该结构体负责接收和解析地盘模块所需要的控制指令*/
typedef struct{
CMD_Chassis_CtrlType_e type;
CMD_Chassis_mode_e mode;
}CMD_Chassis_Ctrl_t;
/* 拨杆位置 */
typedef enum {
CMD_SW_ERR = 0,
CMD_SW_UP = 1,
CMD_SW_MID = 3,
CMD_SW_DOWN = 2,
} CMD_SwitchPos_t;
/*遥控器值，使用CMD_RcLogic函数传入CMD_t结构体*/
typedef struct {
#ifdef dr16_t
float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
float ch_res; /* 第五通道值 */
CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */
uint16_t key; /* 按键值 */
uint16_t res; /* 保留，未启用 */
#else
int16_t ch_x; // X轴（Channel 1）
int16_t mul; // MUL（Channel 2）
int16_t ch_y; // Y轴（Channel 3）
int16_t ch_w; // W轴（Channel 4）
int16_t key[8]; // 按键通道（Channel 5-12）
int16_t offline;
#endif
} __attribute__((packed))CMD_RC_t;
Normal, //无模式
AUTO_NAVI,
AUTO_PICK,
Dribble , //运球
Pitch, //投篮
/*视觉辅助下的投篮*/
Pitch_HAND,
Dribbl_transfer
}CMD_mode_t;
typedef struct {
uint8_t status_fromnuc;
uint8_t ctrl_status; //取其中每一个二进制位用作通信
@ -133,56 +49,95 @@ typedef struct {
{
fp32 angle;
}sick_cali;
} CMD_NUC_t;
/* 拨杆位置 */
typedef enum {
CMD_SW_ERR = 0,
CMD_SW_UP = 1,
CMD_SW_MID = 3,
CMD_SW_DOWN = 2,
} CMD_SwitchPos_t;
typedef struct{
fp32 posy;
fp32 posx;
fp32 posw;
}CMD_FOR_PICK;
/*遥控器值，使用CMD_RcLogic函数传入CMD_t结构体*/
typedef struct {
float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
float ch_res; /* 第五通道值 */
CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */
typedef struct {
fp32 vx;
fp32 vy;
fp32 wz;
uint16_t key; /* 按键值 */
uint16_t res; /* 保留，未启用 */
}CMD_FOR_NAVI;
} __attribute__((packed))CMD_RC_t;
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;
fp32 Vx;
fp32 Vy;
fp32 Vw;
fp32 poscamear;
struct
{
fp32 posx;
fp32 posy;
fp32 posw;
}cmd_pick;
struct
{
fp32 posx;
fp32 posy;
fp32 posw;
}cmd_MID360;
CMD_mode_t CMD_mode;
CMD_CtrlType_t CMD_CtrlType;
CMD_Chassis_Ctrl_t C_cmd;
CMD_FOR_NAVI C_navi;
CMD_FOR_PICK C_pick;
} CMD_t;
/*nuc数据统一到cmd*/
/*
该任务用来处理来自不同控制方式下传回的期望的控制指令在此处统一为通用格式的控制指令发送给其他任务
*/
/* Includes ----------------------------------------------------------------- */
#include "cmd.h"
#include "gpio.h"
#include "user_math.h"
#include <string.h>
/* Private function -------------------------------------------------------- */
/*Export function --------------------------------------------------------------*/
int8_t CMD_Init(CMD_t *cmd);
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) ;
static void CMD_RcLostLogic(CMD_t *cmd);
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc);
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

6
User/device/dr16.c
View File

@ -12,8 +12,8 @@
#include "error_detect.h"
#ifdef dr16_t
//#define
/* Private define ----------------------------------------------------------- */
#define DR16_CH_VALUE_MIN (364u)
#define DR16_CH_VALUE_MID (1024u)
@ -132,5 +132,5 @@ int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc) {
}
#endif

6
User/device/dr16.h
View File

@ -12,10 +12,10 @@
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
#ifdef dr16_t
#define dr16_t
typedef __packed struct
{
uint16_t ch_r_x : 11;
@ -49,5 +49,5 @@ int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc);
#endif
#endif

244
User/device/motor_control.c
View File

@ -1,249 +1,7 @@
/**
****************************(C) COPYRIGHT 2021 QUT****************************
* @file motor_control.c/h
* @brief 3508/2006电机初始化、速度环、位置环；6020电机初始化、位置环（相对角度）
* @note 3508位置环使用的是增量式编码器，没有绝对位置，注意它所对应的角度是M3508的输出轴所对应的角度，由减速比经过换算得来，
* 若要修改对于的真实角度，应该在CAN_RECEIVE中修改相对应的解码函数。
同时3508的控制函数也可对应相同ID的2006配套使用（但注意相对应的角度和速度值可能会出现问题）需要修改相应的减速比
* @history
* Version Date Author Modification remark
* V1.0.0 Dec-23-2021 QUT 1. 完成
* V1.0.1 Dec-23-2023 QUT hks 1. 完成 仅仅对原先函数进行了少量修改，增添了部分注释，未对部分参数进行适配
@verbatim
==============================================================================
==============================================================================
@endverbatim
****************************(C) COPYRIGHT 2021 QUT****************************
*/
#include "motor_control.h"
#include "CAN_receive.h"
#include "pid.h"
#include "user_math.h"
#include "device\device.h"
//3508motor init ID1~4
//mode 1:velocity
// 2:pos
void M3508_motor_init(int mode);
//3508velocity_loop ID1~4
void M3508_velocity_loop(int16_t speed1,int16_t speed2,int16_t speed3,int16_t speed4);
//3508position_loop ID1~4
void M3508_pos_loop(fp32 angle1,fp32 angle2,fp32 angle3,fp32 angle4);
//6020 init ID1
void GM6020_position_loop_init(void);
//6020 pos_loop ID1 relative angle ,this is used for 6020 angel range from 0 to 180
void GM6020_pos_loop(int32_t ecd);
#include "can_use.h"
const motor_measure_t *motor_data[4];//3508 motor _data
pid_type_def motor_pid[4];//3508velocity_pid
pid_type_def angle_pid[4];//3508position_pid
//position_pid time interval 10ms
const fp32 speed_PID[3]={5.0,0.3,0.0}; //P,I,D(velocity)
const fp32 angle_PID[3]={25.0,0.0,1.5}; //P,I,D(position)
//velocity_pid
const fp32 PID[3]={5.0,0.01,0.0};
const motor_measure_t *GM6020_data;//6020 data_motor
gimbal_PID_t gimbal_motor_relative_angle_pid;//6020 relative angle position_pid
pid_type_def gimbal_motor_gyro_pid;//6020 relative angle velocity_pid
gimbal_pid_control_t GM6020_pos_loop_control;//6020 position_control
const fp32 GM6020_PID[3]={3600.0,20.0,0};//6020 velocity_PID
//*****3508*****//
//3508 motor init ID1~4
//mode 1:velocity
// 2:position
void M3508_motor_init(int mode)
{
//get 3508 motor_data from CAN_receive
motor_data[0] = get_chassis_motor_measure_point(0);
motor_data[1] = get_chassis_motor_measure_point(1);
motor_data[2] = get_chassis_motor_measure_point(2);
motor_data[3] = get_chassis_motor_measure_point(3);
switch(mode)
{
//velocity_init
case 1:
for(int i=0;i<4;i++)
{
PID_init(&motor_pid[i],PID_POSITION,PID,16000,2000); //velocity_pid init
}break;
//pos_init
case 2:
for(int i=0;i<4;i++)
{
PID_init(&motor_pid[i],PID_POSITION,speed_PID,16000,2000); //velocity_pid init
PID_init(&angle_pid[i],PID_POSITION,angle_PID,1000,200); // pos_pid init
}break;
}
}
//3508 velocity
/**
* @brief send control current of motor (0x201, 0x202, 0x203, 0x204)
* the param is compatable with the fab data of 3508
* @param[in] speed1: (0x201) 3508 motor control current, range [-16384,16384]
* @param[in] speed2: (0x202) 3508 motor control current, range [-16384,16384]
* @param[in] speed3: (0x203) 3508 motor control current, range [-16384,16384]
* @param[in] speed4: (0x204) 3508 motor control current, range [-16384,16384]
* @retval none
*/
void M3508_velocity_loop(int16_t speed1,int16_t speed2,int16_t speed3,int16_t speed4)
{
int32_t speed[4];
int32_t delta_speed[4];
int i;
speed[0] = speed1;
speed[1] = speed2;
speed[2] = speed3;
speed[3] = speed4;
for(i=0;i<4;i++)
{
delta_speed[i] = PID_calc(&motor_pid[i],motor_data[i]->speed_rpm,speed[i]);
}
CAN_cmd_G3508(delta_speed[0],delta_speed[1],delta_speed[2],delta_speed[3]);
}
//3508 pos
/**
* @brief send control current of motor (0x201, 0x202, 0x203, 0x204)
* this also can used to control the same ID of M2006
* the param angle is correspond to the output shaft of M3508
* @Attention!!!!! the kind fo DJI ecd is incremental.
* @param[in] angle1: (0x201) 3508 motor control current, range [reserved]
* @param[in] angle2: (0x202) 3508 motor control current, range [reserved]
* @param[in] angle3: (0x203) 3508 motor control current, range [reserved]
* @param[in] angle4: (0x204) 3508 motor control current, range [reserved]
* @retval none
*/
void M3508_pos_loop(fp32 angle1,fp32 angle2,fp32 angle3,fp32 angle4)
{
fp32 angle[4];
fp32 delta_angle[4];
int32_t delta_speed[4];
int i;
angle[0] = angle1;
angle[1] = angle2;
angle[2] = angle3;
angle[3] = angle4;
//outer ring angle_loop
for(i=0;i<4;i++)
{
delta_angle[i] = PID_calc(&angle_pid[i],motor_data[i]->total_angle,angle[i]);
}
//inner ring velocity_loop
for(i=0;i<4;i++)
{
delta_speed[i] = PID_calc(&motor_pid[i],motor_data[i]->speed_rpm,delta_angle[i]);
}
CAN_cmd_G3508(delta_speed[0],delta_speed[1],delta_speed[2],delta_speed[3]);
}
//*****6020*****//
//6020motor_init ID1
void GM6020_position_loop_init(void)
{
gimbal_PID_init(&GM6020_pos_loop_control.gimbal_motor_relative_angle_pid,10.0f,0.0f,8.0f,5.0f,0.0f); //angle_loop_pid init
PID_init(&GM6020_pos_loop_control.gimbal_motor_gyro_pid,PID_POSITION,GM6020_PID,30000.0,5000.0); //velocity_loop init
GM6020_data=get_G6020_motor_measure_point();
}
//6020Pos ID1(relative angle)
//this 6020is uesd only for angle_maximum(180)
//ecd encoder valueֵ(0-8191)
void GM6020_pos_loop(int32_t ecd)
{
//set positon
GM6020_pos_loop_control.angel_set=rad(ecd);
//calculate_relative_angle(-PI,PI)
GM6020_pos_loop_control.angel_get=motor_ecd_to_angle_change(GM6020_data->ecd,GM6020_pos_loop_control.angel_set);
//calculate_gyro (set-get)
GM6020_pos_loop_control.gyro_get=rad(GM6020_pos_loop_control.angel_set-GM6020_pos_loop_control.angel_get);
//angle_loop, pid
GM6020_pos_loop_control.gyro_set = gimbal_PID_calc(&GM6020_pos_loop_control.gimbal_motor_relative_angle_pid,GM6020_pos_loop_control.angel_get, GM6020_pos_loop_control.angel_set,GM6020_pos_loop_control.gyro_get);
GM6020_pos_loop_control.current_set = PID_calc(&GM6020_pos_loop_control.gimbal_motor_gyro_pid,GM6020_pos_loop_control.gyro_get, GM6020_pos_loop_control.gyro_set);
//control value
GM6020_pos_loop_control.current_given = (int16_t)(GM6020_pos_loop_control.current_set);
CAN_cmd_gimbal(GM6020_pos_loop_control.current_given,0,0,0);
}
void gimbal_PID_init(gimbal_PID_t *pid, fp32 maxout, fp32 max_iout, fp32 kp, fp32 ki, fp32 kd)
{
// if (pid == NULL)
// {
// return;
// }
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->err = 0.0f;
pid->get = 0.0f;
pid->max_iout = max_iout;
pid->max_out = maxout;
}
fp32 gimbal_PID_calc(gimbal_PID_t *pid, fp32 get, fp32 set, fp32 error_delta)
{
fp32 err;
// if (pid == NULL)
// {
// return 0.0f;
// }
pid->get = get;
pid->set = set;
err = set - get;
pid->err = rad_format(err);
pid->Pout = pid->kp * pid->err;
pid->Iout += pid->ki * pid->err;
pid->Dout = pid->kd * error_delta;
abs_limit_fp(&pid->Iout, pid->max_iout);
pid->out = pid->Pout + pid->Iout + pid->Dout;
abs_limit_fp(&pid->out, pid->max_out);
return pid->out;
}
//calculate relative_angle (-4096,4095)
fp32 motor_ecd_to_angle_change(uint32_t ecd, uint16_t offset_ecd)
{
int32_t relative_ecd = ecd - offset_ecd;
if (relative_ecd > 4095)
{
while(relative_ecd>4095)
{
relative_ecd -= 8191;
}
}
else if (relative_ecd < -4096)
{
while(relative_ecd<-4096)
{
relative_ecd += 8191;
}
}
return relative_ecd * MOTOR_ECD_TO_RAD;
}

60
User/device/motor_control.h
View File

@ -4,58 +4,20 @@
#include "struct_typedef.h"
#include "pid.h"
/*大疆电机解算数据*/
typedef struct
{
fp32 kp;
fp32 ki;
fp32 kd;
fp32 set;
fp32 get;
fp32 err;
fp32 max_out;
fp32 max_iout;
fp32 Pout;
fp32 Iout;
fp32 Dout;
fp32 out;
} gimbal_PID_t;
typedef struct
{
gimbal_PID_t gimbal_motor_relative_angle_pid;
pid_type_def gimbal_motor_gyro_pid;
fp32 angel_set;
fp32 angel_get;
fp32 gyro_set;
fp32 gyro_get;
fp32 current_set;
fp32 current_given;
}gimbal_pid_control_t;
uint16_t ecd;
int16_t speed_rpm;
int16_t given_current;
uint16_t temperate;
int16_t last_ecd;
int16_t round_cnt;
int16_t total_angle;
uint16_t offset_ecd;
uint32_t msg_cnt;
} DJmotor_measure_t;
#define FILTER_BUF_LEN 5
//#define MOTOR_ECD_TO_ANGLE 0.00231311936
#define MOTOR_ECD_TO_ANGLE (360.0 / 8191.0 / (3591.0/187.0))
#define MOTOR_ECD_TO_RAD 0.000766990394f
#define rad_format(Ang) loop_fp32_constrain((Ang), (-4096*MOTOR_ECD_TO_RAD), (4095*MOTOR_ECD_TO_RAD))
#define rad(code) ((code)*MOTOR_ECD_TO_RAD)
void M3508_motor_init(int mode);
void M3508_pos_loop(fp32 angle1,fp32 angle2,fp32 angle3,fp32 angle4);
void GM6020_position_loop_init(void);
void GM6020_pos_loop(int32_t ecd);
void M3508_velocity_loop(int16_t speed1,int16_t speed2,int16_t speed3,int16_t speed4);
fp32 gimbal_PID_calc(gimbal_PID_t *pid, fp32 get, fp32 set, fp32 error_delta);
fp32 motor_ecd_to_angle_change(uint32_t ecd, uint16_t offset_ecd);
void gimbal_PID_init(gimbal_PID_t *pid, fp32 maxout, fp32 max_iout, fp32 kp, fp32 ki, fp32 kd);
void motor_speed_change_low_to_high(int rag);
#endif

8
User/device/vofa.c
View File

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

64
User/task/Calc_task.c
View File

@ -1,64 +0,0 @@
#include "Calc_task.h"
#include "cmsis_os.h"
#include "user_task.h"
#include "chassis.h"
#include "remote_control.h"
#include "map.h"
#include "usart.h"
////路径点选择
//void path_select(void)
//{
// current_path = rc_ctrl.sw[2];
// if(current_path!=last_path)
// {
// switch (current_path)
// {
// //当前点
// case POINT_CURRENT:
//// chassis_pos->IS_SICK = 0;//不启用sick卡位
// path_state.path = &path_current;//选择路径
// path_state.points_num = 1;//确定路径点数
// break;
// //左设定点
// case POINT_LEFT:
//// chassis_pos->IS_SICK = 0;//不启用sick卡位
// path_state.path = &path_left;//选择路径
// path_state.points_num = 1;//确定路径点数
//// chassis_pos->sick.dis_set[2] = SICK_PATH_LEFT_Y;//sick设定值X轴赋值
//// chassis_pos->sick.dis_set[3] = SICK_PATH_LEFT_X;//sick设定值Y轴赋值
// break;
// //前设定点
// case POINT_FRONT:
//// chassis_pos->IS_SICK = 1;//启用sick卡位
// path_state.path = &path_front;//选择路径
// path_state.points_num = 1;//确定路径点数
//// chassis_pos->sick.dis_set[2] = SICK_PATH_FRONT_Y;//sick设定值X轴赋值
//// chassis_pos->sick.dis_set[3] = SICK_PATH_FRONT_X;//sick设定值Y轴赋值
// break;
// }
// chassis_pos->POS_IS_CPT = NO;
//// chassis_pos->SICK_IS_CPT = NO;
// }
// last_path = current_path;
//}
//计算任务
void Task_Calc(void *argument)
{
(void)argument; /* 未使用argument，消除警告 */
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CALCULATE;
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
while(1)
{
tick += delay_tick; /* 计算下一个唤醒时刻 */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

10
User/task/Calc_task.h
View File

@ -1,10 +0,0 @@
#ifndef CALC_TASK_H
#define CALC_TASK_H
void path_select(void);
void cal_task(void);
void calc_init(void);
#endif

15
User/task/can_task.c
View File

@ -30,7 +30,7 @@ void Task_can(void *argument)
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
/* Device Setup */
CAN_Init(&can, &(task_runtime.config.chassis_config->can));
CAN_Init(&can, &(task_runtime.config.config->can));
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
@ -57,14 +57,13 @@ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
CAN_DJIMotor_Control(CAN_MOTOR_PITCH6020,&can_out,&can);
}
if (osMessageQueueGet(task_runtime.msgq.can.output.chassis3508,
&(can_out.motor3508), 0, 0) == osOK) {
CAN_DJIMotor_Control(CAN_MOTOR_3508,&can_out,&can);
&(can_out.motor_CHASSIS3508), 0, 0) == osOK) {
CAN_DJIMotor_Control(CAN_MOTOR_CHASSIS3508,&can_out,&can);
}
if (osMessageQueueGet(task_runtime.msgq.can.output.chassis3508,
&(can_out.motor_UP3508), 0, 0) == osOK) {
CAN_DJIMotor_Control(CAN_MOTOR_UP3508,&can_out,&can);
}
if (osMessageQueueGet(task_runtime.msgq.can.output.chassis6020,
&(can_out.chassis6020),0,0) == osOK) {
CAN_DJIMotor_Control(CAN_MOTOR_CHASSIS6020,&can_out,&can);
}
tick += delay_tick; /* 计算下一个唤醒时刻 */
osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */

10
User/task/chassis_task.c
View File

@ -47,7 +47,7 @@ void Task_Chassis(void *argument)
(void)argument; /* 未使用argument，消除警告*/
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CHASSIS;
Chassis_init(&chassis,&(task_runtime.config.chassis_config->chassis),TASK_FREQ_CHASSIS);
Chassis_init(&chassis,&(task_runtime.config.config->chassis),TASK_FREQ_CHASSIS);
uint32_t tick = osKernelGetTickCount();
@ -70,7 +70,7 @@ void Task_Chassis(void *argument)
osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
/*底盘控制信息获取,目前dji遥控器*/
osMessageQueueGet(task_runtime.msgq.cmd.chassis,&ctrl, NULL, 0);
osMessageQueueGet(task_runtime.msgq.cmd.cmd_ctrl_t,&ctrl, NULL, 0);
/*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/
osKernelLock();
@ -85,13 +85,13 @@ void Task_Chassis(void *argument)
osKernelUnlock();
osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);//清空队列
osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor3508, 0, 0); //发送数据
osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor_CHASSIS3508, 0, 0); //发送数据
osMessageQueueReset(task_runtime.msgq.can.output.pitch6020);
osMessageQueuePut(task_runtime.msgq.can.output.pitch6020, &out.pitch6020, 0, 0);
osMessageQueueReset(task_runtime.msgq.can.output.chassis6020);
osMessageQueuePut(task_runtime.msgq.can.output.chassis6020, &out.chassis6020, 0, 0);
osMessageQueueReset(task_runtime.msgq.can.output.pitch6020 );
osMessageQueuePut(task_runtime.msgq.can.output.pitch6020 , &out.pitch6020, 0, 0);
vofa_send[0] = chassis.vofa_send[0];
vofa_send[1] = chassis.vofa_send[1];

5
User/task/cmd_task.c
View File

@ -46,7 +46,6 @@ void Task_cmd(void *argument){
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);
@ -54,8 +53,8 @@ void Task_cmd(void *argument){
osKernelUnlock(); /* 同上 解锁RTOS内核 */
/*将需要与其他任务共享的数据放到消息队列里 此处主要分享给底盘 后续会添加和上层机构的通信 */
osMessageQueueReset(task_runtime.msgq.cmd.chassis);
osMessageQueuePut(task_runtime.msgq.cmd.chassis,&cmd,0,0);
osMessageQueueReset(task_runtime.msgq.cmd.cmd_ctrl_t);
osMessageQueuePut(task_runtime.msgq.cmd.cmd_ctrl_t,&cmd,0,0);
tick += delay_tick; /*计算下一个唤醒时刻*/
osDelayUntil(tick); /*绝对延时 等待下一个唤醒时刻 */

12
User/task/dr16_task.c
View File

@ -13,7 +13,7 @@
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
#ifdef dr16_t
#ifdef DEBUG
DR16_t dr16;
@ -23,7 +23,7 @@ static DR16_t dr16;
static CMD_RC_t cmd_rc;
#endif
#endif
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
@ -38,13 +38,13 @@ void Task_dr16(void *argument) {
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_DR16;
uint32_t tick = osKernelGetTickCount();
#ifdef dr16_t
DR16_Init(&dr16); /* 初始化dr16 */
#endif
while (1) {
#ifdef dr16_t
#ifdef DEBUG
/* */
@ -65,7 +65,7 @@ void Task_dr16(void *argument) {
osMessageQueueReset(task_runtime.msgq.cmd.raw.rc);
osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0);
}
#endif
tick += delay_tick; /* 计算下一个唤醒时*/

6
User/task/init.c
View File

@ -46,6 +46,8 @@ void Task_Init(void *argument) {
osThreadNew(Task_cmd,NULL,&attr_cmd);
task_runtime.thread.nuc =
osThreadNew(Task_nuc,NULL,&attr_nuc);
task_runtime.thread.up=
osThreadNew(Task_up,NULL,&attr_up);
task_runtime.thread.error_detect =
osThreadNew(Task_error_detect,NULL,&attr_error_detect);
@ -62,8 +64,6 @@ void Task_Init(void *argument) {
task_runtime.msgq.can.output.pitch6020 =
osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL);
task_runtime.msgq.can.output.chassis6020 =
osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL);
/* imu */
@ -77,7 +77,7 @@ void Task_Init(void *argument) {
/*cmd */
task_runtime.msgq.cmd.raw.rc =
osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
task_runtime.msgq.cmd.chassis =
task_runtime.msgq.cmd.cmd_ctrl_t =
osMessageQueueNew(3u, sizeof(CMD_t), NULL);
task_runtime.msgq.cmd.raw.nuc =
osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL);

118
User/task/up_task.c Normal file
View File

@ -0,0 +1,118 @@
/*
上层控制任务
*/
/* Includes ----------------------------------------------------------------- */
#include "up.h"
#include "user_task.h"
#include "can_use.h"
#include <cmsis_os2.h>
#include "vofa.h"
static CAN_t can;
#ifdef DEBUG
CAN_Output_t UP_CAN_out;
UP_t UP ;
CMD_t up_cmd;
#else
static CAN_Output_t up_can_out;
static UP_t UP;
static CMD_t up_cmd;
#endif
float aaa=0;
float bbb=0;
float CCC=0;
float ddd=0;
/**
* \brief
*
* \param argument
*/
void Task_up(void *argument)
{
(void)argument; /* 未使用argument，消除警告*/
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_UP;
uint32_t tick = osKernelGetTickCount();
up_init(&UP,&(task_runtime.config.config->up ),TASK_FREQ_UP);
while(1)
{
#ifdef DEBUG
task_runtime.stack_water_mark.up = osThreadGetStackSpace(osThreadGetId());
#endif
UP_UpdateFeedback(&UP, &can,&up_cmd) ;
// GM6020_control(&UP, 100) ;
// UP_M3508_speed(&UP, 500);
// UP_angle_control(&UP,0,M2006);
//
//
// VESC_M5065_Control(&UP, 20000);
//
UP_control(&UP,&UP_CAN_out,&up_cmd);
// UP.motor_target .go_shoot =aaa;
// UP.motor_target .M2006_angle =bbb ;
// UP.motor_target .M3508_angle =CCC;
// UP.motor_target .go_shoot =aaa;
// UP.motor_target .M2006_angle =bbb;
// UP.motor_target .go_spin =ddd;
//
ALL_Motor_Control(&UP,&UP_CAN_out);
osDelay(1);
/*imu数据获取*/
osMessageQueueGet(task_runtime.msgq.imu.eulr, &UP.pos088.imu_eulr, NULL, 0);
osMessageQueueGet(task_runtime.msgq.imu.gyro, &UP.pos088.bmi088.gyro,NULL, 0);
osMessageQueueGet(task_runtime.msgq.imu.accl, &UP.pos088.bmi088.accl,NULL, 0);
/*can上设备数据获取*/
osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
osMessageQueueGet(task_runtime.msgq.cmd .cmd_ctrl_t , &up_cmd, NULL, 0);
/*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/
osKernelLock();
/*解锁*/
osKernelUnlock();
osMessageQueueReset(task_runtime.msgq.can.output.up3508 );//清空队列
osMessageQueuePut(task_runtime.msgq.can.output.up3508, &UP_CAN_out.motor_UP3508 , 0, 0); //发送数据
osMessageQueuePut(task_runtime.msgq.can.output.shoot5065 ,&UP_CAN_out.chassis5065, 0, 0);
vofa_send [0]=UP.vofa_send [0];
vofa_send [1]=UP.vofa_send [1];
vofa_tx_main (vofa_send);
tick += delay_tick;
osDelayUntil(tick);
}
}

6
User/task/user_task.c
View File

@ -66,4 +66,8 @@ const osThreadAttr_t attr_dr16 = {
.stack_size = 128 *4,
};
const osThreadAttr_t attr_up = {
.name = "up",
.priority = osPriorityRealtime,
.stack_size = 512 * 4,
};

21
User/task/user_task.h
View File

@ -15,6 +15,8 @@
// 分配的频率该如何给定?
#define TASK_FREQ_CHASSIS (900u)
#define TASK_FREQ_UP (900u)
#define TASK_FREQ_CTRL_CMD (500u)
#define TASK_FREQ_NUC (500u)
#define TASK_FREQ_CAN (1000u)
@ -36,6 +38,8 @@ typedef struct {
struct {
osThreadId_t atti_esti;
osThreadId_t chassis;
osThreadId_t up;
osThreadId_t dr16;
osThreadId_t r12ds;
osThreadId_t can;
@ -60,16 +64,18 @@ typedef struct {
}raw;
osMessageQueueId_t chassis;
osMessageQueueId_t cmd_ctrl_t;
osMessageQueueId_t status;
} cmd;
/* can任务放入、读取，电机或电容的输入输出 */
struct {
struct {
osMessageQueueId_t chassis6020;
osMessageQueueId_t chassis5065;
osMessageQueueId_t shoot5065;
osMessageQueueId_t chassis3508;
osMessageQueueId_t pitch6020;
osMessageQueueId_t up3508;
} output;
struct {
osMessageQueueId_t CAN_feedback;//包括底盘，云台6020,3508,5065,sick,can上设备数据
@ -91,6 +97,8 @@ typedef struct {
#ifdef DEBUG
struct {
UBaseType_t chassis;
UBaseType_t up;
UBaseType_t can;
UBaseType_t atti_esti;
UBaseType_t dr16;
@ -108,6 +116,7 @@ typedef struct {
float atti_esti;
float r12ds;
float dr16;
float up;
float cmd;
float nuc;
@ -120,6 +129,7 @@ typedef struct {
float atti_esti;
float r12ds;
float dr16;
float up;
float cmd;
float nuc;
@ -134,6 +144,8 @@ extern const osThreadAttr_t attr_init;
extern const osThreadAttr_t attr_atti_esti;
extern const osThreadAttr_t attr_up;
extern const osThreadAttr_t attr_chassis;
extern const osThreadAttr_t attr_can;
@ -155,6 +167,9 @@ void Task_AttiEsti(void *argument);
void Task_Chassis(void *argument);
void Task_up(void *argument);
void Task_can(void *argument);
void Task_cmd(void *argument);