up_utils
This commit is contained in:
parent
4859c69fad
commit
d0efaf8dab
@ -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,32 @@
|
||||
<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>a,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>7</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>posss</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
@ -877,6 +882,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 +904,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 +916,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>51</FileNumber>
|
||||
<FileNumber>52</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -911,7 +928,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>52</FileNumber>
|
||||
<FileNumber>53</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -923,7 +940,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>53</FileNumber>
|
||||
<FileNumber>54</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -935,7 +952,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>54</FileNumber>
|
||||
<FileNumber>55</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -947,7 +964,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>55</FileNumber>
|
||||
<FileNumber>56</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -959,7 +976,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>56</FileNumber>
|
||||
<FileNumber>57</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -971,7 +988,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>57</FileNumber>
|
||||
<FileNumber>58</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -983,7 +1000,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>58</FileNumber>
|
||||
<FileNumber>59</FileNumber>
|
||||
<FileType>5</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -995,7 +1012,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>59</FileNumber>
|
||||
<FileNumber>60</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1007,7 +1024,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>60</FileNumber>
|
||||
<FileNumber>61</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1019,7 +1036,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>61</FileNumber>
|
||||
<FileNumber>62</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1039,7 +1056,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 +1068,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>63</FileNumber>
|
||||
<FileNumber>64</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1063,7 +1080,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>64</FileNumber>
|
||||
<FileNumber>65</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1075,7 +1092,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>65</FileNumber>
|
||||
<FileNumber>66</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1087,7 +1104,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>66</FileNumber>
|
||||
<FileNumber>67</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1099,7 +1116,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>67</FileNumber>
|
||||
<FileNumber>68</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1111,7 +1128,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>68</FileNumber>
|
||||
<FileNumber>69</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1123,7 +1140,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>69</FileNumber>
|
||||
<FileNumber>70</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1135,7 +1152,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>70</FileNumber>
|
||||
<FileNumber>71</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1147,7 +1164,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>71</FileNumber>
|
||||
<FileNumber>72</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1167,7 +1184,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 +1196,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>73</FileNumber>
|
||||
<FileNumber>74</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1191,7 +1208,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>74</FileNumber>
|
||||
<FileNumber>75</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1203,7 +1220,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>75</FileNumber>
|
||||
<FileNumber>76</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1215,7 +1232,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>76</FileNumber>
|
||||
<FileNumber>77</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1227,7 +1244,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>77</FileNumber>
|
||||
<FileNumber>78</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1247,7 +1264,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 +1276,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>79</FileNumber>
|
||||
<FileNumber>80</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1271,7 +1288,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>80</FileNumber>
|
||||
<FileNumber>81</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1283,7 +1300,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>81</FileNumber>
|
||||
<FileNumber>82</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1295,7 +1312,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>82</FileNumber>
|
||||
<FileNumber>83</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1307,7 +1324,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>83</FileNumber>
|
||||
<FileNumber>84</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1319,7 +1336,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>84</FileNumber>
|
||||
<FileNumber>85</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1339,7 +1356,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 +1368,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>11</GroupNumber>
|
||||
<FileNumber>86</FileNumber>
|
||||
<FileNumber>87</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1363,7 +1380,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>11</GroupNumber>
|
||||
<FileNumber>87</FileNumber>
|
||||
<FileNumber>88</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1383,7 +1400,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 +1420,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 +1432,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>90</FileNumber>
|
||||
<FileNumber>91</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1427,7 +1444,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>91</FileNumber>
|
||||
<FileNumber>92</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1439,7 +1456,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;
|
||||
}
|
||||
|
||||
|
||||
129
User/Module/up.c
129
User/Module/up.c
@ -3,6 +3,8 @@
|
||||
#include "user_math.h"
|
||||
#include "bsp_buzzer.h"
|
||||
#include "bsp_delay.h"
|
||||
|
||||
|
||||
/*接线
|
||||
|
||||
|
||||
@ -80,30 +82,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，
|
||||
*/
|
||||
@ -129,6 +108,7 @@ 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,43 +118,7 @@ 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)
|
||||
{
|
||||
@ -183,15 +127,20 @@ 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))
|
||||
);
|
||||
|
||||
GO_SendData(u,
|
||||
u->motor_target .go_shoot,
|
||||
u->Pitch_Cfg .PitchConfig .pull_speed );//对应的拉动速度
|
||||
|
||||
GO_SendData(
|
||||
&u->motorfeedback.go_data,
|
||||
&u->go_cmd,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,7 +157,7 @@ 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)
|
||||
{
|
||||
|
||||
@ -218,13 +167,20 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
|
||||
/*指针简化*/
|
||||
PitchCfg_t *pitch_cfg = &u->Pitch_Cfg.PitchConfig;
|
||||
|
||||
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->Pitch_Cfg.PitchConfig.Pitch_angle ,48.0f,67.0f);
|
||||
|
||||
/*部分数据更新*/
|
||||
static int is_pitch=1;
|
||||
posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->Pitch_Cfg.Curve);
|
||||
// if (u->Pitch_Cfg.Curve == CURVE_58) {
|
||||
// target->Pitch_angle = 58;
|
||||
// } else {
|
||||
// target->Pitch_angle = 66;
|
||||
// }
|
||||
|
||||
switch (c->CMD_CtrlType )
|
||||
{
|
||||
@ -236,10 +192,11 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
case Normal :
|
||||
/*投篮*/
|
||||
if(is_pitch){
|
||||
target->go_shoot =pitch_cfg->go_init ;
|
||||
target->Shoot_Pitch_angle=pitch_cfg->Pitch_angle;
|
||||
target->Shoot_M2006_angle = pitch_cfg->go_init ;
|
||||
target->Pitch_angle = pitch_cfg->Pitch_angle;
|
||||
is_pitch=0;
|
||||
} //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删
|
||||
|
||||
target->Shoot_M2006_angle =u->Pitch_Cfg .PitchConfig .m2006_init ;
|
||||
u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球
|
||||
|
||||
@ -261,26 +218,31 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
pitch_cfg->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->Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle;
|
||||
break ;
|
||||
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case AUTO:
|
||||
/*自动下数据更新，根据距离切换俯仰角,如需自动调整，放在*/
|
||||
if (u->Pitch_Cfg.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)
|
||||
{
|
||||
u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
|
||||
}
|
||||
/*根据距离实时计算所需pos*/
|
||||
pitch_cfg ->go_release_pos=c->pos;
|
||||
|
||||
pitch_cfg ->go_release_pos=
|
||||
CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->Pitch_Cfg.Curve);
|
||||
|
||||
Pitch_Process(u,out);
|
||||
break ;
|
||||
@ -294,13 +256,20 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
|
||||
}
|
||||
|
||||
break ;
|
||||
|
||||
return 0;
|
||||
case RELAXED:
|
||||
// target->go_shoot= pitch_cfg .
|
||||
|
||||
target->go_shoot = pitch_cfg->go_init;
|
||||
target->Pitch_angle = 58;
|
||||
target->Shoot_M2006_angle =pitch_cfg->m2006_init;
|
||||
|
||||
break ;
|
||||
|
||||
}
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@ -320,10 +289,10 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
|
||||
|
||||
case PITCH_START:
|
||||
// u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动
|
||||
cfg->pull_speed=10;
|
||||
cfg->pull_speed=12;
|
||||
target->go_shoot = cfg->go_pull_pos;
|
||||
|
||||
if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
|
||||
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;
|
||||
@ -336,7 +305,7 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
|
||||
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))
|
||||
if(is_reached (u->motorfeedback .go_data .Pos ,target->go_shoot ,1.0f))
|
||||
{
|
||||
*state=PITCH_LAUNCHING;
|
||||
}
|
||||
|
||||
@ -13,6 +13,7 @@
|
||||
#include "vofa.h"
|
||||
#include "GO_M8010_6_Driver.h"
|
||||
#include "bsp_usart.h"
|
||||
#include "up_utils.h"
|
||||
|
||||
/*
|
||||
状态机
|
||||
@ -34,7 +35,6 @@ typedef enum {
|
||||
PITCH_START, //启动，拉扳机
|
||||
PITCH_PULL_TRIGGER,
|
||||
PITCH_LAUNCHING, // 发射中
|
||||
|
||||
PITCH_COMPLETE // 完成
|
||||
|
||||
} PitchState_t;
|
||||
@ -45,7 +45,7 @@ typedef struct {
|
||||
fp32 m2006_trig; // M2006触发角度0,用来合并扳机
|
||||
fp32 go_init; // GO电机触发位置,初始位置，后续更改用于发射的位置
|
||||
fp32 go_pull_pos; // go上升去合并扳机，扳机位置
|
||||
fp32 Pitch_angle;
|
||||
fp32 Pitch_angle; //俯仰角度，以oid为准
|
||||
|
||||
fp32 pull_speed;//go速度
|
||||
fp32 go_release_pos;//go松开，发射位置
|
||||
@ -55,7 +55,7 @@ typedef struct {
|
||||
typedef struct {
|
||||
PitchState_t PitchState;
|
||||
PitchCfg_t PitchConfig;
|
||||
|
||||
CurveType Curve; //当前函数，雷达距离->pos
|
||||
uint8_t is_init ;
|
||||
} Pitch_Cfg_t;
|
||||
|
||||
@ -101,11 +101,10 @@ typedef struct{
|
||||
fp32 VESC_5065_M2_rpm;
|
||||
|
||||
fp32 Shoot_M2006_angle;
|
||||
fp32 Pitch_M2006_angle;
|
||||
|
||||
fp32 go_shoot;
|
||||
|
||||
fp32 Shoot_Pitch_angle;
|
||||
fp32 Pitch_angle; //以oid的角度为准,目标俯仰角
|
||||
|
||||
}up_motor_target_t;
|
||||
|
||||
@ -125,18 +124,6 @@ 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{
|
||||
|
||||
@ -153,7 +140,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,7 +178,6 @@ 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);
|
||||
|
||||
101
User/Module/up_utils.c
Normal file
101
User/Module/up_utils.c
Normal file
@ -0,0 +1,101 @@
|
||||
#include "up_utils.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);
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
@ -62,7 +62,7 @@ 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,20) ==
|
||||
SIGNAL_NUC_RAW_REDY);
|
||||
}
|
||||
|
||||
|
||||
@ -18,7 +18,7 @@
|
||||
#define TASK_FREQ_UP (900u)
|
||||
|
||||
#define TASK_FREQ_CTRL_CMD (500u)
|
||||
#define TASK_FREQ_NUC (500u)
|
||||
#define TASK_FREQ_NUC (50u)
|
||||
#define TASK_FREQ_CAN (1000u)
|
||||
#define TASK_FREQ_RC (1000u)
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user