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

修改vesc底层控制混乱问题，3508角度环待增加

Browse Source
This commit is contained in:
ZHAISHUI04 2025-03-17 00:32:06 +08:00
parent ea949443ad
commit 06273f875a
12 changed files with 290 additions and 243 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Core/Src/main.c
View File

@ -80,8 +80,7 @@ int main(void)
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* Reset of all peripherals, Initializes the Flash interface and the Systick. * HAL_Init();
/* USER CODE BEGIN Init */
@ -96,6 +95,7 @@ int main(void)
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_DMA_Init();
MX_SPI1_Init();
MX_TIM4_Init();

139
MDK-ARM/AUTO_CHASSIS.uvoptx
View File

@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf>
<nTsel>3</nTsel>
<nTsel>6</nTsel>
<sDll></sDll>
<sDllPa></sDllPa>
<sDlgDll></sDlgDll>
@ -114,7 +114,7 @@
<tDlgDll></tDlgDll>
<tDlgPa></tDlgPa>
<tIfile></tIfile>
<pMon>BIN\CMSIS_AGDI.dll</pMon>
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
</DebugOpt>
<TargetDriverDllRegistry>
<SetRegEntry>
@ -158,17 +158,42 @@
<Ww>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>afsa,0x10</ItemText>
<ItemText>raw,0x0A</ItemText>
</Ww>
<Ww>
<count>1</count>
<WinNumber>1</WinNumber>
<ItemText>motor,0x0A</ItemText>
<ItemText>UP,0x0A</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>CAN_RawTx_t</ItemText>
<ItemText>can,0x0A</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>can_out,0x0A</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>bbb,0x0A</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>aaa,0x0A</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>CCC</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>GO_motor_info</ItemText>
</Ww>
</WatchWindow1>
<WatchWindow2>
@ -835,18 +860,6 @@
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\Module\Chassis.c</PathWithFileName>
<FilenameWithoutPath>Chassis.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>6</GroupNumber>
<FileNumber>48</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\Module\config.c</PathWithFileName>
<FilenameWithoutPath>config.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
@ -854,7 +867,7 @@
</File>
<File>
<GroupNumber>6</GroupNumber>
<FileNumber>49</FileNumber>
<FileNumber>48</FileNumber>
<FileType>5</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -866,7 +879,7 @@
</File>
<File>
<GroupNumber>6</GroupNumber>
<FileNumber>50</FileNumber>
<FileNumber>49</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -886,7 +899,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>51</FileNumber>
<FileNumber>50</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -898,7 +911,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>52</FileNumber>
<FileNumber>51</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -910,7 +923,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>53</FileNumber>
<FileNumber>52</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -922,7 +935,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>54</FileNumber>
<FileNumber>53</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -934,7 +947,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>55</FileNumber>
<FileNumber>54</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -946,7 +959,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>56</FileNumber>
<FileNumber>55</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -958,7 +971,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>57</FileNumber>
<FileNumber>56</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -970,7 +983,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>58</FileNumber>
<FileNumber>57</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -982,7 +995,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>59</FileNumber>
<FileNumber>58</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -994,7 +1007,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>60</FileNumber>
<FileNumber>59</FileNumber>
<FileType>5</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1006,7 +1019,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>61</FileNumber>
<FileNumber>60</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1018,7 +1031,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>62</FileNumber>
<FileNumber>61</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1030,7 +1043,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>63</FileNumber>
<FileNumber>62</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1050,7 +1063,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>64</FileNumber>
<FileNumber>63</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1062,7 +1075,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>65</FileNumber>
<FileNumber>64</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1074,7 +1087,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>66</FileNumber>
<FileNumber>65</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1086,7 +1099,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>67</FileNumber>
<FileNumber>66</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1098,7 +1111,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>68</FileNumber>
<FileNumber>67</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1110,7 +1123,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>69</FileNumber>
<FileNumber>68</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1122,7 +1135,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>70</FileNumber>
<FileNumber>69</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1134,7 +1147,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>71</FileNumber>
<FileNumber>70</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1154,7 +1167,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>72</FileNumber>
<FileNumber>71</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1166,7 +1179,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>73</FileNumber>
<FileNumber>72</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1178,7 +1191,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>74</FileNumber>
<FileNumber>73</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1190,7 +1203,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>75</FileNumber>
<FileNumber>74</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1202,7 +1215,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>76</FileNumber>
<FileNumber>75</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1214,7 +1227,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>77</FileNumber>
<FileNumber>76</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1226,7 +1239,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>78</FileNumber>
<FileNumber>77</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1238,7 +1251,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>79</FileNumber>
<FileNumber>78</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1258,7 +1271,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>80</FileNumber>
<FileNumber>79</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1270,7 +1283,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>81</FileNumber>
<FileNumber>80</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1282,7 +1295,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>82</FileNumber>
<FileNumber>81</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1294,7 +1307,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>83</FileNumber>
<FileNumber>82</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1306,7 +1319,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>84</FileNumber>
<FileNumber>83</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1318,7 +1331,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>85</FileNumber>
<FileNumber>84</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1330,7 +1343,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>86</FileNumber>
<FileNumber>85</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1350,7 +1363,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>87</FileNumber>
<FileNumber>86</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1362,7 +1375,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>88</FileNumber>
<FileNumber>87</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1374,7 +1387,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>89</FileNumber>
<FileNumber>88</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1394,7 +1407,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>12</GroupNumber>
<FileNumber>90</FileNumber>
<FileNumber>89</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1414,7 +1427,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>91</FileNumber>
<FileNumber>90</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1426,7 +1439,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>92</FileNumber>
<FileNumber>91</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1438,7 +1451,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>93</FileNumber>
<FileNumber>92</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1450,7 +1463,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>94</FileNumber>
<FileNumber>93</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>

