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

Compare commits

base: LYL:main
Branches Tags
LYL:main
LYL:xiaomi
..
compare: LYL:xiaomi
Branches Tags
LYL:main
LYL:xiaomi

1 Commits
main ... xiaomi

Author SHA1 Message Date
ZHAISHUI04
f1416036e2 xiaomi 2025-03-12 10:57:03 +08:00
18 changed files with 267 additions and 327 deletions
Show Stats Expand all files Collapse all files

4
Core/Src/main.c
View File

@ -80,7 +80,8 @@ 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 */
@ -95,7 +96,6 @@ int main(void)
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_DMA_Init();
MX_SPI1_Init();
MX_TIM4_Init();

2
Core/Src/usart.c
View File

@ -103,7 +103,7 @@ void MX_USART6_UART_Init(void)
/* USER CODE END USART6_Init 1 */
huart6.Instance = USART6;
huart6.Init.BaudRate = 4000000;
huart6.Init.BaudRate = 115200;
huart6.Init.WordLength = UART_WORDLENGTH_8B;
huart6.Init.StopBits = UART_STOPBITS_1;
huart6.Init.Parity = UART_PARITY_NONE;

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

@ -475,3 +475,5 @@ Log at : 2025/2/23|14:39:36|GMT+0800
[info] Log at : 2025/3/11|21:09:29|GMT+0800
[info] Log at : 2025/3/12|10:32:53|GMT+0800

136
MDK-ARM/AUTO_CHASSIS.uvoptx
View File

@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf>
<nTsel>6</nTsel>
<nTsel>3</nTsel>
<sDll></sDll>
<sDllPa></sDllPa>
<sDlgDll></sDlgDll>
@ -114,7 +114,7 @@
<tDlgDll></tDlgDll>
<tDlgPa></tDlgPa>
<tIfile></tIfile>
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
<pMon>BIN\CMSIS_AGDI.dll</pMon>
</DebugOpt>
<TargetDriverDllRegistry>
<SetRegEntry>
@ -158,42 +158,22 @@
<Ww>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>raw,0x0A</ItemText>
<ItemText>can,0x0A</ItemText>
</Ww>
<Ww>
<count>1</count>
<WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText>
<ItemText>can_out,0x0A</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>can,0x0A</ItemText>
<ItemText>can_rx,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>
<ItemText>UP,0x0A</ItemText>
</Ww>
</WatchWindow1>
<WatchWindow2>
@ -860,6 +840,18 @@
<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>
@ -867,7 +859,7 @@
</File>
<File>
<GroupNumber>6</GroupNumber>
<FileNumber>48</FileNumber>
<FileNumber>49</FileNumber>
<FileType>5</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -879,7 +871,7 @@
</File>
<File>
<GroupNumber>6</GroupNumber>
<FileNumber>49</FileNumber>
<FileNumber>50</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -899,7 +891,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>50</FileNumber>
<FileNumber>51</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -911,7 +903,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>51</FileNumber>
<FileNumber>52</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -923,7 +915,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>52</FileNumber>
<FileNumber>53</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -935,7 +927,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>53</FileNumber>
<FileNumber>54</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -947,7 +939,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>54</FileNumber>
<FileNumber>55</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -959,7 +951,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>55</FileNumber>
<FileNumber>56</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -971,7 +963,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>56</FileNumber>
<FileNumber>57</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -983,7 +975,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>57</FileNumber>
<FileNumber>58</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -995,7 +987,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>58</FileNumber>
<FileNumber>59</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1007,7 +999,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>59</FileNumber>
<FileNumber>60</FileNumber>
<FileType>5</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1019,7 +1011,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>60</FileNumber>
<FileNumber>61</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1031,7 +1023,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>61</FileNumber>
<FileNumber>62</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1043,7 +1035,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>62</FileNumber>
<FileNumber>63</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1063,7 +1055,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>63</FileNumber>
<FileNumber>64</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1075,7 +1067,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>64</FileNumber>
<FileNumber>65</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1087,7 +1079,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>65</FileNumber>
<FileNumber>66</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1099,7 +1091,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>66</FileNumber>
<FileNumber>67</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1111,7 +1103,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>67</FileNumber>
<FileNumber>68</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1123,7 +1115,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>68</FileNumber>
<FileNumber>69</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1135,7 +1127,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>69</FileNumber>
<FileNumber>70</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1147,7 +1139,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>70</FileNumber>
<FileNumber>71</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1167,7 +1159,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>71</FileNumber>
<FileNumber>72</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1179,7 +1171,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>72</FileNumber>
<FileNumber>73</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1191,7 +1183,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>73</FileNumber>
<FileNumber>74</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1203,7 +1195,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>74</FileNumber>
<FileNumber>75</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1215,7 +1207,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>75</FileNumber>
<FileNumber>76</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1227,7 +1219,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>76</FileNumber>
<FileNumber>77</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1239,7 +1231,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>77</FileNumber>
<FileNumber>78</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1251,7 +1243,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>78</FileNumber>
<FileNumber>79</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1271,7 +1263,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>79</FileNumber>
<FileNumber>80</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1283,7 +1275,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>80</FileNumber>
<FileNumber>81</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1295,7 +1287,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>81</FileNumber>
<FileNumber>82</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1307,7 +1299,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>82</FileNumber>
<FileNumber>83</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1319,7 +1311,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>83</FileNumber>
<FileNumber>84</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1331,7 +1323,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>84</FileNumber>
<FileNumber>85</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1343,7 +1335,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>85</FileNumber>
<FileNumber>86</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1363,7 +1355,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>86</FileNumber>
<FileNumber>87</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1375,7 +1367,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>87</FileNumber>
<FileNumber>88</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1387,7 +1379,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>88</FileNumber>
<FileNumber>89</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1407,7 +1399,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>12</GroupNumber>
<FileNumber>89</FileNumber>
<FileNumber>90</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1427,7 +1419,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>90</FileNumber>
<FileNumber>91</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1439,7 +1431,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>91</FileNumber>
<FileNumber>92</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1451,7 +1443,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>92</FileNumber>
<FileNumber>93</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1463,7 +1455,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>93</FileNumber>
<FileNumber>94</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>

