Compare commits
2 Commits
4859c69fad
...
f9e93b88c1
| Author | SHA1 | Date | |
|---|---|---|---|
| f9e93b88c1 | |||
| d0efaf8dab |
4
MDK-ARM/.vscode/keil-assistant.log
vendored
4
MDK-ARM/.vscode/keil-assistant.log
vendored
@ -475,3 +475,7 @@
|
||||
|
||||
[info] Log at : 2025/6/24|03:28:50|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/26|19:47:59|GMT+0800
|
||||
|
||||
[info] Log at : 2025/6/27|16:40:58|GMT+0800
|
||||
|
||||
|
||||
@ -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,13 +114,13 @@
|
||||
<tDlgDll></tDlgDll>
|
||||
<tDlgPa></tDlgPa>
|
||||
<tIfile></tIfile>
|
||||
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
|
||||
<pMon>BIN\CMSIS_AGDI.dll</pMon>
|
||||
</DebugOpt>
|
||||
<TargetDriverDllRegistry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>ST-LINKIII-KEIL_SWO</Key>
|
||||
<Name>-U00260035480000034E575152 -O206 -SF1000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
|
||||
<Name>-U00160029510000164E574E32 -O206 -SF1000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
@ -140,7 +140,7 @@
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>DLGUARM</Key>
|
||||
<Name>(105=-1,-1,-1,-1,0)</Name>
|
||||
<Name></Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
@ -168,27 +168,37 @@
|
||||
<Ww>
|
||||
<count>2</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>f,0x0A</ItemText>
|
||||
<ItemText>chassis,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>3</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>a,0x0A</ItemText>
|
||||
<ItemText>cmd_fromnuc,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>4</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>chassis</ItemText>
|
||||
<ItemText>nucbuf,0x10</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>5</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd_fromnuc,0x0A</ItemText>
|
||||
<ItemText>UP,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>6</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>nucbuf,0x0A</ItemText>
|
||||
<ItemText>tar_angle</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>7</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>up_cmd</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>8</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>a,0x0A</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
@ -877,6 +887,18 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>6</GroupNumber>
|
||||
<FileNumber>50</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\User\Module\up_utils.c</PathWithFileName>
|
||||
<FilenameWithoutPath>up_utils.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
</Group>
|
||||
|
||||
<Group>
|
||||
@ -887,7 +909,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>50</FileNumber>
|
||||
<FileNumber>51</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -899,7 +921,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>51</FileNumber>
|
||||
<FileNumber>52</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -911,7 +933,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>52</FileNumber>
|
||||
<FileNumber>53</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -923,7 +945,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>53</FileNumber>
|
||||
<FileNumber>54</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -935,7 +957,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>54</FileNumber>
|
||||
<FileNumber>55</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -947,7 +969,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>55</FileNumber>
|
||||
<FileNumber>56</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -959,7 +981,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>56</FileNumber>
|
||||
<FileNumber>57</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -971,7 +993,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>57</FileNumber>
|
||||
<FileNumber>58</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -983,7 +1005,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>58</FileNumber>
|
||||
<FileNumber>59</FileNumber>
|
||||
<FileType>5</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -995,7 +1017,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>59</FileNumber>
|
||||
<FileNumber>60</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1007,7 +1029,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>60</FileNumber>
|
||||
<FileNumber>61</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1019,7 +1041,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>61</FileNumber>
|
||||
<FileNumber>62</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1039,7 +1061,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>62</FileNumber>
|
||||
<FileNumber>63</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1051,7 +1073,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>63</FileNumber>
|
||||
<FileNumber>64</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1063,7 +1085,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>64</FileNumber>
|
||||
<FileNumber>65</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1075,7 +1097,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>65</FileNumber>
|
||||
<FileNumber>66</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1087,7 +1109,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>66</FileNumber>
|
||||
<FileNumber>67</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1099,7 +1121,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>67</FileNumber>
|
||||
<FileNumber>68</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1111,7 +1133,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>68</FileNumber>
|
||||
<FileNumber>69</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1123,7 +1145,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>69</FileNumber>
|
||||
<FileNumber>70</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1135,7 +1157,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>70</FileNumber>
|
||||
<FileNumber>71</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1147,7 +1169,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>71</FileNumber>
|
||||
<FileNumber>72</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1167,7 +1189,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>72</FileNumber>
|
||||
<FileNumber>73</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1179,7 +1201,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>73</FileNumber>
|
||||
<FileNumber>74</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1191,7 +1213,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>74</FileNumber>
|
||||
<FileNumber>75</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1203,7 +1225,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>75</FileNumber>
|
||||
<FileNumber>76</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1215,7 +1237,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>76</FileNumber>
|
||||
<FileNumber>77</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1227,7 +1249,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>77</FileNumber>
|
||||
<FileNumber>78</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1247,7 +1269,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>78</FileNumber>
|
||||
<FileNumber>79</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1259,7 +1281,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>79</FileNumber>
|
||||
<FileNumber>80</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1271,7 +1293,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>80</FileNumber>
|
||||
<FileNumber>81</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1283,7 +1305,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>81</FileNumber>
|
||||
<FileNumber>82</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1295,7 +1317,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>82</FileNumber>
|
||||
<FileNumber>83</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1307,7 +1329,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>83</FileNumber>
|
||||
<FileNumber>84</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1319,7 +1341,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>84</FileNumber>
|
||||
<FileNumber>85</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1339,7 +1361,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>11</GroupNumber>
|
||||
<FileNumber>85</FileNumber>
|
||||
<FileNumber>86</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1351,7 +1373,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>11</GroupNumber>
|
||||
<FileNumber>86</FileNumber>
|
||||
<FileNumber>87</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1363,7 +1385,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>11</GroupNumber>
|
||||
<FileNumber>87</FileNumber>
|
||||
<FileNumber>88</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1383,7 +1405,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>12</GroupNumber>
|
||||
<FileNumber>88</FileNumber>
|
||||
<FileNumber>89</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1403,7 +1425,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>89</FileNumber>
|
||||
<FileNumber>90</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1415,7 +1437,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>90</FileNumber>
|
||||
<FileNumber>91</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1427,7 +1449,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>91</FileNumber>
|
||||
<FileNumber>92</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1439,7 +1461,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>92</FileNumber>
|
||||
<FileNumber>93</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
|
||||
@ -1113,6 +1113,11 @@
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\Module\up.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>up_utils.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\Module\up_utils.c</FilePath>
|
||||
</File>
|
||||
</Files>
|
||||
</Group>
|
||||
<Group>
|
||||
|
||||
Binary file not shown.
@ -28,11 +28,13 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
|
||||
c->motorfeedback.rotor_rpm3508[i] = can->motor.chassis_motor3508.as_array[i].rotor_speed;
|
||||
c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current;
|
||||
}
|
||||
for (uint8_t i = 0; i < 4; i++) {
|
||||
|
||||
c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值
|
||||
}
|
||||
|
||||
// for (uint8_t i = 0; i < 4; i++) {
|
||||
//
|
||||
// c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值
|
||||
// }
|
||||
c->sick_cali.sick_dis[0]=can->sickfed.raw_dis[0] ;
|
||||
c->sick_cali.sick_dis[1]=can->sickfed.raw_dis[1]+50 ; //有点误差，手动补偿
|
||||
c->sick_cali.sick_dis[2]=can->sickfed.raw_dis[2] ;
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
|
||||
@ -45,9 +47,14 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
|
||||
}
|
||||
PID_init(&(c->pid.chassis_PICKWzPID_HIGN), PID_POSITION, &(c->param->chassis_PICKWzPID_HIGN_param));
|
||||
PID_init(&(c->pid.chassis_PICKWzPID_LOW), PID_POSITION, &(c->param->chassis_PICKWzPID_LOW_param));
|
||||
PID_init(&(c->pid.SickCaliWzPID), PID_POSITION, &(c->param->Sick_CaliWparam));
|
||||
PID_init(&(c->pid.SickCaliVxPID), PID_POSITION, &(c->param->Sick_CaliXparam));
|
||||
PID_init(&(c->pid.SickCaliVyPID), PID_POSITION, &(c->param->Sick_CaliYparam));
|
||||
/*sick*/
|
||||
PID_init(&(c->pid.SickCaliWzInPID), PID_POSITION, &(c->param->SickCali_WInparam));
|
||||
PID_init(&(c->pid.SickCaliWzOutPID), PID_POSITION, &(c->param->SickCali_WOutparam));
|
||||
PID_init(&(c->pid.SickCaliVxInPID), PID_POSITION, &(c->param->SickCali_XInparam));
|
||||
PID_init(&(c->pid.SickCaliVxOutPID), PID_POSITION, &(c->param->SickCali_XOutparam));
|
||||
PID_init(&(c->pid.SickCaliVyInPID), PID_POSITION, &(c->param->SickCali_YInparam));
|
||||
PID_init(&(c->pid.SickCaliVyOutPID), PID_POSITION, &(c->param->SickCali_YOutparam));
|
||||
/*修正 */
|
||||
PID_init(&(c->pid.Chassis_AngleAdjust), PID_POSITION, &(c->param->Chassis_AngleAdjust_param));
|
||||
|
||||
LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波
|
||||
@ -119,7 +126,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
|
||||
case AUTO:
|
||||
switch (c->chassis_ctrl.mode) {
|
||||
case AUTO_MID360:
|
||||
c->move_vec.Vw = ctrl->cmd_MID360.posw * 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;
|
||||
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
|
||||
@ -136,7 +143,17 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
|
||||
c->move_vec.Vy = ctrl->Vx * 6000;
|
||||
|
||||
break;
|
||||
case PassBall: //
|
||||
|
||||
default:
|
||||
|
||||
break;
|
||||
}
|
||||
case PASS_BALL:
|
||||
switch (c->chassis_ctrl.mode)
|
||||
{
|
||||
case PB_UP:
|
||||
case PB_MID:
|
||||
case PB_DOWN:
|
||||
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;
|
||||
@ -144,10 +161,13 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
|
||||
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
|
||||
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
|
||||
break ;
|
||||
default:
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
break;
|
||||
break;
|
||||
|
||||
default:
|
||||
@ -169,7 +189,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
|
||||
out->motor_CHASSIS3508.as_array[i] = c->final_out.final_3508out[i];
|
||||
}
|
||||
|
||||
c->vofa_send[0] = (fp32)c->sick_cali.sick_dis[1] - c->sick_cali.sick_dis[2];
|
||||
// c->vofa_send[0] = (fp32)c->sick_cali.sick_dis[1] - c->sick_cali.sick_dis[2];
|
||||
|
||||
return CHASSIS_OK;
|
||||
|
||||
@ -191,8 +211,8 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
|
||||
{
|
||||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||||
if (ctrl == NULL) return CHASSIS_ERR_NULL;
|
||||
|
||||
|
||||
fp32 delta_w,delta_x,delta_y;
|
||||
|
||||
fp32 sick0 = c->sick_cali.sick_dis[0] ;
|
||||
fp32 sick1 = c->sick_cali.sick_dis[1];
|
||||
fp32 sick2 = c->sick_cali.sick_dis[2];
|
||||
@ -202,10 +222,9 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
|
||||
fp32 diff_y = -(fp32)(sick1 - c->sick_cali.sickparam.y_set);
|
||||
fp32 diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set);
|
||||
|
||||
|
||||
c->move_vec.Vw = (fabsf(diff_yaw) > param->w_error) ? PID_calc(&(c->pid.SickCaliWzPID), diff_yaw, 0) : 0;
|
||||
c->move_vec.Vx = (fabsf(diff_x) > param->xy_error) ? PID_calc(&(c->pid.SickCaliVxPID), diff_x, 0) : 0;
|
||||
c->move_vec.Vy = (fabsf(diff_y) > param->xy_error) ? PID_calc(&(c->pid.SickCaliVyPID), diff_y, 0) : 0;
|
||||
delta_w=(&c->pid.SickCaliWzInPID, diff_yaw, 0);
|
||||
delta_x=(&c->pid.SickCaliVxInPID, diff_x, 0);
|
||||
delta_y=(&c->pid.SickCaliVyInPID, diff_y,0);
|
||||
|
||||
static uint16_t reach_cnt = 0;
|
||||
if (fabsf(diff_yaw) <= param->w_error &&
|
||||
|
||||
@ -80,9 +80,13 @@ typedef struct
|
||||
pid_param_t NaviVy_param;
|
||||
pid_param_t NaviVw_param;
|
||||
|
||||
pid_param_t Sick_CaliWparam;
|
||||
pid_param_t Sick_CaliYparam;
|
||||
pid_param_t Sick_CaliXparam;
|
||||
pid_param_t SickCali_WInparam;
|
||||
pid_param_t SickCali_WOutparam;
|
||||
pid_param_t SickCali_YInparam;
|
||||
pid_param_t SickCali_YOutparam;
|
||||
pid_param_t SickCali_XInparam;
|
||||
pid_param_t SickCali_XOutparam;
|
||||
|
||||
pid_param_t Chassis_AngleAdjust_param;
|
||||
|
||||
sickparam_t sickparam;
|
||||
@ -154,12 +158,15 @@ typedef struct{
|
||||
pid_type_def chassis_PICKWzPID_HIGN;
|
||||
/*存在较低误差*/
|
||||
pid_type_def chassis_PICKWzPID_LOW;
|
||||
|
||||
pid_type_def Chassis_AngleAdjust;
|
||||
|
||||
pid_type_def SickCaliWzPID;
|
||||
pid_type_def SickCaliVxPID;
|
||||
pid_type_def SickCaliVyPID;
|
||||
/*sick */
|
||||
pid_type_def SickCaliWzInPID;
|
||||
pid_type_def SickCaliWzOutPID;
|
||||
pid_type_def SickCaliVxInPID;
|
||||
pid_type_def SickCaliVxOutPID;
|
||||
pid_type_def SickCaliVyInPID;
|
||||
pid_type_def SickCaliVyOutPID;
|
||||
|
||||
|
||||
}pid;
|
||||
|
||||
@ -37,18 +37,18 @@ static const ConfigParam_t param ={
|
||||
.out_limit = 3000.0f,
|
||||
},
|
||||
.Pitch_M2006_angle_param = {
|
||||
.p = 600.0f,
|
||||
.p = 1600.0f,
|
||||
.i = 0.0f,
|
||||
.d = 3.0f,
|
||||
.i_limit = 2000.0f,
|
||||
.out_limit = 3000.0f,
|
||||
.i_limit = 4000.0f,
|
||||
.out_limit = 20000.0f,
|
||||
},
|
||||
.Pitch_M2006_speed_param={
|
||||
.p = 5.0f,
|
||||
.i = 0.3f,
|
||||
.p = 8.5f,
|
||||
.i = 0.03f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 2000.0f,
|
||||
.out_limit = 3000.0f,
|
||||
.out_limit = 15000.0f,
|
||||
},
|
||||
.Pitch_Angle_M2006_speed_param={ //俯仰跑速度环
|
||||
.p = 20.0f,
|
||||
@ -71,20 +71,32 @@ static const ConfigParam_t param ={
|
||||
},
|
||||
|
||||
|
||||
/*上层其他参数*/
|
||||
/*上层其他参数,一些只是初始值，在运行过程中会被更改*/
|
||||
|
||||
/*投球*/
|
||||
.PitchCfg = {
|
||||
.m2006_init =-150, //释放的角度
|
||||
.m2006_trig =0, //可以拉住发射的角度
|
||||
.go_init = -50, //仅用在go上电，初始位置
|
||||
.go_pull_pos =-214,
|
||||
.Pitch_angle =58,
|
||||
|
||||
.pull_speed =5,
|
||||
|
||||
.LaunchCfg = {
|
||||
.m2006_init = -150.0f, // M2006初始角度
|
||||
.m2006_trig = 0.0f, // M2006触发角度
|
||||
.go_pull_pos = -214.0f, // go上升去合并扳机，扳机位置
|
||||
.go_up_speed = 12.0f, // 上升速度
|
||||
.go_down_speed = 6.0f, // 下降速度
|
||||
.Pitch_angle = 66, //俯仰角
|
||||
.go_init = -50
|
||||
},
|
||||
.PitchCfg = {
|
||||
|
||||
.go_init = -50, //仅用在go上电，初始位置
|
||||
.go_release_pos=-200,
|
||||
.Pitch_angle =66,
|
||||
|
||||
},
|
||||
.PassCfg={
|
||||
.go_wait =-10,
|
||||
.go_release_pos =-150,
|
||||
.Pitch_angle=66,
|
||||
.go_up_speed =12,
|
||||
.go_down_speed =5,
|
||||
},
|
||||
},
|
||||
.chassis = {/**/
|
||||
|
||||
|
||||
387
User/Module/up.c
387
User/Module/up.c
@ -3,6 +3,8 @@
|
||||
#include "user_math.h"
|
||||
#include "bsp_buzzer.h"
|
||||
#include "bsp_delay.h"
|
||||
|
||||
|
||||
/*接线
|
||||
|
||||
|
||||
@ -41,13 +43,19 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
|
||||
LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f);
|
||||
|
||||
|
||||
if (!u->Pitch_Cfg .is_init) {
|
||||
u->Pitch_Cfg .PitchConfig = u->param ->PitchCfg ;//赋值
|
||||
u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球
|
||||
u->Pitch_Cfg .is_init = 1;
|
||||
if (!u->PitchContext .is_init) {
|
||||
u->PitchContext .PitchConfig = u->param ->PitchCfg ;//赋值
|
||||
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新
|
||||
u->PitchContext .is_init = 1;
|
||||
}
|
||||
|
||||
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
|
||||
if(!u->PassContext.is_init) {
|
||||
u->PassContext .PassCfg = u->param ->PassCfg ;//赋值
|
||||
u->PassContext .PassState = PASS_STOP; //状态更新
|
||||
u->PassContext .is_init = 1;
|
||||
}
|
||||
u->LaunchContext.LaunchCfg = u->param->LaunchCfg;
|
||||
|
||||
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
|
||||
|
||||
}
|
||||
|
||||
@ -80,30 +88,7 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *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，
|
||||
*/
|
||||
@ -128,7 +113,8 @@ 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;
|
||||
|
||||
@ -138,44 +124,6 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
|
||||
|
||||
|
||||
|
||||
/*go电机控制*/
|
||||
|
||||
int8_t GO_SendData(UP_t *u, float pos, float limit)
|
||||
{
|
||||
static int is_calibration=0;
|
||||
static fp32 error=0; //误差量
|
||||
|
||||
u->motorfeedback .go_data = get_GO_measure_point() ;
|
||||
|
||||
|
||||
// 读取参数
|
||||
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
|
||||
@ -183,16 +131,22 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
|
||||
&u->pid .Shoot_M2006angle ,
|
||||
&u->pid .Shoot_M2006speed ,
|
||||
u->motor_target .Shoot_M2006_angle );
|
||||
|
||||
/*俯仰2006，双环，内使用oid角度环，外电机速度环
|
||||
|
||||
*/
|
||||
|
||||
DJ_Speed_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5],&u->pid .Pitch_M2006_speed,
|
||||
(PID_calc (&(u->pid .Pitch_M2006_angle ),
|
||||
u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle))
|
||||
u->motorfeedback .Encoder.angle,u->motor_target .Pitch_angle)+0.14)
|
||||
);
|
||||
|
||||
GO_SendData(
|
||||
&u->motorfeedback.go_data,
|
||||
&u->go_cmd,u->motor_target .go_shoot,
|
||||
u->motor_target.go_pull_speed
|
||||
);
|
||||
|
||||
GO_SendData(u,
|
||||
u->motor_target .go_shoot,
|
||||
u->Pitch_Cfg .PitchConfig .pull_speed );//对应的拉动速度
|
||||
|
||||
|
||||
for(int i=0;i<4;i++){
|
||||
out ->motor_UP3508_Dribble.as_array[i]=u->final_out.DJfinal_out [i] ;
|
||||
}
|
||||
@ -208,156 +162,267 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
|
||||
}
|
||||
|
||||
|
||||
|
||||
fp32 posss=0;
|
||||
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
{
|
||||
|
||||
if(u ==NULL) return 0;
|
||||
if(u ==NULL) return 0;
|
||||
if(out ==NULL) return 0;
|
||||
if(c ==NULL) return 0;
|
||||
|
||||
/*指针简化*/
|
||||
PitchCfg_t *pitch_cfg = &u->Pitch_Cfg.PitchConfig;
|
||||
|
||||
PitchCfg_t *pitch_cfg = &u->PitchContext.PitchConfig;
|
||||
LaunchCfg_t *LaunchCfg =&u->LaunchContext.LaunchCfg;
|
||||
up_motor_target_t *target=&u->motor_target ;
|
||||
static int is_pitch=1;
|
||||
|
||||
|
||||
/*config值限位*/
|
||||
abs_limit_min_max_fp(&u->Pitch_Cfg.PitchConfig.go_release_pos ,-215.0f,0.0f);
|
||||
|
||||
abs_limit_min_max_fp(&u->PitchContext.PitchConfig.go_release_pos ,-215.0f,0.0f);
|
||||
abs_limit_min_max_fp(&u->PitchContext.PitchConfig.Pitch_angle ,48.0f,67.0f);
|
||||
|
||||
/*部分数据更新*/
|
||||
static int is_pitch=1;
|
||||
posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PitchContext.Curve);
|
||||
// if (u->PitchContext.Curve == CURVE_58) {
|
||||
// target->Pitch_angle = 58;
|
||||
// } else {
|
||||
// target->Pitch_angle = 66;
|
||||
// }
|
||||
|
||||
switch (c->CMD_CtrlType )
|
||||
{
|
||||
case RCcontrol: //在手动模式下
|
||||
|
||||
|
||||
|
||||
switch (c-> CMD_mode )
|
||||
{
|
||||
|
||||
case Normal :
|
||||
case Normal : //复位，go位置和俯仰角保持LaunchCfg最后修改后的位置，扳机2006角度复位用于发射
|
||||
/*投篮*/
|
||||
if(is_pitch){
|
||||
target->go_shoot =pitch_cfg->go_init ;
|
||||
target->Shoot_Pitch_angle=pitch_cfg->Pitch_angle;
|
||||
target->go_shoot = LaunchCfg->go_init ;
|
||||
target->Pitch_angle = LaunchCfg->Pitch_angle;
|
||||
is_pitch=0;
|
||||
} //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删
|
||||
target->Shoot_M2006_angle =u->Pitch_Cfg .PitchConfig .m2006_init ;
|
||||
u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球
|
||||
|
||||
} //初始上电
|
||||
LaunchCfg->go_up_speed=15;
|
||||
target->Pitch_angle =LaunchCfg->Pitch_angle;
|
||||
|
||||
|
||||
target->Shoot_M2006_angle=LaunchCfg->m2006_init;
|
||||
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新
|
||||
u->LaunchContext.LaunchState = Launch_Stop;
|
||||
break;
|
||||
|
||||
case Pitch :
|
||||
if (u->Pitch_Cfg .PitchState ==PITCH_PREPARE)
|
||||
if (u->PitchContext .PitchState ==PITCH_PREPARE)
|
||||
{
|
||||
u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
|
||||
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
|
||||
}
|
||||
|
||||
Pitch_Process(u,out);
|
||||
Pitch_Process(u,out,c);
|
||||
|
||||
|
||||
break ;
|
||||
case UP_Adjust: //测试用
|
||||
pitch_cfg ->pull_speed=5;
|
||||
case UP_Adjust: //测试用,手动使用pitch下的cfg
|
||||
LaunchCfg->go_up_speed=5;
|
||||
pitch_cfg ->go_release_pos += c->Vx ;
|
||||
pitch_cfg->Pitch_angle += c->Vy/100;
|
||||
LaunchCfg->Pitch_angle += c->Vy/100;
|
||||
|
||||
target->go_shoot=u->Pitch_Cfg.PitchConfig.go_release_pos;
|
||||
target->Shoot_Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle;
|
||||
target->go_shoot=pitch_cfg ->go_release_pos;
|
||||
target->Pitch_angle=LaunchCfg->Pitch_angle;
|
||||
break ;
|
||||
|
||||
|
||||
default:
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case AUTO:
|
||||
/*自动下数据更新，根据距离切换俯仰角,如需自动调整，放在*/
|
||||
// if (u->PitchContext.Curve == CURVE_58) {
|
||||
// target->Pitch_angle = 58;
|
||||
// } else {
|
||||
// target->Pitch_angle = 66;
|
||||
// }
|
||||
|
||||
switch(c-> CMD_mode)
|
||||
{
|
||||
case AUTO_MID360_Pitch:
|
||||
pitch_cfg ->go_init=LowPassFilter2p_Apply(&u->filled[0],c->pos);
|
||||
|
||||
if (u->Pitch_Cfg .PitchState ==PITCH_PREPARE)
|
||||
if (u->PitchContext .PitchState ==PITCH_PREPARE || u->PitchContext .PitchState ==PITCH_COMPLETE)
|
||||
{
|
||||
u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
|
||||
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
|
||||
}
|
||||
/*根据距离实时计算所需pos*/
|
||||
pitch_cfg ->go_release_pos=c->pos;
|
||||
|
||||
|
||||
Pitch_Process(u,out);
|
||||
/*根据距离实时计算所需pos*/
|
||||
if(c->pos){
|
||||
pitch_cfg ->go_release_pos=
|
||||
CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PitchContext.Curve);
|
||||
}
|
||||
else pitch_cfg ->go_release_pos=0 ;
|
||||
Pitch_Process(u,out,c);
|
||||
break ;
|
||||
|
||||
case AUTO_MID360:
|
||||
target->Shoot_M2006_angle =pitch_cfg ->m2006_init ;
|
||||
target->Shoot_M2006_angle =LaunchCfg ->m2006_init ;
|
||||
|
||||
u->Pitch_Cfg .PitchState = PITCH_PREPARE;
|
||||
|
||||
|
||||
break ;
|
||||
default:
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
break ;
|
||||
case PASS_BALL:
|
||||
// if (u->PitchContext.Curve == CURVE_58) {
|
||||
// target->Pitch_angle = 58;
|
||||
// } else {
|
||||
// target->Pitch_angle = 66;
|
||||
// }
|
||||
Pass_Process(u,out,c);
|
||||
break;
|
||||
case RELAXED:
|
||||
// target->go_shoot= pitch_cfg .
|
||||
target->Pitch_angle = 58;
|
||||
target->go_shoot = -210.0f;
|
||||
// target->Pitch_angle = 58;
|
||||
|
||||
|
||||
break ;
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
|
||||
{
|
||||
//复用发射，
|
||||
int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){
|
||||
|
||||
PitchCfg_t *cfg = &u->Pitch_Cfg.PitchConfig;
|
||||
PitchState_t *state =&u->Pitch_Cfg .PitchState;
|
||||
up_motor_target_t *target=&u->motor_target ;
|
||||
|
||||
switch(*state){
|
||||
|
||||
case PITCH_START:
|
||||
// u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动
|
||||
cfg->pull_speed=10;
|
||||
target->go_shoot = cfg->go_pull_pos;
|
||||
switch(LaunchContext->LaunchState){
|
||||
case Launch_Stop: break;
|
||||
case Launch_PREPARE:
|
||||
u->motor_target.go_shoot = StartPos;
|
||||
if(is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f)&&
|
||||
is_reached(u->motorfeedback.go_data.W,0,1.0f)){
|
||||
//根据位置和速度判断是否到达初始位置
|
||||
LaunchContext->LaunchState = Launch_START;
|
||||
|
||||
}break;
|
||||
|
||||
case Launch_START:
|
||||
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_up_speed;
|
||||
u->motor_target.go_shoot = u->LaunchContext.LaunchCfg.go_pull_pos;
|
||||
if(u->motorfeedback .go_data .Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
|
||||
u->motor_target.Shoot_M2006_angle = u->LaunchContext.LaunchCfg.m2006_trig ;//设置2006角度，关闭
|
||||
|
||||
LaunchContext->LaunchState = Launch_TRIGGER;
|
||||
}break;
|
||||
|
||||
if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
|
||||
target->Shoot_M2006_angle = cfg->m2006_trig ;//设置2006角度，0
|
||||
u->Pitch_Cfg .PitchConfig .pull_speed=6;
|
||||
*state=PITCH_PULL_TRIGGER;
|
||||
}//更改标志位
|
||||
|
||||
break ;
|
||||
case PITCH_PULL_TRIGGER:
|
||||
case Launch_TRIGGER:
|
||||
|
||||
if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1){ //当2006的总角度小于1，可以认为已经勾上,误差为1
|
||||
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_down_speed;
|
||||
u->motor_target.go_shoot = EndPos ;
|
||||
if(is_reached(u->motorfeedback.go_data.Pos,EndPos,1.0f))
|
||||
LaunchContext->LaunchState = Launch_SHOOT_WAIT;
|
||||
} break;
|
||||
|
||||
if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1) //当2006的总角度小于1，可以认为已经勾上,误差为1
|
||||
{
|
||||
target->go_shoot=cfg->go_release_pos;
|
||||
if(is_reached (u->motorfeedback .go_data ->Pos ,target->go_shoot ,1.0f))
|
||||
{
|
||||
*state=PITCH_LAUNCHING;
|
||||
}
|
||||
}
|
||||
case Launch_SHOOT_WAIT:
|
||||
if(u->motorfeedback.DJmotor_feedback[4].total_angle<-1) //认为发射
|
||||
LaunchContext->LaunchState = Launch_DONE;
|
||||
break;
|
||||
case Launch_DONE:
|
||||
break ;
|
||||
|
||||
case PITCH_LAUNCHING: //等待发射
|
||||
// *state=PITCH_COMPLETE;
|
||||
break ;
|
||||
case PITCH_COMPLETE: //发射完成
|
||||
|
||||
|
||||
break ;
|
||||
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
|
||||
{
|
||||
/*简化*/
|
||||
PitchCfg_t *cfg = &u->PitchContext.PitchConfig;
|
||||
PitchState_t *state =&u->PitchContext .PitchState;
|
||||
up_motor_target_t *target=&u->motor_target ;
|
||||
LaunchContext_t *LaunchContext = &u->LaunchContext;
|
||||
// 可根据实际需要传入不同的起始和末位置,起始：当前位置
|
||||
Pitch_Launch_Sequence(u, LaunchContext, u->motorfeedback.go_data.Pos, cfg->go_release_pos, out);
|
||||
|
||||
|
||||
switch(*state){
|
||||
|
||||
case PITCH_START:
|
||||
LaunchContext->LaunchState = Launch_PREPARE;
|
||||
|
||||
*state=PITCH_WAIT;
|
||||
break;
|
||||
|
||||
case PITCH_WAIT:
|
||||
|
||||
if(LaunchContext->LaunchState==Launch_DONE)
|
||||
*state=PITCH_COMPLETE;
|
||||
break;
|
||||
case PITCH_COMPLETE:
|
||||
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
{
|
||||
|
||||
PassCfg_t *PassCfg = &u->PassContext.PassCfg;
|
||||
PassState_t *state = &u->PassContext.PassState;
|
||||
up_motor_target_t *target=&u->motor_target ;
|
||||
LaunchContext_t *LaunchContext = &u->LaunchContext;
|
||||
|
||||
Pass_Sequence_Check(u,c);
|
||||
|
||||
if(c->pos) //
|
||||
{
|
||||
PassCfg ->go_release_pos =
|
||||
CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PitchContext.Curve);
|
||||
|
||||
}
|
||||
switch (*state) { //遥控器按键进行状态切换
|
||||
case PASS_STOP:
|
||||
|
||||
break;
|
||||
case PASS_IDLE:
|
||||
LaunchContext->LaunchState = Launch_PREPARE;
|
||||
break;
|
||||
case PASS_PREPARE:
|
||||
target->go_pull_speed=PassCfg->go_up_speed;
|
||||
|
||||
Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,PassCfg->go_wait,out);
|
||||
|
||||
|
||||
break;
|
||||
case PASS_START:
|
||||
if(LaunchContext->LaunchState == Launch_SHOOT_WAIT){
|
||||
target->go_pull_speed=PassCfg->go_down_speed;
|
||||
target->go_shoot = PassCfg->go_release_pos ;
|
||||
}
|
||||
break ;
|
||||
case PASS_POS_PREPARE:
|
||||
target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;//发射
|
||||
break;
|
||||
case PASS_COMPLETE:
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
115
User/Module/up.h
115
User/Module/up.h
@ -13,6 +13,7 @@
|
||||
#include "vofa.h"
|
||||
#include "GO_M8010_6_Driver.h"
|
||||
#include "bsp_usart.h"
|
||||
#include "up_utils.h"
|
||||
|
||||
/*
|
||||
状态机
|
||||
@ -27,27 +28,50 @@
|
||||
|
||||
|
||||
*/
|
||||
/*共用发射，重复部分*/
|
||||
typedef struct {
|
||||
fp32 m2006_init; // M2006初始角度
|
||||
fp32 m2006_trig; // M2006触发角度0,用来合并扳机
|
||||
fp32 go_init;
|
||||
fp32 go_pull_pos; // go上升去合并扳机，扳机位置
|
||||
fp32 go_up_speed;
|
||||
fp32 go_down_speed;
|
||||
|
||||
fp32 Pitch_angle;
|
||||
} LaunchCfg_t;
|
||||
typedef enum {
|
||||
Launch_Stop,
|
||||
Launch_PREPARE, //
|
||||
Launch_START, //启动，从指定位置上升，扣动扳机
|
||||
Launch_TRIGGER, //拉动到指定位置
|
||||
Launch_SHOOT_WAIT, // 发射等待
|
||||
Launch_SHOOT,
|
||||
Launch_DONE,
|
||||
|
||||
|
||||
}LaunchState_t;
|
||||
typedef struct {
|
||||
LaunchCfg_t LaunchCfg;
|
||||
LaunchState_t LaunchState;
|
||||
} LaunchContext_t;
|
||||
|
||||
|
||||
/* 投球状态定义 */
|
||||
typedef enum {
|
||||
|
||||
PITCH_PREPARE, // 准备阶段
|
||||
PITCH_START, //启动，拉扳机
|
||||
PITCH_PULL_TRIGGER,
|
||||
PITCH_LAUNCHING, // 发射中
|
||||
|
||||
|
||||
PITCH_WAIT, // 发射等待
|
||||
PITCH_COMPLETE // 完成
|
||||
|
||||
} PitchState_t;
|
||||
|
||||
/* 投球参数配置 */
|
||||
typedef struct {
|
||||
fp32 m2006_init; // M2006初始角度
|
||||
fp32 m2006_trig; // M2006触发角度0,用来合并扳机
|
||||
fp32 go_init; // GO电机触发位置,初始位置，后续更改用于发射的位置
|
||||
fp32 go_pull_pos; // go上升去合并扳机，扳机位置
|
||||
fp32 Pitch_angle;
|
||||
|
||||
fp32 pull_speed;//go速度
|
||||
|
||||
fp32 go_init; // GO电机触发位置,初始位置，后续更改用于发射的位置
|
||||
fp32 Pitch_angle; //俯仰角度，以oid为准
|
||||
fp32 go_release_pos;//go松开，发射位置
|
||||
|
||||
} PitchCfg_t;
|
||||
@ -55,19 +79,34 @@ typedef struct {
|
||||
typedef struct {
|
||||
PitchState_t PitchState;
|
||||
PitchCfg_t PitchConfig;
|
||||
|
||||
CurveType Curve; //当前函数，雷达距离->pos
|
||||
uint8_t is_init ;
|
||||
} Pitch_Cfg_t;
|
||||
} PitchContext_t;
|
||||
|
||||
/*运球*/
|
||||
/*传球过程 */
|
||||
typedef enum {
|
||||
DRIBBLE_PREPARE, // 准备阶段
|
||||
DRIBBLE_START,
|
||||
DRIBBLE_TRANSLATE, // 平移过程
|
||||
DRIBBLE_PROCESS_DOWN, // 运球过程，球下落
|
||||
DRIBBLE_PROCESS_UP, // 运球过程，球上升
|
||||
DRIBBLE_DONE // 运球结束
|
||||
} DribbleState_t;
|
||||
PASS_STOP, // 停止状态,未进入传球模式
|
||||
PASS_IDLE, // 空闲状态，进入未开始
|
||||
PASS_PREPARE, // 传球准备
|
||||
PASS_START, // 启动传球,拉动到等球位置
|
||||
PASS_POS_PREPARE, // 传球位置准备，对准篮筐，go的位置
|
||||
PASS_COMPLETE // 发射
|
||||
} PassState_t;
|
||||
typedef struct {
|
||||
|
||||
fp32 go_wait; // GO等待位置
|
||||
fp32 go_release_pos; // GO电机发射位置
|
||||
fp32 Pitch_angle; // 俯仰角度，以oid为准
|
||||
fp32 go_up_speed; // GO电机上升速度
|
||||
fp32 go_down_speed; // GO电机下降速度
|
||||
|
||||
} PassCfg_t;
|
||||
typedef struct {
|
||||
PassState_t PassState;
|
||||
PassCfg_t PassCfg;
|
||||
CurveType Curve; //当前函数，雷达距离->pos
|
||||
uint8_t is_init ;
|
||||
} PassContext_t;
|
||||
|
||||
typedef struct {
|
||||
|
||||
@ -91,7 +130,10 @@ typedef struct
|
||||
|
||||
pid_param_t Pitch_Angle_M2006_speed_param;
|
||||
|
||||
LaunchCfg_t LaunchCfg;
|
||||
PitchCfg_t PitchCfg;
|
||||
PassCfg_t PassCfg;
|
||||
|
||||
|
||||
GO_MotorCmd_t go_cmd;
|
||||
}UP_Param_t;
|
||||
@ -101,11 +143,10 @@ typedef struct{
|
||||
fp32 VESC_5065_M2_rpm;
|
||||
|
||||
fp32 Shoot_M2006_angle;
|
||||
fp32 Pitch_M2006_angle;
|
||||
|
||||
fp32 go_shoot;
|
||||
|
||||
fp32 Shoot_Pitch_angle;
|
||||
fp32 go_pull_speed;
|
||||
fp32 Pitch_angle; //以oid的角度为准,目标俯仰角
|
||||
|
||||
}up_motor_target_t;
|
||||
|
||||
@ -125,26 +166,16 @@ typedef struct{
|
||||
|
||||
|
||||
}up_pid_t;
|
||||
typedef struct
|
||||
{
|
||||
fp32 ecd;
|
||||
fp32 rpm;
|
||||
uint8_t id;
|
||||
fp32 orig_angle;
|
||||
fp32 last_angle;
|
||||
int32_t round_cnt;
|
||||
int init_cnt;
|
||||
fp32 total_angle;
|
||||
}DJmotor_feedback_t;
|
||||
|
||||
|
||||
typedef struct{
|
||||
|
||||
uint8_t up_task_run;
|
||||
const UP_Param_t *param;
|
||||
const UP_Param_t *param;
|
||||
LaunchContext_t LaunchContext;
|
||||
/*投篮过程*/
|
||||
Pitch_Cfg_t Pitch_Cfg;
|
||||
|
||||
PitchContext_t PitchContext;
|
||||
/*传球过程*/
|
||||
PassContext_t PassContext;
|
||||
CMD_t *cmd;
|
||||
|
||||
struct{
|
||||
@ -153,7 +184,7 @@ typedef struct{
|
||||
fp32 VESC_5065_M1_rpm;
|
||||
fp32 VESC_5065_M2_rpm;
|
||||
}VESC;
|
||||
GO_MotorData_t *go_data;//存放go电机数据
|
||||
GO_MotorData_t go_data;//存放go电机数据
|
||||
|
||||
DJmotor_feedback_t DJmotor_feedback[6];
|
||||
|
||||
@ -191,14 +222,16 @@ typedef struct{
|
||||
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);
|
||||
int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed);
|
||||
|
||||
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out);
|
||||
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c);
|
||||
int8_t Pass_Sequence_Check(UP_t *u,CMD_t *c);
|
||||
int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c);
|
||||
int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out);
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
160
User/Module/up_utils.c
Normal file
160
User/Module/up_utils.c
Normal file
@ -0,0 +1,160 @@
|
||||
#include "up_utils.h"
|
||||
#include "up.h"
|
||||
|
||||
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;
|
||||
|
||||
}
|
||||
/*go电机控制*/
|
||||
|
||||
int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, float limit)
|
||||
{
|
||||
|
||||
*go_data = *get_GO_measure_point() ;
|
||||
|
||||
|
||||
// 读取参数
|
||||
float tff = go_cmd->T; // 前馈力矩
|
||||
float kp = go_cmd->K_P; // 位置刚度
|
||||
float kd = go_cmd->K_W; // 速度阻尼
|
||||
float q_desired = go_cmd->Pos; // 期望位置（rad）
|
||||
float q_current = go_data->Pos; // 当前角度位置（rad）
|
||||
float dq_desired = go_cmd->W; // 期望角速度（rad/s）
|
||||
float dq_current = go_data->W; // 当前角速度（rad/s）
|
||||
|
||||
// 计算输出力矩 tau
|
||||
float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);
|
||||
|
||||
/*限制最大输入来限制最大输出*/
|
||||
if (pos - q_current > limit) {
|
||||
go_cmd->Pos = q_current + limit; // 限制位置
|
||||
}else if (pos - q_current < -limit) {
|
||||
go_cmd->Pos = q_current - limit; // 限制位置
|
||||
}else {
|
||||
go_cmd->Pos = pos; // 允许位置
|
||||
}
|
||||
|
||||
// 发送数据
|
||||
GO_M8010_send_data(go_cmd);
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
// 计算58度曲线
|
||||
static float curve_58(float d) {
|
||||
return 0.448f * d * d + 24.8604f * d - 177.99552f;
|
||||
}
|
||||
|
||||
// 计算66度曲线
|
||||
static float curve_66(float d) {
|
||||
return 8.84935f * d * d - 10.5424f * d - 126.791f;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
曲线切换,用于距离和pos拟合
|
||||
迟滞区x-y
|
||||
曲线x重合区，根据当前函数和变化方向切换
|
||||
*/
|
||||
float CurveChange(float d, float x, float y, CurveType *cs)
|
||||
{
|
||||
|
||||
if (*cs == CURVE_66) {
|
||||
if (d > y) {
|
||||
*cs = CURVE_58;
|
||||
}
|
||||
} else { // CURVE_58
|
||||
if (d < x) {
|
||||
*cs = CURVE_66;
|
||||
}
|
||||
}
|
||||
|
||||
// 根据当前曲线返回结果
|
||||
if (*cs == CURVE_58) {
|
||||
return curve_58(d);
|
||||
} else {
|
||||
return curve_66(d);
|
||||
}
|
||||
}
|
||||
|
||||
int8_t Pass_Sequence_Check(UP_t *u, CMD_t *c) //按键顺序检测，传球，按需求改
|
||||
{
|
||||
PassState_t *state = &u->PassContext.PassState;
|
||||
static enum {
|
||||
SEQ_IDLE,
|
||||
SEQ_MID1,
|
||||
SEQ_UP,
|
||||
SEQ_MID2,
|
||||
SEQ_DOWN
|
||||
} seq = SEQ_IDLE;
|
||||
|
||||
switch (seq) {
|
||||
case SEQ_IDLE:
|
||||
if (c->CMD_mode == PB_MID) {
|
||||
seq = SEQ_MID1;
|
||||
*state = PASS_IDLE;
|
||||
}
|
||||
break;
|
||||
case SEQ_MID1:
|
||||
if (c->CMD_mode == PB_UP) {
|
||||
seq = SEQ_UP;
|
||||
*state = PASS_PREPARE;
|
||||
} else if (c->CMD_mode != PB_MID) {
|
||||
seq = SEQ_IDLE;
|
||||
*state = PASS_STOP;
|
||||
}
|
||||
break;
|
||||
case SEQ_UP:
|
||||
if (c->CMD_mode == PB_MID) {
|
||||
seq = SEQ_MID2;
|
||||
*state = PASS_START;
|
||||
} else if (c->CMD_mode != PB_UP) {
|
||||
seq = SEQ_IDLE;
|
||||
*state = PASS_STOP;
|
||||
}
|
||||
break;
|
||||
case SEQ_MID2:
|
||||
if (c->CMD_mode == PB_DOWN) {
|
||||
seq = SEQ_DOWN;
|
||||
*state = PASS_POS_PREPARE;
|
||||
} else if (c->CMD_mode != PB_MID) {
|
||||
seq = SEQ_IDLE;
|
||||
*state = PASS_STOP;
|
||||
}
|
||||
break;
|
||||
case SEQ_DOWN:
|
||||
if (c->CMD_mode == PB_MID) {
|
||||
seq = SEQ_IDLE;
|
||||
*state = PASS_IDLE; // 或 PASS_COMPLETE，看你需求
|
||||
} else if (c->CMD_mode != PB_DOWN) {
|
||||
seq = SEQ_IDLE;
|
||||
*state = PASS_STOP;
|
||||
}
|
||||
break;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
56
User/Module/up_utils.h
Normal file
56
User/Module/up_utils.h
Normal file
@ -0,0 +1,56 @@
|
||||
#ifndef UP_UTILS_H
|
||||
#define UP_UTILS_H
|
||||
|
||||
#include "gpio.h"
|
||||
#include "user_math.h"
|
||||
#include "bsp_buzzer.h"
|
||||
#include "bsp_delay.h"
|
||||
#include "struct_typedef.h"
|
||||
#include "pid.h"
|
||||
#include "user_math.h"
|
||||
#include "ahrs.h"
|
||||
#include "filter.h"
|
||||
#include "bsp_usart.h"
|
||||
#include "GO_M8010_6_Driver.h"
|
||||
|
||||
|
||||
#define CAN2_G3508_ID (0x200)
|
||||
#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编码值转轴角度
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
fp32 ecd;
|
||||
fp32 rpm;
|
||||
uint8_t id;
|
||||
fp32 orig_angle;
|
||||
fp32 last_angle;
|
||||
int32_t round_cnt;
|
||||
int init_cnt;
|
||||
fp32 total_angle;
|
||||
}DJmotor_feedback_t;
|
||||
|
||||
|
||||
typedef enum {
|
||||
CURVE_58,
|
||||
CURVE_66
|
||||
} CurveType;
|
||||
|
||||
|
||||
static float curve_58(float d) ;
|
||||
static float curve_66(float d) ;
|
||||
float CurveChange(float d, float x, float y, CurveType *cs);
|
||||
|
||||
int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, float limit);
|
||||
|
||||
int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle);
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@ -164,10 +164,10 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
|
||||
}
|
||||
else if(rc->dr16.sw_l==CMD_SW_DOWN)
|
||||
{
|
||||
cmd ->CMD_CtrlType =RCcontrol;
|
||||
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，无模式
|
||||
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左下，右中，无模式
|
||||
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式
|
||||
cmd ->CMD_CtrlType =PASS_BALL;
|
||||
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =PB_UP; //左下，右上，无模式
|
||||
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode = PB_MID; //左下，右中，无模式
|
||||
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =PB_DOWN; //左下，右上，无模式
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -14,6 +14,7 @@
|
||||
typedef enum{
|
||||
RCcontrol,//遥控器控制，左按键上，控制上层
|
||||
AUTO,
|
||||
PASS_BALL,//传球模式
|
||||
RELAXED,//异常模式
|
||||
|
||||
}CMD_CtrlType_t;
|
||||
@ -29,11 +30,13 @@ typedef enum{
|
||||
|
||||
UP_Adjust,//上层调整
|
||||
Chassis_Adjust_Sick,
|
||||
|
||||
PassBall,
|
||||
/*传球占三个,逻辑在up.h里，简化就这样写了*/
|
||||
PB_UP,
|
||||
PB_MID,
|
||||
PB_DOWN,
|
||||
|
||||
|
||||
Pitch, //投篮,底盘锁定
|
||||
Pitch,
|
||||
|
||||
|
||||
Dribbl_transfer
|
||||
|
||||
@ -62,7 +62,8 @@ int8_t NUC_Restart(void) {
|
||||
return DEVICE_OK;
|
||||
}
|
||||
bool_t NUC_WaitDmaCplt(void) {
|
||||
return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,100) ==
|
||||
|
||||
return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,50) ==
|
||||
SIGNAL_NUC_RAW_REDY);
|
||||
}
|
||||
|
||||
@ -169,7 +170,7 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
|
||||
int8_t NUC_HandleOffline(CMD_NUC_t *cmd)
|
||||
{
|
||||
if(cmd == NULL) return DEVICE_ERR_NULL;
|
||||
memset(cmd, 0, sizeof(*cmd));
|
||||
// memset(cmd, 0, sizeof(*cmd));
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@ -59,7 +59,7 @@ void vofa_tx_main(float *data)
|
||||
|
||||
/*在下面使用对应的串口发送函数*/
|
||||
// HAL_UART_Transmit_DMA(&huart6, packet, sizeof(packet));
|
||||
// CDC_Transmit_FS( packet, sizeof(packet));
|
||||
// osDelay(1);
|
||||
CDC_Transmit_FS( packet, sizeof(packet));
|
||||
osDelay(1);
|
||||
|
||||
}
|
||||
@ -98,7 +98,7 @@ void Task_Chassis(void *argument)
|
||||
vofa_send[6] = chassis.vofa_send[6];
|
||||
vofa_send[7] = chassis.vofa_send[7];
|
||||
|
||||
vofa_tx_main(vofa_send);
|
||||
// vofa_tx_main(vofa_send);
|
||||
tick += delay_tick;
|
||||
osDelayUntil(tick);
|
||||
|
||||
|
||||
@ -36,7 +36,7 @@ void Task_nuc(void *argument){
|
||||
a++;
|
||||
NUC_StartReceiving();
|
||||
NUC_StartSending(NUC_send.send);
|
||||
// NUC_StartSending (send);
|
||||
NUC_StartSending (send);
|
||||
if (NUC_WaitDmaCplt()){
|
||||
NUC_RawParse(&cmd_fromnuc);
|
||||
}
|
||||
|
||||
@ -91,8 +91,11 @@ void Task_up(void *argument)
|
||||
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_send [0]=UP.vofa_send [0];
|
||||
vofa_send [1]=UP.vofa_send [1];
|
||||
vofa_send [2]=UP.vofa_send [2];
|
||||
vofa_send [3]=UP.vofa_send [3];
|
||||
vofa_send [4]=UP.vofa_send [4];
|
||||
vofa_tx_main (vofa_send);
|
||||
|
||||
tick += delay_tick;
|
||||
|
||||
@ -18,7 +18,7 @@
|
||||
#define TASK_FREQ_UP (900u)
|
||||
|
||||
#define TASK_FREQ_CTRL_CMD (500u)
|
||||
#define TASK_FREQ_NUC (500u)
|
||||
#define TASK_FREQ_NUC (200u)
|
||||
#define TASK_FREQ_CAN (1000u)
|
||||
#define TASK_FREQ_RC (1000u)
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user