5
MDK-ARM/AUTO_CHASSIS.uvprojx
View File

@ -1098,11 +1098,6 @@
<Group>
<GroupName>User/Module</GroupName>
<Files>
<File>
<FileName>Chassis.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\Module\Chassis.c</FilePath>
</File>
<File>
<FileName>config.c</FileName>
<FileType>1</FileType>

BIN
MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf
View File

Binary file not shown.

215
User/Module/config.c
View File

@ -28,36 +28,21 @@ ConfigParam_t param_chassis ={
static const ConfigParam_t param_chassis ={
//
//typedef struct
//{
// /*该部分决定PID的参数整定在config中修改*/
// pid_param_t VESC_5065_M1_param;
// pid_param_t VESC_5065_M2_param;
//
// pid_param_t UP_GM6020_speed_param;
// pid_param_t UP_GM6020_angle_param;
//
// pid_param_t M2006_speed_param;
// pid_param_t M2006_angle_param;
//
//}UP_Param_t;
#endif
.up={
.M2006_angle_param = { // 外环（角度环）
.p = 25.0f, // 原13.0 → 提升P加速响应
.i = 0.0f, // 保持
.d = 1.5f, // 原1.8 → 增强D抑制振荡
.i_limit = 1000.0f,// 原2000 → 限制积分累积
.M2006_angle_param = {
.p = 25.0f,
.i = 0.0f,
.d = 1.5f,
.i_limit = 1000.0f,
.out_limit = 3000.0f,
},
.M2006_speed_param = { // 内环（速度环）
.p = 5.0f, // 原5.5 → 提升P加快跟踪
.i = 0.3f, // 保持
.d = 0.0f, // 原0.4 → 抑制新增高频噪声
.M2006_speed_param = {
.p = 5.0f,
.i = 0.3f,
.d = 0.0f,
.i_limit = 2000.0f,
.out_limit = 3000.0f,
},
@ -81,99 +66,107 @@ static const ConfigParam_t param_chassis ={
.d = 3.2f,
.i_limit = 200.0f,
.out_limit =6000.0f,
},
.go_param={
.rev = 0,
.T=0.1,
.W=0.1,
.K_P=0.1,
.K_W=0.1,
}
},
.chassis = {/**/
.C6020pitAngle_param = {
.p = 15.0f,
.i = 0.3f,
.d =0.0f,
.i_limit = 200.0f,
.out_limit = 3000.0f,
},
.C6020pitOmega_param = {
.p =30.0f,
.i =0.3f,
.d =0.0f,
.i_limit = 200.0f,
.out_limit = 3000.0f
},
// .chassis = {/**/
// .C6020pitAngle_param = {
// .p = 15.0f,
// .i = 0.3f,
// .d =0.0f,
// .i_limit = 200.0f,
// .out_limit = 3000.0f,
// },
// .C6020pitOmega_param = {
// .p =30.0f,
// .i =0.3f,
// .d =0.0f,
// .i_limit = 200.0f,
// .out_limit = 3000.0f
// },
//
// .Gimbal_yawAngle_param = {
// .p =8.0f,
// .i =0.0f,
// .d =0.0f,
// .i_limit = 200.0f,
// .out_limit = 3000.0f
// },
//
// .Gimbal_yawOmega_param = {
// .p =18.0f,
// .i =0.0f,
// .d =0.0f,
// .i_limit = 200.0f,
// .out_limit = 3000.0f
// },
//
// .Gimbal_pitchAngle_param = {
// .p =8.0f,
// .i =0.0f,
// .d =0.0f,
// .i_limit = 200.0f,
// .out_limit = 3000.0f
// },
//
// .Gimbal_pitchOmega_param = {
// .p =18.0f,
// .i =0.0f,
// .d =0.0f,
// .i_limit = 200.0f,
// .out_limit = 3000.0f
// },
// .AngleCor_param = {
// .p =0.8f,
// .i =0.0f,
// .d =1.0f,
// .i_limit = 0.0f,
// .out_limit =5000.0f,
// },
//
// .OmegaCor_param = {
// .p =23.5f,
// .i =0.0f,
// .d =0.05f,
// .i_limit = 0.0f,
// .out_limit =5000.0f,
// },
//
// .ImuCor_param = {
// .p =95.0f,
// .i =0.0f,
// .d =0.0f,
// .i_limit = 0.0f,
// .out_limit =200.0f,
// },
//
// .DisCamera_param = {
// .p =80.0f,
// .i =0.1f,
// .d =0.0f,
// .i_limit = 0.0f,
// .out_limit =5000.0f,
// },
.Gimbal_yawAngle_param = {
.p =8.0f,
.i =0.0f,
.d =0.0f,
.i_limit = 200.0f,
.out_limit = 3000.0f
},
// .M3508_param = {
// .p = 15.1f,
// .i = 0.02f,
// .d = 3.2f,
// .i_limit = 200.0f,
// .out_limit =6000.0f,
// }
//
.Gimbal_yawOmega_param = {
.p =18.0f,
.i =0.0f,
.d =0.0f,
.i_limit = 200.0f,
.out_limit = 3000.0f
},
.Gimbal_pitchAngle_param = {
.p =8.0f,
.i =0.0f,
.d =0.0f,
.i_limit = 200.0f,
.out_limit = 3000.0f
},
.Gimbal_pitchOmega_param = {
.p =18.0f,
.i =0.0f,
.d =0.0f,
.i_limit = 200.0f,
.out_limit = 3000.0f
},
.AngleCor_param = {
.p =0.8f,
.i =0.0f,
.d =1.0f,
.i_limit = 0.0f,
.out_limit =5000.0f,
},
.OmegaCor_param = {
.p =23.5f,
.i =0.0f,
.d =0.05f,
.i_limit = 0.0f,
.out_limit =5000.0f,
},
.ImuCor_param = {
.p =95.0f,
.i =0.0f,
.d =0.0f,
.i_limit = 0.0f,
.out_limit =200.0f,
},
.DisCamera_param = {
.p =80.0f,
.i =0.1f,
.d =0.0f,
.i_limit = 0.0f,
.out_limit =5000.0f,
},
.M3508_param = {
.p = 15.1f,
.i = 0.02f,
.d = 3.2f,
.i_limit = 200.0f,
.out_limit =6000.0f,
}
},
// },
.can = {

34
User/Module/up.c
View File

@ -23,11 +23,17 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
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 );
}
}
@ -49,19 +55,21 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can) {
int8_t cnt=0;
/*上层电机控制*/
int8_t UP_M2006_angle(UP_t *u, fp32 target_angle) {
int8_t UP_angle(UP_t *u, fp32 target_angle) {
// 获取当前编码器角度
float angle = u->motorfeedback.M2006_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.last_angle = angle;
u->M2006.init_cnt++; // 初始化计数器递增
return 0; // 初始化阶段不执行控制
return 0;
}
// 计算角度差值（处理编码器溢出）
float delta = angle - u->M2006.last_angle;
if (delta > 4096) {
u->M2006.round_cnt--; // 逆时针跨圈
@ -71,10 +79,10 @@ int8_t UP_M2006_angle(UP_t *u, fp32 target_angle) {
u->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;
// PID控制计算
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);
u->motor_target.M2006_angle = target_angle;
@ -100,7 +108,7 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 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_M2out =-u->motor_target .VESC_5065_M2_rpm;
}
@ -116,7 +124,13 @@ 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)
{
GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), id,u->param->go_param .rev ,u->param->go_param .T ,u->param->go_param .W ,AngleChange(RADIAN,pos),u->param->go_param .K_P ,u->param->go_param .K_W );
}
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
@ -149,3 +163,5 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out)
case STATE_POST_LAUNCH : break ;
}
}