5
MDK-ARM/AUTO_CHASSIS.uvprojx
View File

@ -1098,6 +1098,11 @@
<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.

11
README.md
View File

@ -1,11 +1,6 @@
r2上层代码
*代码框架
3.12 增加go电机，需测试
在24年r2代码上修改
3.13 go已测试，控制和反馈正常
3.15 vesc控制有问题，id为1和2时，出现只能控制第一个的情况，查找原因是can_use里对id为3进行delay，删除for循环，直接整个控制代码osdelay，问题消失，不加延时无法控制任何电机。原因未查明，但可以正常使用
3.16 up.c里3508需增加位置环，id1，2分别为2006，3508位置环，适应升降机构。
go电机需换rs485模块测试，目前存在可控但收不到反馈数据的情况
12.2 修改遥控器控制

215
User/Module/config.c
View File

@ -28,21 +28,36 @@ 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,
.i = 0.0f,
.d = 1.5f,
.i_limit = 1000.0f,
.M2006_angle_param = { // 外环（角度环）
.p = 25.0f, // 原13.0 → 提升P加速响应
.i = 0.0f, // 保持
.d = 1.5f, // 原1.8 → 增强D抑制振荡
.i_limit = 1000.0f,// 原2000 → 限制积分累积
.out_limit = 3000.0f,
},
.M2006_speed_param = {
.p = 5.0f,
.i = 0.3f,
.d = 0.0f,
.M2006_speed_param = { // 内环（速度环）
.p = 5.0f, // 原5.5 → 提升P加快跟踪
.i = 0.3f, // 保持
.d = 0.0f, // 原0.4 → 抑制新增高频噪声
.i_limit = 2000.0f,
.out_limit = 3000.0f,
},
@ -66,107 +81,99 @@ 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
// },
//
// .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,
// },
.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,
},
// .M3508_param = {
// .p = 15.1f,
// .i = 0.02f,
// .d = 3.2f,
// .i_limit = 200.0f,
// .out_limit =6000.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 = {

37
User/Module/up.c
View File

@ -12,8 +12,7 @@
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
{
u->param = param; /*初始化参数 */
GO_M8010_init();
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 ));
@ -23,17 +22,11 @@ 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 );
}
}
@ -55,21 +48,19 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can) {
int8_t cnt=0;
/*上层电机控制*/
int8_t UP_angle(UP_t *u, fp32 target_angle) {
int8_t UP_M2006_angle(UP_t *u, fp32 target_angle) {
// 获取当前编码器角度
// switch (u->)
float angle = u->motorfeedback.M2006_angle;
// 初始化阶段：前50次循环记录初始值
if (u->M2006.init_cnt < 50) {
u->M2006.orig_angle = angle; // 记录初始编码器值
u->M2006.last_angle = angle;
u->M2006.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--; // 逆时针跨圈
@ -79,10 +70,10 @@ int8_t UP_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;
@ -108,7 +99,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;
}
@ -124,13 +115,7 @@ int8_t GM6020_control(UP_t *u,fp32 angle)
PID_calc (&(u->pid .GM6020_speed ),u->motorfeedback.rotor_pit6020rpm ,delat_speed);
u->motor_target .rotor_pit6020angle =angle ;
}
/*go电机控制*/
int8_t GO_SendData(UP_t *u,int id,float pos)
{
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)
@ -163,5 +148,3 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out)
case STATE_POST_LAUNCH : break ;
}
}

57
User/Module/up.h
View File