55
User/Module/up.h
View File

@ -34,6 +34,31 @@ typedef struct {
}UP_Imu_t;
/**
*@input[in] rev 暂时不知道干啥用的，github为回答issue
*@input[in] T 力矩，单位N·m
*@input[in] Pos 目标位置，单位rad
*@input[in] W 目标速度，单位rad/s
*@input[in] K_P 位置环环kp
*@input[in] K_W 速度环kp
*@note 控制公式 ： t = T + （设定位置 - 实际位置）*K_P + （设定速度 - 实际速度）*K_W
*/
typedef struct {
int rev; //暂时不知何用
float T;
float W;
float K_P;
float K_W;
}GO_param_t;
typedef enum {
M2006 = 1,
M3508
} MotorType_t;
typedef struct
{
@ -48,17 +73,20 @@ typedef struct
pid_param_t M2006_angle_param;
pid_param_t M3508_speed_param;
GO_param_t go_param;
}UP_Param_t;
typedef struct
{
float orig_angle;
int8_t offset_flag;
float angle;
float last_angle;
float total_angle;
MotorType_t motor;
float orig_angle;
float last_angle;
int32_t round_cnt;
uint16_t init_cnt;
float total_angle;
}M2006_data;
}motor_angle_data;
typedef struct{
@ -68,13 +96,10 @@ typedef struct{
UP_Imu_t pos088;
OperationState ctrl;
struct {
float orig_angle; // 初始偏移量
float last_angle; // 上一次编码器值
int32_t round_cnt; // 圈数计数器
uint16_t init_cnt; // 初始化计数器（前50次记录初始值）
float total_angle;
} M2006;
motor_angle_data M2006;
motor_angle_data M3508;
GO_Motorfield *GO_motor_info[GO_NUM];//存放go电机数据
struct{
fp32 rotor_pit6020ecd;
@ -145,8 +170,8 @@ 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 GM6020_control(UP_t *u,fp32 angle);
int8_t VESC_M5065_Control(UP_t *u,fp32 speed);
int8_t UP_M2006_angle(UP_t *u,fp32 target_angle);
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 ALL_Motor_Control(UP_t *u,CAN_Output_t *out);
int8_t UP_M2006_angle(UP_t *u,fp32 target_angle);

18
User/device/GO_M8010_6_Driver.c
View File

@ -6,7 +6,7 @@
#include "bsp_usart.h"
#include <cmsis_os2.h>
GO_Motorfield GO_motor_info[GO_NUM];//存放电机数据
GO_Motorfield GO_motor_info[GO_NUM];//存放电机数据
static const float gravit_const =9.81;//计算前馈力矩有关
//数据处理函数
@ -14,11 +14,11 @@ static void GO_M8010_recv_data(uint8_t* Temp_buffer);
/**
*@brief 电机初始化
*/
void GO_M8010_init (){
void GO_M8010_init (void){
for (uint8_t id= 0; id <GO_NUM ;id++)
{
GO_motor_info[id].id = id;
GO_motor_info[id].mode = 1;
GO_motor_info[id].mode = 1; //位置模式
GO_motor_info[id].correct = 1;
GO_motor_info[id].MError = 0;
GO_motor_info[id].Temp = 0;
@ -47,12 +47,12 @@ void USART6_RxCompleteCallback(void )
uint16_t crc = CRC16_Calc(Temp_buffer,sizeof(Temp_buffer)-2,0x0000);
if ((Temp_buffer[14] != (crc&0xFF)) || (Temp_buffer[15] != ((crc>>8) & 0xFF)))
{
HAL_GPIO_WritePin(LED_B_GPIO_Port, LED_B_Pin, GPIO_PIN_SET); //蓝色灯亮
HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET); //红色灯灭
// HAL_GPIO_WritePin(LED_B_GPIO_Port, LED_B_Pin, GPIO_PIN_SET); //蓝色灯亮
// HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET); //红色灯灭
return;
}
HAL_GPIO_WritePin(GPIOH, GPIO_PIN_11, GPIO_PIN_SET); // indicate CRC correct
HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET); //红色灯灭
// HAL_GPIO_WritePin(GPIOH, GPIO_PIN_11, GPIO_PIN_SET); // indicate CRC correct
// HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET); //红色灯灭
GO_M8010_recv_data(Temp_buffer);
}
@ -79,11 +79,11 @@ void uartTxCB(UART_HandleTypeDef *huart)
*@input[in] K_W 速度环kp
*@note 控制公式 ： t = T + （设定位置 - 实际位置）*K_P + （设定速度 - 实际速度）*K_W
*/
GO_Motorfield* motor;
void GO_M8010_send_data(UART_HandleTypeDef *huart, int id,int rev,float T,float Pos,
float W,float K_P,float K_W)
{
GO_Motorfield* motor;
// a pointer to target motor
motor = GO_motor_info+id;
@ -130,7 +130,7 @@ void GO_M8010_send_data(UART_HandleTypeDef *huart, int id,int rev,float T,float
// interrupt buffer sending
HAL_UART_Transmit_IT(huart,motor->buffer,sizeof(motor->buffer));
osDelay(1000);
osDelay(1);
HAL_UART_Receive_DMA(huart, Temp_buffer, 16);//有无都可以

29
User/device/GO_M8010_6_Driver.h
View File

@ -19,21 +19,18 @@ typedef struct
uint8_t id :4;
uint8_t status :3;
uint8_t none :1;
} RIS_Mode_t /*__attribute__((packed))*/;
} RIS_Mode_t ;
/**
* @brief <EFBFBD><EFBFBD><EFBFBD>״̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
*
*/
typedef struct
{
int16_t tor_des; // <20><><EFBFBD><EFBFBD><EFBFBD>ؽ<EFBFBD><D8BD><EFBFBD><EFBFBD>Ť<EFBFBD><C5A4> unit: N.m (q8)
int16_t spd_des; // <20><><EFBFBD><EFBFBD><EFBFBD>ؽ<EFBFBD><D8BD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> unit: rad/s (q7)
int32_t pos_des; // <20><><EFBFBD><EFBFBD><EFBFBD>ؽ<EFBFBD><D8BD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB> unit: rad (q15)
uint16_t k_pos; // <20><><EFBFBD><EFBFBD><EFBFBD>ؽڸն<DAB8>ϵ<EFBFBD><CFB5> unit: 0.0-1.0 (q15)
uint16_t k_spd; // <20><><EFBFBD><EFBFBD><EFBFBD>ؽ<EFBFBD><D8BD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5> unit: 0.0-1.0 (q15)
int16_t tor_des;
int16_t spd_des;
int32_t pos_des;
uint16_t k_pos;
uint16_t k_spd;
} RIS_Comd_t;
@ -61,12 +58,12 @@ typedef struct {
uint16_t correct;
int MError;
int Temp;
float tar_pos; // target position
float tar_w; // target speed
float T; // <20><>ǰʵ<C7B0>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float W; // <20><>ǰʵ<C7B0>ʵ<EFBFBD><CAB5><EFBFBD>ٶȣ<D9B6><C8A3><EFBFBD><EFBFBD>٣<EFBFBD>
float Pos; // <20><>ǰ<EFBFBD><C7B0><EFBFBD>λ<EFBFBD><CEBB>
int footForce; // They dont even know what 7 is this so we dont update this
float tar_pos;
float tar_w;
float T;
float W;
float Pos;
int footForce;
uint8_t buffer[17];
uint8_t Rec_buffer[16];
ControlData_t motor_send_data;

6
User/device/can_use.c
View File

@ -266,10 +266,10 @@ int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_
for(int i=0 ; i < 4 ; i ++)
{
if(i==2) //此处可能同时发送四个vesc导致只有三个轮子接收到数据 故加上delay
{
// if(i==2) //此处可能同时发送四个vesc导致只有三个轮子接收到数据 故加上delay
// {
osDelay(1);
}
// }
//将单个电机的期望输出值通过联合体拆分
Byte[0] = raw[i].byte.byte1;
Byte[1] = raw[i].byte.byte2;

5
User/task/can_task.c
View File

@ -73,10 +73,7 @@ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
&(can_out.chassis5065), 0, 0) == osOK) {
CAN_VESC_Control(1,CAN_MOTOR_CHASSIS5065, &can_out ,&can);
}
if (osMessageQueueGet(task_runtime.msgq.can.output.shoot5065,
&(can_out.chassis5065), 0, 0) == osOK) {
CAN_VESC_Control(2,CAN_MOTOR_CHASSIS5065, &can_out ,&can);
}
tick += delay_tick; /* 计算下一个唤醒时刻 */
osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */
}

19
User/task/up_task.c
View File

@ -29,7 +29,9 @@ static UP_t UP;
#endif
float aaa=0;
float bbb=0;
float CCC=0;
/**
@ -53,11 +55,20 @@ void Task_up(void *argument)
#endif
UP_UpdateFeedback(&UP, &can) ;
// GM6020_control(&UP, 100) ;
// UP_M2006_angle(&UP, 180);
UP_angle(&UP, bbb);
// UP_M3508_speed(&UP, 500);
VESC_M5065_Control(&UP, 00);
// VESC_M5065_Control(&UP, 20000);
GO_SendData(&UP, 1,CCC);
GO_SendData(&UP, 0,aaa);
ALL_Motor_Control(&UP,&UP_CAN_out);
// GO_M8010_send_data(&huart6, 0,0,0,5,10,0.12,0.08);
osDelay(1);