@ -34,32 +34,7 @@ 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
{
/*该部分决定PID的参数整定在config中修改*/
@ -72,21 +47,18 @@ typedef struct
pid_param_t M2006_speed_param;
pid_param_t M2006_angle_param;
pid_param_t M3508_speed_param;
GO_param_t go_param;
pid_param_t M3508_speed_param;
}UP_Param_t;
typedef struct
{
MotorType_t motor;
float orig_angle;
float last_angle;
int32_t round_cnt;
uint16_t init_cnt;
float total_angle;
float orig_angle;
int8_t offset_flag;
float angle;
float last_angle;
float total_angle;
}motor_angle_data;
}M2006_data;
typedef struct{
@ -96,10 +68,13 @@ typedef struct{
UP_Imu_t pos088;
OperationState ctrl;
motor_angle_data M2006;
motor_angle_data M3508;
GO_Motorfield *GO_motor_info[GO_NUM];//存放go电机数据
struct {
float orig_angle; // 初始偏移量
float last_angle; // 上一次编码器值
int32_t round_cnt; // 圈数计数器
uint16_t init_cnt; // 初始化计数器（前50次记录初始值）
float total_angle;
} M2006;
struct{
fp32 rotor_pit6020ecd;
@ -170,8 +145,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 GO_SendData(UP_t *u,int id,float pos);
int8_t UP_angle(UP_t *u, fp32 target_angle) ;
int8_t UP_M2006_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);

12
User/bsp/crc16.c
View File

@ -48,15 +48,3 @@ bool CRC16_Verify(const uint8_t *buf, size_t len) {
((const uint16_t *)((const uint8_t *)buf +
(len % 2)))[len / sizeof(uint16_t) - 1];
}
uint16_t do_crc_table(uint8_t *ptr, int len)
{
uint16_t crc = 0x0000;
while(len--)
{
crc = (crc >> 8) ^ crc16_tab[(crc ^ *ptr++) & 0xff];
}
return crc;
}

1
User/bsp/crc16.h
View File

@ -12,7 +12,6 @@ extern "C" {
uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc);
bool CRC16_Verify(const uint8_t *buf, size_t len);
uint16_t do_crc_table(uint8_t *ptr, int len);
#ifdef __cplusplus
}

22
User/device/GO_M8010_6_Driver.c
View File

@ -6,19 +6,19 @@
#include "bsp_usart.h"
#include <cmsis_os2.h>
GO_Motorfield GO_motor_info[GO_NUM];//存放电机数据
static const float gravit_const =9.81;//计算前馈力矩有关
GO_Motorfield GO_motor_info[GO_NUM];//存放电机数据
const float gravit_const =9.81;//计算前馈力矩有关
//数据处理函数
static void GO_M8010_recv_data(uint8_t* Temp_buffer);
/**
*@brief 电机初始化
*/
void GO_M8010_init (void){
void GO_M8010_init (){
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;
@ -38,7 +38,7 @@ void GO_M8010_init (void){
//暂存接收数据
uint8_t Temp_buffer[16];
static uint8_t Temp_buffer[16];
void USART6_RxCompleteCallback(void )
{
@ -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);
}
@ -83,8 +83,8 @@ void uartTxCB(UART_HandleTypeDef *huart)
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
GO_Motorfield* motor;
motor = GO_motor_info+id;
//这个rev是干啥的
@ -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(1);
osDelay(100);
HAL_UART_Receive_DMA(huart, Temp_buffer, 16);//有无都可以

56
User/device/GO_M8010_6_Driver.h
View File

@ -19,35 +19,39 @@ typedef struct
uint8_t id :4;
uint8_t status :3;
uint8_t none :1;
} RIS_Mode_t ;
typedef struct
{
int16_t tor_des;
int16_t spd_des;
int32_t pos_des;
uint16_t k_pos;
uint16_t k_spd;
} RIS_Comd_t;
} RIS_Mode_t /*__attribute__((packed))*/;
/**
*
* @brief <EFBFBD><EFBFBD><EFBFBD>״̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
*
*/
typedef struct
{
uint8_t head[2];
RIS_Mode_t mode;
RIS_Comd_t comd;
uint16_t CRC16;
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)
} RIS_Comd_t; // <20><><EFBFBD>Ʋ<EFBFBD><C6B2><EFBFBD> 12Byte
} ControlData_t;
/**
* @brief <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD><EFBFBD><EFBFBD>ʽ
*
*/
typedef struct
{
uint8_t head[2]; // <20><>ͷ 2Byte
RIS_Mode_t mode; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ 1Byte
RIS_Comd_t comd; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 12Byte
uint16_t CRC16; // CRC 2Byte
} ControlData_t; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 17Byte
@ -58,12 +62,12 @@ typedef struct {
uint16_t correct;
int MError;
int Temp;
float tar_pos;
float tar_w;
float T;
float W;
float Pos;
int footForce;
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
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;

1
User/device/can_use.h
View File

@ -102,6 +102,7 @@ typedef struct {
BSP_CAN_t chassis5065;
BSP_CAN_t motor3508;
BSP_CAN_t pitch6020;
BSP_CAN_t sick;
} CAN_Params_t;

5
User/task/can_task.c
View File

@ -73,7 +73,10 @@ 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); /* 运行结束，等待下一个周期唤醒 */
}

22
User/task/up_task.c
View File

@ -29,9 +29,7 @@ static UP_t UP;
#endif
float aaa=0;
float bbb=0;
float CCC=0;
/**
@ -55,23 +53,11 @@ void Task_up(void *argument)
#endif
UP_UpdateFeedback(&UP, &can) ;
// GM6020_control(&UP, 100) ;
UP_angle(&UP, bbb);
// UP_M3508_speed(&UP, 500);
// VESC_M5065_Control(&UP, 20000);
GO_SendData(&UP, 1,CCC);
GO_SendData(&UP, 0,aaa);
// UP_M2006_angle(&UP, 180);
// UP_M3508_speed(&UP, 500);
// VESC_M5065_Control(&UP, 1000);
ALL_Motor_Control(&UP,&UP_CAN_out);
osDelay(1);
/*imu数据获取*/
osMessageQueueGet(task_runtime.msgq.imu.eulr, &UP.pos088.imu_eulr, NULL, 0);