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

Compare commits

..

No commits in common. "main" and "Cooperation" have entirely different histories.
main ... Cooperatio

35 changed files with 1090 additions and 1598 deletions

2
.gitignore vendored
View File

@ -17,7 +17,7 @@
*.dbg*
*.uvguix.*
.mxproject
*.axf
RTE/
Templates/
Examples/

1
Core/Src/main.c
View File

@ -81,7 +81,6 @@ int main(void)
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */

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

@ -471,19 +471,3 @@
[info] Log at : 2025/6/22|14:26:08|GMT+0800
[info] Log at : 2025/6/23|20:59:34|GMT+0800
[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
[info] Log at : 2025/6/28|01:22:24|GMT+0800
[info] Log at : 2025/6/30|18:03:06|GMT+0800
[info] Log at : 2025/7/1|14:01:22|GMT+0800
[info] Log at : 2025/7/7|02:36:22|GMT+0800

3
MDK-ARM/.vscode/settings.json vendored
View File

@ -97,7 +97,6 @@
"gimbal_task.h": "c",
"gimgal.h": "c",
"navi.h": "c",
"up.h": "c",
"bsp_usart.h": "c"
"up.h": "c"
}
}

164
MDK-ARM/R2.uvoptx
View File

@ -26,7 +26,7 @@
<ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName>
<TargetOption>
<CLKADS>25000000</CLKADS>
<CLKADS>12000000</CLKADS>
<OPTTT>
<gFlags>1</gFlags>
<BeepAtEnd>1</BeepAtEnd>
@ -117,6 +117,11 @@
<pMon>BIN\CMSIS_AGDI.dll</pMon>
</DebugOpt>
<TargetDriverDllRegistry>
<SetRegEntry>
<Number>0</Number>
<Key>ST-LINKIII-KEIL_SWO</Key>
<Name>-U00260035480000034E575152 -O206 -SF5000 -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>
<Key>ARMRTXEVENTFLAGS</Key>
@ -134,13 +139,13 @@
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
<Key>CMSIS_AGDI</Key>
<Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
<Key>DLGUARM</Key>
<Name></Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
<Key>DLGUARM</Key>
<Name></Name>
<Key>CMSIS_AGDI</Key>
<Name>-X"Any" -UAny -O206 -S9 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -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>
@ -153,57 +158,72 @@
<Ww>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>tar_angle</ItemText>
<ItemText>can,0x0A</ItemText>
</Ww>
<Ww>
<count>1</count>
<WinNumber>1</WinNumber>
<ItemText>huart-&gt;ErrorCode</ItemText>
<ItemText>chassis,0x0A</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>error_code</ItemText>
<ItemText>UP,0x0A</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>posss</ItemText>
<ItemText>NUC_send,0x0A</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText>
<ItemText>pid,0x0A</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>chassis,0x0A</ItemText>
<ItemText>pid_param,0x0A</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>nuc_raw</ItemText>
<ItemText>cmd_rc,0x0A</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc,0x0A</ItemText>
<ItemText>cbuf,0x10</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>drop_message,0x0A</ItemText>
<ItemText>cmd</ItemText>
</Ww>
<Ww>
<count>9</count>
<WinNumber>1</WinNumber>
<ItemText>\\R2\../User/task/chassis_task.c\chassis.param-&gt;M3508_param.d</ItemText>
<ItemText>rc_ctrl,0x0A</ItemText>
</Ww>
<Ww>
<count>10</count>
<WinNumber>1</WinNumber>
<ItemText>\\R2\../User/task/nuc_task.c\cmd_fromnuc</ItemText>
<ItemText>nuc_raw,0x0A</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc,0x0A</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>nucbuf,0x0A</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>LD_raw,0x0A</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
@ -250,10 +270,10 @@
<pMultCmdsp></pMultCmdsp>
<DebugDescription>
<Enable>1</Enable>
<EnableFlashSeq>1</EnableFlashSeq>
<EnableFlashSeq>0</EnableFlashSeq>
<EnableLog>0</EnableLog>
<Protocol>2</Protocol>
<DbgClock>10000000</DbgClock>
<DbgClock>5000000</DbgClock>
</DebugDescription>
</TargetOption>
</Target>
@ -292,7 +312,7 @@
<Group>
<GroupName>Application/User/Core</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -432,7 +452,7 @@
<Group>
<GroupName>Drivers/STM32F4xx_HAL_Driver</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -704,7 +724,7 @@
<Group>
<GroupName>Drivers/CMSIS</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -724,7 +744,7 @@
<Group>
<GroupName>Middlewares/FreeRTOS</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -892,18 +912,6 @@
<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>
@ -914,7 +922,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>51</FileNumber>
<FileNumber>50</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -926,7 +934,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>52</FileNumber>
<FileNumber>51</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -938,7 +946,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>53</FileNumber>
<FileNumber>52</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -950,7 +958,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>54</FileNumber>
<FileNumber>53</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -962,7 +970,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>55</FileNumber>
<FileNumber>54</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -974,7 +982,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>56</FileNumber>
<FileNumber>55</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -986,7 +994,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>57</FileNumber>
<FileNumber>56</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -998,7 +1006,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>58</FileNumber>
<FileNumber>57</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1010,7 +1018,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>59</FileNumber>
<FileNumber>58</FileNumber>
<FileType>5</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1022,7 +1030,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>60</FileNumber>
<FileNumber>59</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1034,7 +1042,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>61</FileNumber>
<FileNumber>60</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1046,7 +1054,7 @@
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>62</FileNumber>
<FileNumber>61</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1066,7 +1074,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>63</FileNumber>
<FileNumber>62</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1078,7 +1086,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>64</FileNumber>
<FileNumber>63</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1090,7 +1098,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>65</FileNumber>
<FileNumber>64</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1102,7 +1110,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>66</FileNumber>
<FileNumber>65</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1114,7 +1122,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>67</FileNumber>
<FileNumber>66</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1126,7 +1134,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>68</FileNumber>
<FileNumber>67</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1138,7 +1146,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>69</FileNumber>
<FileNumber>68</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1150,7 +1158,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>70</FileNumber>
<FileNumber>69</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1162,7 +1170,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>71</FileNumber>
<FileNumber>70</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1174,7 +1182,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>72</FileNumber>
<FileNumber>71</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1194,7 +1202,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>73</FileNumber>
<FileNumber>72</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1206,7 +1214,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>74</FileNumber>
<FileNumber>73</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1218,7 +1226,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>75</FileNumber>
<FileNumber>74</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1230,7 +1238,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>76</FileNumber>
<FileNumber>75</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1242,7 +1250,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>77</FileNumber>
<FileNumber>76</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1254,7 +1262,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>78</FileNumber>
<FileNumber>77</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1274,7 +1282,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>79</FileNumber>
<FileNumber>78</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1286,7 +1294,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>80</FileNumber>
<FileNumber>79</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1298,7 +1306,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>81</FileNumber>
<FileNumber>80</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1310,7 +1318,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>82</FileNumber>
<FileNumber>81</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1322,7 +1330,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>83</FileNumber>
<FileNumber>82</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1334,7 +1342,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>84</FileNumber>
<FileNumber>83</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1346,7 +1354,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>85</FileNumber>
<FileNumber>84</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1366,7 +1374,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>86</FileNumber>
<FileNumber>85</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1378,7 +1386,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>87</FileNumber>
<FileNumber>86</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1390,7 +1398,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>88</FileNumber>
<FileNumber>87</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1410,7 +1418,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>12</GroupNumber>
<FileNumber>89</FileNumber>
<FileNumber>88</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1424,13 +1432,13 @@
<Group>
<GroupName>Middlewares/USB_Device_Library</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>90</FileNumber>
<FileNumber>89</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1442,7 +1450,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>91</FileNumber>
<FileNumber>90</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1454,7 +1462,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>92</FileNumber>
<FileNumber>91</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1466,7 +1474,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>93</FileNumber>
<FileNumber>92</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>

5
MDK-ARM/R2.uvprojx
View File

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

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

Binary file not shown.

100
Middlewares/Third_Party/Protocol/protocol.h vendored
View File

@ -1,100 +0,0 @@
/*
视觉与电控通信协议
*/
#pragma once
#include <stdint.h>
#include <stdbool.h>
#ifdef __cplusplus
extern "C"
{
#endif
#define NUC_HEAD (0xFF)
#define NUC_END (0xFE)
#define NUC_ID (0x09)
#define NUC_CTRL (0x01)
#define IMU_ID (0x01)
#define CMD_ID (0x02)
/* 电控 -> 视觉 IMU数据结构体*/
typedef struct __attribute__((packed))
{
struct __attribute__((packed))
{
float x;
float y;
float z;
} gyro; /* 陀螺仪数据 */
struct __attribute__((packed))
{
float x;
float y;
float z;
} accl; /* 四元数 */
struct __attribute__((packed))
{
float q0;
float q1;
float q2;
float q3;
} quat; /* 四元数 */
} Protocol_UpDataIMU_t;
/* 电控 -> 视觉 控制数据结构体*/
typedef struct __attribute__((packed))
{
int8_t cmd; /* 控制命令 */
} Protocol_UpDataCMD_t;
/* 视觉 -> 电控 数据结构体*/
typedef struct __attribute__((packed))
{
struct __attribute__((packed))
{
float vx; /* x轴移动速度 */
float vy; /* y轴移动速度 */
float wz; /* z轴转动速度 */
} chassis_move_vec; /* 底盘移动向量 */
float distance; /* 距离（单位：米） */
float angle; /* 角度（单位：弧度） */
bool notice; /* 控制命令 */
} Protocol_DownData_t;
typedef struct __attribute__((packed))
{
uint8_t head;
uint8_t id; /* 数据包ID */
uint8_t ctrl; /* 控制字 */
Protocol_UpDataIMU_t data;
uint8_t end; /* 数据包结束符 */
} Protocol_UpPackageIMU_t;
typedef struct __attribute__((packed))
{
uint8_t head;
uint8_t id;
uint8_t ctrl;
Protocol_UpDataCMD_t data;
uint8_t end;
} Protocol_UpPackageCMD_t;
typedef struct __attribute__((packed))
{
Protocol_DownData_t data;
uint16_t crc16;
} Protocol_DownPackage_t;
#ifdef __cplusplus
}
#endif

2
User/Algorithm/error_detect.h
View File

@ -1,4 +1,6 @@
#ifndef ERROR_DETECT_H
#define ERROR_DETECT_H
#include "struct_typedef.h"

15
User/Algorithm/user_math.c
View File

@ -405,19 +405,4 @@ bool is_reached_multiple(float current1, float current2, float current3, float t
count++; // 所有条件都满足，计数加 1
}
return false; // 未满足条件达到阈值，返回 0
}
/**
* @brief 底盘无头模式转换函数
* @param vx 输入 X 方向速度（全局坐标系），输出转换后的底盘 X 速度
* @param vy 输入 Y 方向速度（全局坐标系），输出转换后的底盘 Y 速度
* @param vw 旋转速度（直接控制，不受影响）
* @param yaw 当前陀螺仪 yaw 角度（弧度制）
*/
void ChassisHeadlessMode(double *vx, double *vy, double yaw) {
double vx_global = *vx;
double vy_global = *vy;
// 标准二维旋转变换（逆时针为正）
*vx = vx_global * cosf(yaw) - vy_global * sinf(yaw);
*vy = vx_global * sinf(yaw) + vy_global * cosf(yaw);
}

1
User/Algorithm/user_math.h
View File

@ -165,6 +165,5 @@ void normalize_vector(double x, double y, double *out_x, double *out_y) ;
bool is_reached(float current, float target, float mistake) ;
bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) ;
fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max);
void ChassisHeadlessMode(double *vx, double *vy, double yaw) ;
#endif

216
User/Module/Chassis.c
View File

@ -3,6 +3,7 @@
#include "user_math.h"
#include "bsp_buzzer.h"
#include "bsp_delay.h"
/*
机器人坐标系，向前x，右y，上yaw
不同于nuc (前x，左y，上yaw)
@ -27,13 +28,10 @@ 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; //没使用原值
// }
c->sick_cali.sick_dis[0]=(fp32)(can->sickfed.raw_dis[0]) ;
c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+30) ; //有点误差，手动补偿
c->sick_cali.sick_dis[2]=(fp32)(can->sickfed.raw_dis[2] );
for (uint8_t i = 0; i < 4; i++) {
c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值
}
return CHASSIS_OK;
}
@ -42,65 +40,37 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
if (c == NULL) return CHASSIS_ERR_NULL;
c->param = param;
for (int i = 0; i < 3; i++) {
for (int i = 0; i < 4; i++) {
PID_init(&(c->pid.chassis_3508VecPID[i]), PID_POSITION_D, &(c->param->M3508_param));
}
PID_init(&(c->pid.chassis_3508VecPID[3]), PID_POSITION_D, &(c->param->M3508_param_other));
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));
/*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.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));
PID_init(&(c->pid.Mid360AngleInPID), PID_POSITION, &(c->param->Mid360AngleInPID_param));
PID_init(&(c->pid.Mid360AngleOutPID), PID_POSITION, &(c->param->Mid360AngleOutPID_param));
/*修正 */
PID_init(&(c->pid.Chassis_AngleAdjust), PID_POSITION, &(c->param->Chassis_AngleAdjust_param));
LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波
LowPassFilter2p_Init(&(c->filled[1]), target_freq, 50.0f);
LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f);
LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f);
KalmanCreate(&c->extKalman[0],10,1);
KalmanCreate(&c->extKalman[1],10,1);
KalmanCreate(&c->extKalman[2],10,1);
LowPassFilter2p_Init(&(c->filled[1]), target_freq, 80.0f); // w滤波
LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f); // y滤波
LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f); // x滤波
c->sick_cali .sickparam=c->param ->sickparam ;
c->ang_cail.is_open=1;
return CHASSIS_OK;
}
//void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
// fp64 Nor_Vx, Nor_Vy;
// normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy);
//
// c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+c->ang_cail.out; // 右前
// c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+c->ang_cail.out; // 右后
// c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +c->ang_cail.out; // 左后
// c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +c->ang_cail.out; // 左前
//
// Chassis_AngleCompensate(c);
//}
fp32 gyro_kp = 1.0f;
fp32 PIAN = 0;
fp32 pian_yaw;
void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
fp64 Nor_Vx, Nor_Vy;
normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy);
ChassisHeadlessMode(&Nor_Vx,&Nor_Vy,c->pos088.imu_eulr.yaw);
c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+c->ang_cail.out; // 右前
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+c->ang_cail.out; // 右后
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +c->ang_cail.out; // 左后
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +c->ang_cail.out; // 左前
c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+pian_yaw; // 右前
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+pian_yaw; // 右后
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +pian_yaw; // 左后
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +pian_yaw; // 左前
Chassis_AngleCompensate(c);
@ -114,20 +84,14 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);
/*初始数据*/
c->to_nuc.send = 0;
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
c->NUC_send.send[0] = 0;
c->sick_cali.is_reach = 0;
c->move_vec.Vw=0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
switch (c->chassis_ctrl.ctrl) {
case RCcontrol:
c->to_nuc.send = 0;
switch (c->chassis_ctrl.mode) {
case Normal:
c->move_vec.Vw = ctrl->Vw * 8000;
@ -143,71 +107,41 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->move_vec.Vw = ctrl->Vw * 6000;
break;
case Chassis_Adjust_Sick:
case Chassis_Adjust:
if(abs(ctrl->cmd_MID360.angle)<1.0f){
c->move_vec.Vx = ctrl->cmd_MID360.dis * 3000;
}
if(-ctrl->cmd_MID360.dis>0.02f){
c->move_vec.Vw = -ctrl->cmd_MID360.angle * 1000;
}
// sick_calibration(c, ctrl, out);
// c->to_nuc.send = (c->sick_cali.is_reach == 1) ? 1 : 0;
sick_calibration(c, ctrl, out);
c->NUC_send.send[0] = (c->sick_cali.is_reach == 1) ? 1 : 0;
break;
default:
break;
}
break;
case AUTO:
switch (c->chassis_ctrl.mode) {
case AUTO_MID360:
c->move_vec.Vw = ctrl->Vw * 8000;
c->move_vec.Vx = ctrl->Vy * 8000;
c->move_vec.Vy = ctrl->Vx * 8000;
// c->move_vec.Vw = -ctrl->cmd_MID360.angle * 3000;
// abs_limit_fp(&c->move_vec.Vw, 5000.0f);
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);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
c->NUC_send.send[0] = 1;
break;
case AUTO_MID360_Pitch:
c->move_vec.Vw = -ctrl->cmd_MID360.angle * 3000;
c->move_vec.Vx = ctrl->Vy * 8000;
c->move_vec.Vy = ctrl->Vx * 8000;
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
break;
case REPOSITION:
c->to_nuc.send = 1;
break;
case AUTO_MID360_Adjust:
c->move_vec.Vw = ctrl->Vw * 6000;
c->move_vec.Vx = ctrl->Vy * 6000;
c->move_vec.Vy = ctrl->Vx * 6000;
break;
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.angle * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
break ;
}
break;
case RELAXED:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
break ;
break;
break;
default:
@ -215,7 +149,6 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
break;
}
// 电机速度限幅
abs_limit_fp(&c->move_vec.Vx, 6000.0f);
abs_limit_fp(&c->move_vec.Vy, 6000.0f);
@ -230,7 +163,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];
return CHASSIS_OK;
@ -247,50 +180,33 @@ sick2,左1
*/
fp32 diff_yaw,diff_y,diff_x;
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 sick0 = c->sick_cali.sick_dis[0] ;
fp32 sick1 = c->sick_cali.sick_dis[1];
fp32 sick2 = c->sick_cali.sick_dis[2];
const sickparam_t *param = &c->sick_cali.sickparam;
fp32 diff_yaw = -(fp32)(sick1 - sick2);
fp32 diff_y = -(fp32)(sick1 - c->sick_cali.sickparam.y_set);
fp32 diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set);
diff_yaw =LowPassFilter2p_Apply(&(c->filled[1]), -(fp32)(sick1 +10 - sick2));
diff_y = LowPassFilter2p_Apply(&(c->filled[2]),-(fp32)(sick1 - c->sick_cali.sickparam.y_set));
diff_x = LowPassFilter2p_Apply(&(c->filled[3]),(fp32)(sick0 - c->sick_cali.sickparam.x_set));
delta_w=PID_calc(&c->pid.SickCaliWzOutPID, diff_yaw, 0);
delta_x=PID_calc(&c->pid.SickCaliVxOutPID, diff_x, 0);
delta_y=PID_calc(&c->pid.SickCaliVyOutPID, diff_y,0);
if(fabs(diff_yaw)>param->w_error){
c->move_vec.Vw=PID_calc(&c->pid.SickCaliWzInPID,-delta_w,0);
}
// if(fabs(diff_y)>param->xy_error){
//
// c->move_vec.Vy=PID_calc(&c->pid.SickCaliVyInPID,delta_y,0);
// }
// if(fabs(diff_x)>param->xy_error){
//
// c->move_vec.Vx=PID_calc(&c->pid.SickCaliVxInPID,-delta_x,0);
// }
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;
static uint16_t reach_cnt = 0;
if (fabsf(diff_yaw) <= param->w_error &&
fabsf(diff_x) <= param->xy_error &&
fabsf(diff_y) <= param->xy_error) {
reach_cnt++;
if (reach_cnt >= 20) {
if (reach_cnt >= 50) {
c->sick_cali.is_reach = 1;
@ -303,25 +219,37 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
return CHASSIS_OK;
}
pid_type_def pid;
pid_param_t pid_param={
.p = 10.0f,
.i = 0.00f,
.d = 0.00f,
.i_limit = 2000.0f,
.out_limit =5000.0f,
};
fp32 cur_angle,pian_angel;
int8_t Chassis_AngleCompensate(Chassis_t *c)
{
if (c == NULL) return CHASSIS_ERR_NULL;
if(c->ang_cail.is_open==0) return 0;
// int is_in;//是否初始化
// if(is_in==0){
PID_init(&pid,PID_POSITION,&pid_param);
// is_in=1;
// }
static fp32 pian_angel,cur_angle;
if((c->move_vec.Vx || c->move_vec.Vy) && (c->move_vec.Vw== 0))
{
c->ang_cail.ang_error=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
pian_angel=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
}
else {
c->ang_cail.ang_cur=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
c->ang_cail.ang_error=0;
cur_angle=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
pian_angel=0;
}
c->ang_cail.out = PID_calc(&c->pid.Chassis_AngleAdjust,pian_angel,0);
pian_yaw = PID_calc(&pid,pian_angel,0);
}

58
User/Module/Chassis.h
View File

@ -31,7 +31,6 @@
#include "can_use.h"
#include "cmd.h"
#include "filter.h"
#include "kalman.h"
#define CHASSIS_OK (0)
#define CHASSIS_ERR (-1)
@ -73,8 +72,6 @@ typedef struct
/*该部分决定PID的参数整定在config中修改*/
pid_param_t M3508_param;
pid_param_t M3508_param_other;
pid_param_t chassis_PICKWzPID_HIGN_param;
pid_param_t chassis_PICKWzPID_LOW_param;
@ -83,19 +80,9 @@ typedef struct
pid_param_t NaviVy_param;
pid_param_t NaviVw_param;
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;
pid_param_t Mid360AngleInPID_param;
pid_param_t Mid360AngleOutPID_param;
pid_param_t Sick_CaliWparam;
pid_param_t Sick_CaliYparam;
pid_param_t Sick_CaliXparam;
sickparam_t sickparam;
@ -111,6 +98,11 @@ typedef struct
fp32 Vw;
fp32 mul;//油门倍率
}ChassisMove_Vec;
typedef struct
{
fp32 send[4];
}NUC_send_t;
/**
* @brief
@ -161,49 +153,27 @@ typedef struct{
pid_type_def chassis_PICKWzPID_HIGN;
/*存在较低误差*/
pid_type_def chassis_PICKWzPID_LOW;
pid_type_def Chassis_AngleAdjust;
/*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_type_def Mid360AngleInPID;
pid_type_def Mid360AngleOutPID;
pid_type_def SickCaliWzPID;
pid_type_def SickCaliVxPID;
pid_type_def SickCaliVyPID;
}pid;
fp32 vofa_send[8];
/*卡尔曼滤波*/
extKalman_t extKalman[3];
LowPassFilter2p_t filled[8]; /* 输出滤波器滤波器数组 */
/*角度偏转修正 */
struct{
int is_open;
fp32 ang_error;
fp32 ang_cur;
fp32 out;
}ang_cail;
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
struct {
fp32 sick_dis[4]; //获取到的sick激光值
int32_t sick_dis[4]; //获取到的sick激光值
sickparam_t sickparam;
int is_reach;
}sick_cali;
struct {
fp32 send;
}to_nuc;
NUC_send_t NUC_send;
}Chassis_t;

251
User/Module/config.c
View File

@ -37,18 +37,18 @@ static const ConfigParam_t param ={
.out_limit = 3000.0f,
},
.Pitch_M2006_angle_param = {
.p = 1600.0f,
.p = 600.0f,
.i = 0.0f,
.d = 3.0f,
.i_limit = 4000.0f,
.out_limit = 20000.0f,
.i_limit = 2000.0f,
.out_limit = 3000.0f,
},
.Pitch_M2006_speed_param={
.p = 8.5f,
.i = 0.03f,
.p = 5.0f,
.i = 0.3f,
.d = 0.0f,
.i_limit = 2000.0f,
.out_limit = 15000.0f,
.out_limit = 3000.0f,
},
.Pitch_Angle_M2006_speed_param={ //俯仰跑速度环
.p = 20.0f,
@ -58,6 +58,28 @@ static const ConfigParam_t param ={
.out_limit = 3000.0f,
},
.Dribble_M3508_speed_param={ //三摩擦速度环
.p = 30.0f,
.i = 0.45f,
.d = 0.0f,
.i_limit = 3000.0f,
.out_limit = 5000.0f,
},
.Dribble_translate_M3508_speed_param={//平移用3508速度环
.p = 5.0f,
.i = 0.3f,
.d = 0.0f,
.i_limit = 2000.0f,
.out_limit = 3000.0f,
},
.Dribble_translate_M3508_angle_param= { //平移用3508角度环
.p = 25.0f,
.i = 0.0f,
.d = 1.5f,
.i_limit = 1000.0f,
.out_limit = 3000.0f,
},
.go_cmd={
.id =0,
@ -71,69 +93,43 @@ static const ConfigParam_t param ={
},
/*上层其他参数,一些只是初始值，在运行过程中会被更改*/
/*上层其他参数*/
/*运球*/
.DribbleCfg = {
.dribble_flag=0,
.m3508_init_ang = -5,
.m3508_translate_angle = 1000,
.rev_spd=-2000,
.spd= 4200, // 转动速度
.init_spd=0,
.light_3508_flag=0,//3508平移处的光电，0初始，1到位置
.light_ball_flag=0,//检测球的flag
},
/*投球*/
.LaunchCfg = {
.m2006_init = -140.0f, // M2006初始角度
.m2006_trig = 0.0f, // M2006触发角度
.go_pull_pos = -210.0f, // go上升去合并扳机，扳机位置
.go_up_speed = 15.0f, // 上升速度
.go_down_speed = 12.0f, // 下降速度
.Pitch_angle = 66, //俯仰角
.go_init = -50
},
.PitchCfg = {
.m2006_init =-150, //释放的角度
.m2006_trig =0, //可以拉住发射的角度
.go_init = -50, //仅用在go上电，初始位置
.go_release_pos=-200,
.Pitch_angle =66,
.go_up_speed =15,
.go_down_speed =10,
.go_pull_pos =-214,
.Pitch_angle =58,
.pull_speed =5,
},
.PassCfg={
.go_wait =-10,
.go_release_pos =-150,
.Pitch_angle=66,
.go_up_speed =15,
.go_down_speed =10,
},
.MID360Cfg={
.go_release_pos=-200, // GO电机发射位置
.go_up_speed=15, // GO电机上升速度
.go_down_speed=10, // GO电机下降速度
},
},
.chassis = {/**/
.M3508_param = {
// .p = 15.1f,
// .i = 0.02f,
// .d = 3.2f,
// .i_limit = 200.0f,
// .out_limit =6000.0f,
.p = 20.5f,
.i = 0.00005f,
.p = 15.1f,
.i = 0.02f,
.d = 3.2f,
.i_limit = 200.0f,
.out_limit =6000.0f,
},
.M3508_param_other= {
.p = 16.5f,
.i = 0.00001f,
.d = 3.2f,
.i_limit = 200.0f,
.out_limit =6000.0f,
},
/*视觉*/
.chassis_PICKWzPID_HIGN_param ={
@ -150,135 +146,32 @@ static const ConfigParam_t param ={
.i_limit = 50.0f,
.out_limit =1000.0f,
},
// .SickCali_WInparam = {
// .p = 3.0f,
// .i = 0.000f,
// .d = 0.0f,
// .i_limit = 500.0f,
// .out_limit = 2000.0f,
// },
// .SickCali_WOutparam = {
// .p = 2.6f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 1000.0f,
// },
// .SickCali_YInparam = {
// .p = 1.0f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 5000.0f,
// },
// .SickCali_YOutparam = {
// .p = 4.5f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 00.0f,
// .out_limit = 1000.0f,
// },
// .SickCali_XInparam = {
// .p = 1.0f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 5000.0f,
// },
// .SickCali_XOutparam = {
// .p = 4.5f,
// .i = 0.00f,
// .d = 0.0f,
// .i_limit = 500.0f,
// .out_limit = 2000.0f,
// },
.SickCali_WInparam = {
.p = 3.0f,
.i = 0.000f,
.d = 0.0f,
.i_limit = 500.0f,
.out_limit = 2000.0f,
},
.SickCali_WOutparam = {
.p = 2.6f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 1000.0f,
},
.SickCali_YInparam = {
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 5000.0f,
},
.SickCali_YOutparam = {
.p = 4.5f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 00.0f,
.out_limit = 1000.0f,
},
.SickCali_XInparam = {
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 5000.0f,
},
.SickCali_XOutparam = {
.p = 4.5f,
.i = 0.00f,
.d = 0.0f,
.i_limit = 500.0f,
.out_limit = 2000.0f,
},
.Chassis_AngleAdjust_param={
.p = 10.0f,
.i = 0.0f,
.Sick_CaliWparam ={
.p = 4.5f,
.i = 0.005f,
.d = 0.0f,
.i_limit = 0.0f,
.i_limit = 500.0f,
.out_limit =1000.0f,
},
.Mid360AngleInPID_param={
.p = 0.0f,
.i = 0.0f,
},
.Sick_CaliYparam ={
.p = 2.9f,
.i = 0.0051f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit =1000.0f,
},
.Mid360AngleOutPID_param={
.p = 10.0f,
.i = 0.0f,
.i_limit = 500.0f,
.out_limit =3000.0f,
},
.Sick_CaliXparam ={
.p = 2.9f,
.i = 0.0065f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit =1000.0f,
},
.i_limit = 500.0f,
.out_limit =3000.0f,
},
.sickparam={
.w_error=2,
.xy_error=3,
.x_set=10000,
.y_set=2000,
.w_error=5,
.xy_error=5,
.x_set=927,
.y_set=1255,
},

649
User/Module/up.c
View File

@ -3,10 +3,13 @@
#include "user_math.h"
#include "bsp_buzzer.h"
#include "bsp_delay.h"
/*接线
上层用到三个光电
1.PE9 发射用 输出高电平 LIGHTA
2.PE11 运球上3508 输出高电平 LIGHTB
3.PE13 三摩擦检测球 输出高电平LIGHTC
*/
@ -23,7 +26,6 @@
// 定义继电器控制
#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, RELAY_Pin, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
{
u->param = param; /*初始化参数 */
@ -40,26 +42,31 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
PID_init (&u->pid .Pitch_M2006_angle ,PID_POSITION ,&(u->param->Pitch_M2006_angle_param ));
PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param ));
PID_init (&u->pid .Dribble_translate_M3508_angle ,PID_POSITION ,&(u->param->Dribble_translate_M3508_angle_param ));
PID_init (&u->pid .Dribble_translate_M3508_speed ,PID_POSITION ,&(u->param->Dribble_translate_M3508_speed_param ));
for(int i=0;i<3;i++){
PID_init (&u->pid .Dribble_M3508_speed[i] ,PID_POSITION ,&(u->param->Dribble_M3508_speed_param ));
}
u->go_cmd =u->param ->go_cmd ;
LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f);
if (!u->PitchContext .is_init) {
u->PitchContext .PitchConfig = u->param ->PitchCfg ;//赋值
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新
u->PitchContext .is_init = 1;
// 初始化上层状态机
if (!u->DribbleCont .is_init) { //检查是否为第一次运行状态机，运球
u->DribbleCont .DribbleConfig = u->param ->DribbleCfg ;//赋值
u->DribbleCont .DribbleState = DRIBBLE_PREPARE;
u->DribbleCont .is_init = 1;
}
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;
u->MID360Context.MID360Cfg = u->param->MID360Cfg;
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
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;
}
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
}
@ -88,11 +95,38 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
}
u->cmd =c;
/*光电状态更新*/
u->DribbleCont .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin);
u->DribbleCont .DribbleConfig .light_3508_flag =HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin);
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，
*/
@ -118,7 +152,6 @@ 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;
@ -128,34 +161,71 @@ 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)
{
/*目标值限位*/
abs_limit_min_max_fp(&u->motor_target.go_shoot ,-215.0f,0.0f);
abs_limit_min_max_fp(&u->motor_target.Pitch_angle ,48.0f,71.5f);
//电机控制 ，传进can
DJ_Speed_Control(u,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed[0],u->motor_target .Dribble_M3508_speed[0]);
DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed[1],u->motor_target .Dribble_M3508_speed[1]);
DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed[2],u->motor_target .Dribble_M3508_speed[2]);
DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] ,
&u->pid .Dribble_translate_M3508_angle ,
&u->pid .Dribble_translate_M3508_speed ,
u->motor_target .Dribble_translate_M3508_angle );
DJ_Angle_Control(u,0x205,&u->motorfeedback .DJmotor_feedback[4] ,
&u->pid .Shoot_M2006angle ,
&u->pid .Shoot_M2006speed ,
u->motor_target .Shoot_M2006_angle );
/*俯仰2006，双环，内使用oid角度环，外电机速度环
*/
// DJ_Angle_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5] ,
// &u->pid .Pitch_M2006_angle ,
// &u->pid .Pitch_M2006_speed ,
// u->motor_target .Pitch_M2006_angle );
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 .Pitch_angle)+0.14)
u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle))
);
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] ;
@ -172,299 +242,370 @@ 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->PitchContext.PitchConfig;
LaunchCfg_t *LaunchCfg =&u->LaunchContext.LaunchCfg;
PitchCfg_t *pitch_cfg = &u->Pitch_Cfg.PitchConfig;
DribbleCfg_t *DribbleCont = &u->DribbleCont.DribbleConfig;
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);
switch (c->CMD_CtrlType )
{
case RCcontrol: //在手动模式下
switch (c-> CMD_mode )
{
case Normal : //复位，go位置和俯仰角保持LaunchCfg最后修改后的位置，扳机2006角度复位用于发射
case Normal :
/*投篮*/
if(is_pitch){
target->go_shoot = LaunchCfg->go_init ;
target->Pitch_angle = LaunchCfg->Pitch_angle;
target->go_shoot =pitch_cfg->go_init ;
target->Shoot_Pitch_angle=pitch_cfg->Pitch_angle;
is_pitch=0;
} //初始上电
//LaunchCfg->go_up_speed=15;
// target->Pitch_angle =LaunchCfg->Pitch_angle;
target->go_pull_speed=LaunchCfg->go_up_speed;
target->Shoot_M2006_angle=LaunchCfg->m2006_init;
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新
u->LaunchContext.LaunchState = Launch_Stop;
} //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删
target->Shoot_M2006_angle =u->Pitch_Cfg .PitchConfig .m2006_init ;
u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球
/*运球*/
RELAY1_TOGGLE(0);//关闭气缸
for(int i=0;i<3;i++){
target->Dribble_M3508_speed[i]=0;
}
target->Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig .m3508_translate_angle;
u->DribbleCont .DribbleState = DRIBBLE_PREPARE; //重置最初状态
break;
case Pitch :
if (u->PitchContext .PitchState ==PITCH_PREPARE)
if (u->Pitch_Cfg .PitchState ==PITCH_PREPARE)
{
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
}
Pitch_Process(u,out,c);
Pitch_Process(u,out);
break ;
case UP_Adjust: //测试用,手动使用pitch下的cfg
case UP_Adjust: //测试用
pitch_cfg ->pull_speed=5;
pitch_cfg ->go_release_pos += c->Vx ;
LaunchCfg->Pitch_angle += c->Vy/100;
pitch_cfg->Pitch_angle += c->Vy/100;
target->go_shoot=pitch_cfg ->go_release_pos;
target->Pitch_angle=LaunchCfg->Pitch_angle;
target->go_shoot=u->Pitch_Cfg.PitchConfig.go_release_pos;
target->Shoot_Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle;
break ;
default:
break;
case Dribble:
{
if(u->DribbleCont.DribbleState== DRIBBLE_PREPARE){
u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;
}
//光电状态更新
Dribble_Process(u,out);
}break ;
default:
break;
}
break;
case AUTO:
UP_AUTO_Control(u,out,c);
break ;
case PASS_BALL:
Pass_Process(u,out,c);
break;
case RELAXED:
// target->go_shoot= pitch_cfg .
target->Pitch_angle = 66;
target->go_shoot = -190.0f;
//失控下，最好切手动，手动用pitch
pitch_cfg ->go_release_pos=-190;
// target->Pitch_angle = 58;
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;
// if(u->CoContext .CoState == CoPITCH){
// u->CoContext.CoState=CoPITCH_MID360;
// }
// Co_Process(u,out);
Pitch_Process(u,out);
break ;
}
case AUTO_MID360:
target->Shoot_M2006_angle =pitch_cfg ->m2006_init ;
u->Pitch_Cfg .PitchState = PITCH_PREPARE;
u->DribbleCont .DribbleState = DRIBBLE_PREPARE;
break ;
}
return 0;
}
}
//复用发射，
int8_t Pitch_Launch_Sequence(UP_t *u,CMD_t *c, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){
/*电机位置到达判断*/
bool is_GoStartReach=is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f); //go开始位置
bool is_GoSpeedReach=is_reached(u->motorfeedback.go_data.W,0,1.0f) ; //go速度归零判断
bool is_GoEndReach =(u->motorfeedback .go_data .Pos < -209); //go去上拉钩子位置，判断到达
bool is_HookDone=(u->motorfeedback .DJmotor_feedback [4].total_angle>-5.0f); //2006钩子放下判断
bool is_Shoot=(u->motorfeedback.DJmotor_feedback[4].total_angle<-10);//是否发射判断
switch(LaunchContext->LaunchState){
case Launch_Stop: break;
case Launch_PREPARE:
u->motor_target.go_shoot = StartPos;
if(is_GoStartReach&& is_GoSpeedReach){
//根据位置和速度判断是否到达初始位置
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(is_GoEndReach){ //检测go位置到达最上面，这里的检测条件可以更改
u->motor_target.Shoot_M2006_angle = u->LaunchContext.LaunchCfg.m2006_trig ;//设置2006角度，关闭
LaunchContext->LaunchState = Launch_TRIGGER;
}break;
case Launch_TRIGGER:
if( is_HookDone ){ //当2006的总角度小于1，可以认为已经勾上,误差为1
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_down_speed;
u->motor_target.go_shoot = EndPos ;
} break;
case Launch_SHOOT_WAIT:
if(is_Shoot) //认为发射
LaunchContext->LaunchState = Launch_DONE;
break;
default:break;
}
}
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
{
/*简化*/
PitchCfg_t *cfg = &u->PitchContext.PitchConfig;
PitchState_t *state =&u->PitchContext .PitchState;
PitchCfg_t *cfg = &u->Pitch_Cfg.PitchConfig;
PitchState_t *state =&u->Pitch_Cfg .PitchState;
up_motor_target_t *target=&u->motor_target ;
LaunchContext_t *LaunchContext = &u->LaunchContext;
// 可根据实际需要传入不同的起始和末位置,起始：当前位置
LaunchContext->LaunchCfg.go_up_speed= cfg->go_up_speed;
LaunchContext->LaunchCfg.go_down_speed= cfg->go_down_speed;
abs_limit_min_max_fp(&cfg->go_release_pos ,-215.0f,0.0f);
abs_limit_min_max_fp(&cfg->Pitch_angle,48.0f,67.0f);
Pitch_Launch_Sequence(u, c,LaunchContext, u->motorfeedback.go_data.Pos, cfg->go_release_pos, out);
switch(*state){
case PITCH_START:
LaunchContext->LaunchState = Launch_PREPARE;
// u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动
cfg->pull_speed=10;
target->go_shoot = cfg->go_pull_pos;
*state=PITCH_WAIT;
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;
}//更改标志位
case PITCH_WAIT:
break ;
case PITCH_PULL_TRIGGER:
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;
}
}
break ;
case PITCH_LAUNCHING: //等待发射
// *state=PITCH_COMPLETE;
break ;
case PITCH_COMPLETE: //发射完成
break ;
}
return 0;
}
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
{
static int common_speed_flag=0;//是否共速
static int common_speed_reverse_flag=0;//是否共速
DribbleCfg_t *DribbleCfg = &u->DribbleCont.DribbleConfig;
up_motor_target_t *target=&u->motor_target ;
DribbleState_t *DribbleState=&u->DribbleCont.DribbleState;
switch (u->DribbleCont.DribbleState) {
case DRIBBLE_TRANSLATE:
target->Dribble_translate_M3508_angle =DribbleCfg->m3508_translate_angle;//平行移动
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主
{
u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态
}
break;
case DRIBBLE_PROCESS_DOWN:
target->Dribble_M3508_speed[0]=DribbleCfg->spd;
target->Dribble_M3508_speed[1]=DribbleCfg->spd;
target->Dribble_M3508_speed[2]=DribbleCfg->spd;
if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm,
u->motorfeedback .DJmotor_feedback [1].rpm,
u->motorfeedback .DJmotor_feedback [2].rpm,
DribbleCfg->spd,
DribbleCfg->spd,
DribbleCfg->spd, 50.0f,50)
) {
RELAY1_TOGGLE(1); //速度达到后打开气缸
common_speed_flag =1;
}
if(common_speed_flag){
if(u->DribbleCont .DribbleConfig .light_ball_flag == 0){//球下落检测，反转
target->Dribble_M3508_speed[0]=DribbleCfg->rev_spd;
target->Dribble_M3508_speed[1]=DribbleCfg->rev_spd;
target->Dribble_M3508_speed[2]=DribbleCfg->rev_spd;
*DribbleState=DRIBBLE_PROCESS_UP;
}
}
break;
case DRIBBLE_PROCESS_UP:
common_speed_flag =0;
if((u->motorfeedback .DJmotor_feedback [0].rpm<-500)&&
(u->motorfeedback .DJmotor_feedback [1].rpm<-500)&&
(u->motorfeedback .DJmotor_feedback [2].rpm<-500)
){
common_speed_reverse_flag=1;
}
if(common_speed_reverse_flag){
if(DribbleCfg->light_ball_flag == 1){
*DribbleState=DRIBBLE_DONE;
RELAY1_TOGGLE(0); //关闭气缸
}
}
break ;
case DRIBBLE_DONE:
common_speed_reverse_flag=0;
for(int i=0;i<3;i++){
target->Dribble_M3508_speed[i]=DribbleCfg->init_spd ;//三摩擦速度归0
}
/*标志位清零*/
DribbleCfg->light_ball_flag=0;
DribbleCfg->light_3508_flag=0;
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)
// CoPREPARE, // 准备阶段
// CoTRANSLATE_OUT,//上方平移,去运球
// CoDRIBBLE, //运球阶段
// CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
// CoPITCH, //发射
// CoDONE,
//int8_t Trans_Process (UP_t *u, CAN_Output_t *out)
//{
//
//}
int8_t Co_Process(UP_t *u, CAN_Output_t *out)
{
switch(u->CoContext .CoState )
{
case CoPREPARE:
PassCfg_t *PassCfg = &u->PassContext.PassCfg;
PassState_t *state = &u->PassContext.PassState;
PassContext_t *PassContext = &u->PassContext;
up_motor_target_t *target=&u->motor_target ;
LaunchContext_t *LaunchContext = &u->LaunchContext;
break ;
case CoTRANSLATE_OUT:
u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动
Pass_Sequence_Check(u,c);
/*俯仰角度，力度转换*/
PassCfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3.3,4.3,&u->PassContext.Curve);
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle > 500)//速度为0认为卡主
{
u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，运球转移状态
u->Pitch_Cfg .PitchState = PITCH_START;
if (u->PassContext.Curve == CURVE_58) {
target->Pitch_angle = 58;
PassCfg->Pitch_angle=58;
}
else {
target->Pitch_angle = 66;
PassCfg->Pitch_angle=66;
}
abs_limit_min_max_fp(&PassCfg->go_release_pos ,-215.0f,0.0f);
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,c,LaunchContext,u->motorfeedback.go_data.Pos,PassCfg->go_wait,out);
break;
case PASS_START:
if(LaunchContext->LaunchState == Launch_TRIGGER){
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;
}
int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
/*自动下数据更新，根据距离切换俯仰角,如需自动调整，放在*/
up_motor_target_t *target=&u->motor_target ;
LaunchContext_t *LaunchContext = &u->LaunchContext;
MID360Context_t *MID360Context=&u->MID360Context;
MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg;
u->PitchContext.PitchConfig.go_release_pos =MID360Cfg->go_release_pos;
/*俯仰角度，力度转换，加数值限位*/
if(c->cmd_MID360.dis<1.3f){
MID360Cfg->go_release_pos=-150.0f;
}else {
if(u->MID360Context.Curve==CURVE_58){
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis)+0.20f,3.3,4.3,&u->MID360Context.Curve);
u->CoContext .CoState =CoDRIBBLE;
}
else {
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis)+0.1f,3.3,4.3,&u->MID360Context.Curve);
break;
}
}
if (u->MID360Context.Curve == CURVE_58) {
target->Pitch_angle = 58;
} else {
target->Pitch_angle = 66;
}
abs_limit_min_max_fp(&MID360Cfg->go_release_pos ,-215.0f,0.0f);
LaunchContext->LaunchCfg.go_up_speed=MID360Cfg->go_up_speed;
LaunchContext->LaunchCfg.go_down_speed=MID360Cfg->go_down_speed;
switch(c-> CMD_mode)
case CoDRIBBLE:
Dribble_Process(u,out); //状态停在DRIBBLE_DONE
Pitch_Process(u,out); //状态停在PITCH_LAUNCHING
//状态停在对应位置时,平移去给发射送球
if(u->DribbleCont .DribbleState ==DRIBBLE_DONE && u->Pitch_Cfg.PitchState ==PITCH_LAUNCHING)
{
u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig .m3508_init_ang;
}
if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleCont .DribbleConfig.m3508_init_ang,1.0f))
{
u->motor_target.Dribble_M3508_speed[0]=1000;
u->motor_target.Dribble_M3508_speed[1]=1000;
u->motor_target.Dribble_M3508_speed[2]=1000;
case AUTO_MID360_Pitch:
if(MID360Context->IsLaunch==0){
MID360Context->IsLaunch=1;
LaunchContext->LaunchState = Launch_PREPARE;
}
Pitch_Launch_Sequence(u,c,LaunchContext,u->motorfeedback.go_data.Pos,MID360Cfg->go_release_pos,out);
u->CoContext .CoState =CoTRANSLATE_IN;
}
break ;
case CoTRANSLATE_IN:
if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm,
u->motorfeedback .DJmotor_feedback [1].rpm,
u->motorfeedback .DJmotor_feedback [2].rpm,
u->motor_target.Dribble_M3508_speed [0],
u->motor_target.Dribble_M3508_speed [1],
u->motor_target.Dribble_M3508_speed[2],
50.0f,100)
) {
RELAY1_TOGGLE(1); //速度达到后打开气缸,送给发射
}
if(u->DribbleCont .DribbleConfig.light_ball_flag == 0)
{
u->motor_target.Dribble_M3508_speed[0]=0;
u->motor_target.Dribble_M3508_speed[1]=0;
u->motor_target.Dribble_M3508_speed[2]=0;
u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动
}
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle > 900)//速度为0认为卡主,卡到最右端
{
u->CoContext .CoState =CoPITCH;
}
break ;
case CoPITCH:
case CoPITCH_MID360:
u->motor_target .go_shoot=u->Pitch_Cfg.PitchConfig.go_init;
break ;
break ;
case AUTO_MID360:
target->Shoot_M2006_angle= LaunchContext->LaunchCfg.m2006_init;
MID360Context->IsLaunch=0;
// u->cmd_from_nuc.lock=0;
break ;
default:
break;
}
}
}

202
User/Module/up.h
View File

@ -13,7 +13,6 @@
#include "vofa.h"
#include "GO_M8010_6_Driver.h"
#include "bsp_usart.h"
#include "up_utils.h"
/*
状态机
@ -28,105 +27,103 @@
*/
/*共用发射，重复部分*/
/*配合过程状态*/
typedef enum {
CoPREPARE, // 准备阶段
CoTRANSLATE_OUT,//上方平移,去运球
CoDRIBBLE, //运球和蓄力阶段,再平移回去
CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
CoPITCH, //发射
CoPITCH_MID360, //雷达发射
CoDONE,
}CoState_t;
/*总配合上下文*/
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;
CoState_t CoState;
fp32 Pitch_angle;
} LaunchCfg_t;
typedef enum {
Launch_Stop,
Launch_PREPARE, //
Launch_START, //启动，从指定位置上升，扣动扳机
Launch_TRIGGER, //拉动到指定位置
Launch_SHOOT_WAIT, // 发射等待
Launch_SHOOT,
Launch_DONE,
uint8_t is_init ;
} CoCon_t;
}LaunchState_t;
typedef struct {
LaunchCfg_t LaunchCfg;
LaunchState_t LaunchState;
} LaunchContext_t;
/* 投球状态定义 */
typedef enum {
PITCH_PREPARE, // 准备阶段
PITCH_START, //启动，拉扳机
PITCH_WAIT, // 发射等待
PITCH_PULL_TRIGGER,
PITCH_LAUNCHING, // 发射中
PITCH_COMPLETE // 完成
} PitchState_t;
/* 投球参数配置 */
typedef struct {
fp32 m2006_init; // M2006初始角度
fp32 m2006_trig; // M2006触发角度0,用来合并扳机
fp32 go_init; // GO电机触发位置,初始位置，后续更改用于发射的位置
fp32 Pitch_angle; //俯仰角度，以oid为准
fp32 go_pull_pos; // go上升去合并扳机，扳机位置
fp32 Pitch_angle;
fp32 pull_speed;//go速度
fp32 go_release_pos;//go松开，发射位置
fp32 go_up_speed;
fp32 go_down_speed;
} PitchCfg_t;
//fp32 go_pull_pos;
// fp32 m2006_init; // M2006初始角度
// fp32 m2006_trig; // M2006触发角度
// fp32 go_init; // GO初始位置
// fp32 go_release; // GO释放阈值
// fp32 go_recv; // GO接球位置
// fp32 screw_init; // 螺杆初始值
// fp32 pitch_ang; // 投球角度
// fp32 pull_spd; // 拉动速度
/* 投球控制上下文 */
typedef struct {
PitchState_t PitchState;
PitchCfg_t PitchConfig;
uint8_t is_init ;
} PitchContext_t;
/*传球过程 */
uint8_t is_init ;
} Pitch_Cfg_t;
/*运球*/
typedef enum {
PASS_STOP, // 停止状态,未进入传球模式
PASS_IDLE, // 空闲状态，进入未开始
PASS_PREPARE, // 传球准备
PASS_START, // 启动传球,拉动到等球位置
PASS_POS_PREPARE, // 传球位置准备，对准篮筐，go的位置
PASS_COMPLETE // 发射
} PassState_t;
DRIBBLE_PREPARE, // 准备阶段
DRIBBLE_START,
DRIBBLE_TRANSLATE, // 平移过程
DRIBBLE_PROCESS_DOWN, // 运球过程，球下落
DRIBBLE_PROCESS_UP, // 运球过程，球上升
DRIBBLE_DONE // 运球结束
} DribbleState_t;
/* 参数配置结构体 */
typedef struct {
int8_t dribble_flag;//当移动三摩擦后为1，否则为0，防止发射撞到
fp32 go_wait; // GO等待位置
fp32 go_release_pos; // GO电机发射位置
fp32 Pitch_angle; // 俯仰角度，以oid为准
fp32 go_up_speed; // GO电机上升速度
fp32 go_down_speed; // GO电机下降速度
fp32 m3508_init_ang; // 3508平移前位置
fp32 m3508_translate_angle; // 平移后位置
fp32 init_spd;
fp32 spd; // 转动速度
fp32 rev_spd;
} PassCfg_t;
/*光电标志位，初始值均为0，触发为1*/
int light_3508_flag;//3508平移处的光电，0初始，1到位置
int light_ball_flag;//检测球的flag
} DribbleCfg_t;
/* 状态机上下文 */
typedef struct {
PassState_t PassState;
PassCfg_t PassCfg;
CurveType Curve; //当前函数，雷达距离->pos
uint8_t is_init ;
} PassContext_t;
/*雷达自动*/
typedef struct {
fp32 go_release_pos; // GO电机发射位置
fp32 go_up_speed; // GO电机上升速度
fp32 go_down_speed; // GO电机下降速度
} MID360Cfg_t;
typedef struct {
MID360Cfg_t MID360Cfg;
CurveType Curve; //当前函数，雷达距离->pos
int IsLaunch; //是否在发射过程
} MID360Context_t;
DribbleState_t DribbleState;
DribbleCfg_t DribbleConfig;
uint8_t is_init;
} DribbleCont_t;
typedef struct {
@ -151,10 +148,14 @@ typedef struct
pid_param_t Pitch_Angle_M2006_speed_param;
LaunchCfg_t LaunchCfg;
pid_param_t Dribble_M3508_speed_param;//三摩擦用的速度环
pid_param_t Dribble_translate_M3508_speed_param;//平移用的速度环
pid_param_t Dribble_translate_M3508_angle_param;//平移用的角度环
DribbleCfg_t DribbleCfg;
PitchCfg_t PitchCfg;
PassCfg_t PassCfg;
MID360Cfg_t MID360Cfg;
GO_MotorCmd_t go_cmd;
}UP_Param_t;
@ -164,10 +165,13 @@ typedef struct{
fp32 VESC_5065_M2_rpm;
fp32 Shoot_M2006_angle;
fp32 Pitch_M2006_angle;
fp32 go_shoot;
fp32 go_pull_speed;
fp32 Pitch_angle; //以oid的角度为准,目标俯仰角
fp32 Dribble_M3508_speed[3];//运球转速
fp32 Dribble_translate_M3508_angle;//平移用的3508转动角度
fp32 Shoot_Pitch_angle;
}up_motor_target_t;
@ -185,20 +189,36 @@ typedef struct{
pid_type_def Pitch_Angle_M2006_speed;
pid_type_def Dribble_M3508_speed[3];//三摩擦用的速度环
pid_type_def Dribble_translate_M3508_speed;//平移用的速度环
pid_type_def Dribble_translate_M3508_angle;//平移用的角度环
}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;
LaunchContext_t LaunchContext;
uint8_t up_task_run;
const UP_Param_t *param;
CoCon_t CoContext;
/*运球过程*/
DribbleCont_t DribbleCont;
/*投篮过程*/
PitchContext_t PitchContext;
/*传球过程*/
PassContext_t PassContext;
/*自动过程*/
MID360Context_t MID360Context;
Pitch_Cfg_t Pitch_Cfg;
CMD_t *cmd;
struct{
@ -207,7 +227,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];
@ -215,7 +235,6 @@ typedef struct{
uint32_t ecd;
float angle;
}Encoder;
}motorfeedback;
@ -235,11 +254,7 @@ typedef struct{
}final_out;
// struct {
// float angle;
// float dis;
// uint8_t lock;
// }cmd_from_nuc;
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
fp32 vofa_send[8];
@ -250,17 +265,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,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,CMD_t *c, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out);
int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c);
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out);
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out);
int8_t Co_Process(UP_t *u, CAN_Output_t *out);
#endif

173
User/Module/up_utils.c
View File

@ -1,173 +0,0 @@
#include "up_utils.h"
#include "up.h"
int8_t DJ_processdata(DJmotor_feedback_t *f, fp32 ecd_to_angle)
{
fp32 angle, delta;
angle = f->ecd;
// 初始化阶段，记录 offset
if (f->init_cnt < 50) {
f->offset_ecd = (uint16_t)angle; // 记录初始偏移
f->orig_angle = angle - f->offset_ecd; // orig_angle 归零
f->last_angle = angle - f->offset_ecd;
f->init_cnt++;
return 0;
}
// 使用 offset 修正
angle = angle - f->offset_ecd;
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;
}
// 计算66度曲线（偏上）
static float curve_66(float d) {
return 2.9851104*pow(d,4) -36.164382*pow(d,3) + 159.54844*pow(d,2) -273.62856*d + 43.092452;
}
// 计算58度曲线（偏下）
static float curve_58(float d) {
// return 0.9242f * d * d + 19.4246f * d - 154.9055f;
// return 2.638517*pow(d,4) -47.996138*pow(d,3) + 325.38515*pow(d,2) -950.18155*d + 919.86543;
return 2.7775f * d * d - 0.5798f * d - 113.1488;
}
/*
曲线切换,用于距离和pos拟合
迟滞区x-y
曲线x重合区，根据当前函数和变化方向切换
*/
fp32 ang_58;
fp32 ang_66;
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) {
ang_58=d;
return curve_58(d);
} else {
ang_66=d;
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;
}

58
User/Module/up_utils.h
View File

@ -1,58 +0,0 @@
#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;
uint16_t offset_ecd;
uint32_t msg_cnt;
}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

56
User/bsp/protocol.h
View File

@ -14,11 +14,6 @@ extern "C" {
#define HEAD (0xFF)
#define TAIL (0xFE)
#define IMU_ID (0x01)
#define CMD_ID (0x02)
#define TYPE (0x09)
#define NAVI (0x05)
#define PICK (0x06)
@ -37,55 +32,31 @@ typedef struct __attribute__((packed)) {
typedef struct __attribute__((packed)) {
Protocol_ID_t recv_id;//作为帧头使用确认通信ID正确
uint8_t status; /* 控制命令 */
struct __attribute__((packed)) {
float posy; /*pitch轴*/
float posx; /*yaw轴*/
char cmd;
float dis; /*距离*/
}pick;
struct __attribute__((packed)) {
float vx; /* x轴移动速度 */
float vy; /* y轴移动速度 */
float wz; /* z轴转动速度 */
float dis;
}navi;
} Protocol_DownDataChassis_t;
/* 电控 -> 视觉 IMU数据结构体*/
typedef struct __attribute__((packed))
{
struct __attribute__((packed))
{
float x;
float y;
float z;
} gyro; /* 陀螺仪数据 */
struct __attribute__((packed))
{
float x;
float y;
float z;
} accl; /* 四元数 */
struct __attribute__((packed))
{
float q0;
float q1;
float q2;
float q3;
} quat; /* 四元数 */
} Protocol_UpDataIMU_t;
typedef struct __attribute__((packed))
{
uint8_t status;
} Protocol_UpDataCMD_t;
/* 视觉 -> 电控 上层机构数据结构体*/
typedef struct __attribute__((packed)) {
float angle; // 角度信息
float dis; // 位移信息
uint8_t status; /* 控制命令 */
} Protocol_DownDataUpper_t;
typedef struct __attribute__((packed)) {
Protocol_UpDataMCU_t data;
} Protocol_UpPackageMCU_t;
@ -98,7 +69,6 @@ typedef struct __attribute__((packed)) {
typedef struct __attribute__((packed)) {
Protocol_DownDataUpper_t data;
} Protocol_DownPackageUpper_t;
#ifdef __cplusplus
}
#endif

77
User/device/can_use.c
View File

@ -2,9 +2,6 @@
/*
CAN总线上的设备1到7
将所有CAN总线上挂载的设备抽象成单个设备进行管理和控制
底盘4个3508，3个dt35接收板，终端电阻勿开，会出现数据收发异常情况
*/
/* Includes ----------------------------------------------------------------- */
@ -14,9 +11,13 @@
#include "error_detect.h"
#define CAN_MOTOR_ENC_RES (8191) /* 电机编码器分辨率 */
#define CAN_MOTOR_CUR_RES (16384)
#define CAN_PITCH6020_CTRL_ID (0x2ff)
#define CAN_G3508_CTRL_ALL_CHASSIS_ID (0x200)
/*CAN2上预留>4电机*/
/*CAN2上有6个大疆电机*/
#define CAN_G3508_CTRL_ALL_UP_SHOOT_ID (0x1ff)
#define CAN_G3508_CTRL_ALL_UP_Dribble_ID (0x200)
@ -56,39 +57,26 @@ static void CAN_DJIMotor_Decode(CAN_MotorFeedback_t *feedback,
feedback->torque_current =
raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES;
}
static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,int id,
static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,
const uint8_t *raw) {
if (feedback == NULL || raw == NULL) return;
static uint16_t sick_rec[4];
switch (id)
{
case CAN_SICK_ID_1:
feedback->raw_dis[0] = (uint16_t)(raw[0] << 8| raw[1]);
break ;
case CAN_SICK_ID_2:
feedback->raw_dis[1] = (uint16_t)(raw[0] << 8| raw[1]);
break ;
case CAN_SICK_ID_3:
feedback->raw_dis[2] = (uint16_t)(raw[0] << 8| raw[1]);
break ;
case CAN_SICK_ID_4:
feedback->raw_dis[3] = (uint16_t)(raw[0] << 8| raw[1]);
break ;
}
sick_rec[0] = (uint16_t)(raw[0] << 8 | raw[1]);
sick_rec[1] = (uint16_t)(raw[2] << 8 | raw[3]);
sick_rec[2] = (uint16_t)(raw[4] << 8 | raw[5]);
sick_rec[3] = (uint16_t)(raw[6] << 8 | raw[7]);
feedback ->raw_dis [0]= sick_rec[0];
feedback ->raw_dis [1]= sick_rec[1];
feedback ->raw_dis [2]= sick_rec[2];
feedback ->raw_dis [3]= sick_rec[3];
}
void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
const uint8_t *raw) {
if (feedback == NULL || raw == NULL) return;
switch(raw[1])//判断编码器id
{
{
case 0x01:
switch(raw[2])//判断指令id
@ -296,6 +284,25 @@ void CAN_Encoder_Control(CAN_t *can)
return DEVICE_OK;
}
//用来问答接收来自sick的数据
void CAN_Sick_Control(CAN_t *can)
{
raw_tx.tx_header.StdId = 0x301;
raw_tx.tx_header.IDE = CAN_ID_STD;
raw_tx.tx_header.DLC = 0x08;
raw_tx.tx_header.RTR = CAN_RTR_DATA;//˽ߝ֡
raw_tx.tx_data[0] = 0x00;
raw_tx.tx_data[1] = 0x00;
raw_tx.tx_data[2] = 0x00;
raw_tx.tx_data[3] = 0x00;
raw_tx.tx_data[4] = 0x00;
raw_tx.tx_data[5] = 0x00;
raw_tx.tx_data[6] = 0x00;
raw_tx.tx_data[7] = 0x00;
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->sick),&raw_tx.tx_header,raw_tx.tx_data,&(can->mailbox.chassis));
}
int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_t *can){
if (output == NULL) return DEVICE_ERR_NULL;
int Byte[4];
@ -367,19 +374,7 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
CAN_DJIMotor_Decode(&(can->motor.chassis_motor3508.as_array[index]), can_rx->rx_data);
detect_hook(CHASSIS3508M1_TOE + index);
break;
case CAN_SICK_ID_1:
CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_1, can_rx->rx_data);
break;
case CAN_SICK_ID_2:
CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_2, can_rx->rx_data);
break;
case CAN_SICK_ID_3:
CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_3, can_rx->rx_data);
break;
case CAN_SICK_ID_4:
CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_4, can_rx->rx_data);
break;
default:
break;
@ -410,7 +405,9 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
CAN_DJIMotor_Decode(&(can->motor.up_shoot_motor3508.as_array[index]), can_rx->rx_data);
break;
case CAN_SICK_ID:
CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);
break;
case CAN_Encoder_ID:
CAN_Encoder_Decode(&(can->Oid),can_rx->rx_data);
can->recive_flag |= 1 << 10;

9
User/device/can_use.h
View File

@ -18,10 +18,7 @@ typedef enum {
CAN_UP_M3508_M5_ID = 0x205, /* 1 */
CAN_UP_M3508_M6_ID = 0x206, /* 2 */
CAN_SICK_ID_1=0x301,
CAN_SICK_ID_2=0x302,
CAN_SICK_ID_3=0x303,
CAN_SICK_ID_4=0x304,
CAN_SICK_ID=0x305,
CAN_Encoder_ID=0x01,
@ -195,10 +192,12 @@ int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group,CAN_Output_t *output,CAN_t *c
int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_t *can);
void CAN_Sick_Control(CAN_t *can);
void CAN_Encoder_Control(CAN_t *can);
void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
const uint8_t *raw) ;
static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,int id,
static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,
const uint8_t *raw) ;
#endif

41
User/device/cmd.c
View File

@ -89,23 +89,32 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
if(cmd == NULL) return -1;
if(n == NULL) return -1;
cmd->cmd_status = n->status_fromnuc;
cmd->raw_status = n->ctrl_status;
for (int i = 0; i < 7; ++i)
{ // 从最低位到最高位遍历
cmd->status[i] = ((cmd->raw_status) & (1 << i)) ? 1 : 0;
}
switch(cmd->cmd_status){
case MID:
cmd->cmd_MID360.posx = n->MID360.vx;
cmd->cmd_MID360.posy = n->MID360.vy;
cmd->cmd_MID360.posw = n->MID360.wz;
cmd->pos =n->MID360 .pos ;
break;
case VISION :
cmd ->cmd_pick .posx =n->camera.data1 ;
cmd ->cmd_pick .posy =n->camera.data2 ;
cmd ->cmd_pick .posw =n->camera.data3 ;
break;
cmd->cmd_MID360.angle = n->MID360.angle;
cmd->cmd_MID360.dis = n->MID360.dis;
}
return 0;
}
@ -137,7 +146,7 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左上，右上，
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Chassis_Adjust; //左上，右上，手动调整
if(rc->dr16.res > 3000) cmd ->CMD_mode =UP_Adjust;
}
@ -156,8 +165,8 @@ 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_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; //左下，右上，无模式
}
@ -184,7 +193,7 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
if(cmd ->CMD_CtrlType ==AUTO){
/*自动下的*/
if (rc->LD.key_C ==CMD_SW_UP) cmd ->CMD_mode =Normal;
if (rc->LD.key_C ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360_Adjust;
else {
if(rc->LD.key_E ==CMD_SW_DOWN ) cmd ->CMD_mode =Normal;
@ -195,10 +204,10 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
}
else if(cmd ->CMD_CtrlType ==RCcontrol){
/*手动下的*/
if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode=Normal ;
if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode=Dribble ;
else if(rc->LD.key_G ==CMD_SW_UP) cmd ->CMD_mode =Pitch;
else if(rc->LD .key_H==CMD_SW_UP) cmd ->CMD_mode =UP_Adjust;
else if(rc->LD.key_C == CMD_SW_UP) cmd->CMD_mode =Chassis_Adjust_Sick;
else if(rc->LD.key_C == CMD_SW_UP) cmd->CMD_mode =Chassis_Adjust;
else cmd ->CMD_mode =Normal;
}
}

28
User/device/cmd.h
View File

@ -14,7 +14,6 @@
typedef enum{
RCcontrol,//遥控器控制，左按键上，控制上层
AUTO,
PASS_BALL,//传球模式
RELAXED,//异常模式
}CMD_CtrlType_t;
@ -26,28 +25,29 @@ typedef enum{
AUTO_MID360,
AUTO_MID360_Pitch,
REPOSITION,//雷达调整
AUTO_MID360_Adjust,//雷达调整
UP_Adjust,//上层调整
Chassis_Adjust_Sick,
/*传球占三个,逻辑在up.h里，简化就这样写了*/
PB_UP,
PB_MID,
PB_DOWN,
Chassis_Adjust,
Pitch,
Dribble , //运球
Pitch, //投篮,底盘锁定
Dribbl_transfer
}CMD_mode_t;
typedef struct {
uint8_t status_fromnuc;
uint8_t ctrl_status; //取其中每一个二进制位用作通信
struct
{
fp32 vx;
fp32 vy;
fp32 wz;
fp32 pos;
fp32 angle;
fp32 dis;
char flag;
}MID360;
struct
@ -142,9 +142,9 @@ typedef struct {
}cmd_pick;
struct
{
fp32 angle;
fp32 dis;
fp32 posx;
fp32 posy;
fp32 posw;
}cmd_MID360;

246
User/device/nuc.c
View File

@ -2,145 +2,167 @@
#include "crc16.h"
#include <string.h>
#include "error_detect.h"
volatile uint32_t drop_message = 0;
static volatile uint32_t drop_message = 0;
static osThreadId_t thread_alert;
uint8_t nucbuf[10];
char SendBuffer[7];
uint8_t nucbuf[31];
static char SendBuffer[19];
static void NUC_CBLTCallback(void)
{
osThreadFlagsSet(thread_alert, SIGNAL_NUC_RAW_REDY);
// detect_hook(NUC_TOE);
static void NUC_IdleCallback(void) {
osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
detect_hook(NUC_TOE);
}
static void NUC_ERRORCALLBACK(void)
{
NUC_Restart();
uint32_t error_code = HAL_UART_GetError(BSP_UART_GetHandle(BSP_UART_NUC));
// osThreadFlagsSet(thread_alert, SIGNAL_NUC_ERROR);
int8_t NUC_Init(NUC_t *nuc){
if(nuc == NULL) return DEVICE_ERR_NULL;
if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_NUC,BSP_UART_IDLE_LINE_CB,
NUC_IdleCallback);
return DEVICE_OK;
}
int8_t NUC_Init(NUC_t *nuc)
{
if (nuc == NULL)
return DEVICE_ERR_NULL;
if ((thread_alert = osThreadGetId()) == NULL)
return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_RX_CPLT_CB,
NUC_CBLTCallback);
BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_ERROR_CB,
NUC_ERRORCALLBACK);
return DEVICE_OK;
}
int8_t NUC_StartReceiving()
{
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
int8_t NUC_StartReceiving() {
if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
(uint8_t *)nucbuf,
sizeof(nucbuf)) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
int8_t NUC_Restart(void)
{
int8_t NUC_StartSending(fp32 *data) {
union
{
float x[4];
char data[16];
}instance;
for (int i = 0; i < 4; i++) {
instance.x[i] = data[i];
}
SendBuffer[0] = 0xFC; //发送ID
SendBuffer[1] = 0x01; //控制帧
for(int i = 2; i < 18; i++)
{
SendBuffer[i] = instance.data[i-2];
}
SendBuffer[18] = 0xFD; //结束符
if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
(uint8_t *)SendBuffer,
sizeof(SendBuffer)) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
int8_t NUC_Restart(void) {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_NUC));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_NUC));
return DEVICE_OK;
}
bool_t NUC_WaitDmaCplt(void)
{
return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll, 0) ==
bool_t NUC_WaitDmaCplt(void) {
return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,100) ==
SIGNAL_NUC_RAW_REDY);
}
int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc)
{
if (n == NULL)
return DEVICE_ERR_NULL;
union
{
float x[2];
char data[8];
} instance;
if (nucbuf[0] != HEAD)
goto error; // 发送ID不是底盘
else
{
if (nucbuf[9] != TAIL)
goto error;
int8_t NUC_RawParse(CMD_NUC_t *n){
if(n ==NULL) return DEVICE_ERR_NULL;
union
{
float x[5];
char data[20];
}instance;
if(nucbuf[0]!=HEAD) goto error; //发送ID不是底盘
else{
n->status_fromnuc =nucbuf[1];
n->ctrl_status =nucbuf[2];
switch (n->status_fromnuc)
{
case VISION://控制帧0x02
/* 协议格式
0xFF HEAD
0x02 控制帧
0x01 相机帧
vx fp32
vy fp32
wz fp32
0xFE TAIL
*/
instance.data[3] = nucbuf[4];
instance.data[2] = nucbuf[3];
instance.data[1] = nucbuf[2];
instance.data[0] = nucbuf[1];
n->MID360.angle = instance.x[0]; //
instance.data[7] = nucbuf[8];
instance.data[6] = nucbuf[7];
instance.data[5] = nucbuf[6];
instance.data[4] = nucbuf[5];
n->MID360.dis = instance.x[1]; //
instance.data[3] = nucbuf[3];
instance.data[2] = nucbuf[4];
instance.data[1] = nucbuf[5];
instance.data[0] = nucbuf[6];
n->camera .data1 = instance.x[0]; //
instance.data[7] = nucbuf[7];
instance.data[6] = nucbuf[8];
instance.data[5] = nucbuf[9];
instance.data[4] = nucbuf[10];
n->camera .data2 = instance.x[1];//
instance.data[11] = nucbuf[11];
instance.data[10] = nucbuf[12];
instance.data[9] = nucbuf[13];
instance.data[8] = nucbuf[14];
n->camera .data3 = instance.x[2];//
break;
case MID ://控制帧0x08
/* 协议格式
0xFF HEAD
0x09 控制帧
0x01 相机帧
vx fp32
vy fp32
wz fp32
0xFE TAIL
*/
if(nucbuf[24]!=TAIL)goto error;
instance.data[3] = nucbuf[6];
instance.data[2] = nucbuf[5];
instance.data[1] = nucbuf[4];
instance.data[0] = nucbuf[3];
n->MID360.vx = instance.x[0]; //
instance.data[7] = nucbuf[10];
instance.data[6] = nucbuf[9];
instance.data[5] = nucbuf[8];
instance.data[4] = nucbuf[7];
n->MID360.vy = instance.x[1];//
instance.data[11] = nucbuf[14];
instance.data[10] = nucbuf[13];
instance.data[9] = nucbuf[12];
instance.data[8] = nucbuf[11];
n->MID360.wz = instance.x[2];//
instance.data[15] = nucbuf[18];
instance.data[14] = nucbuf[17];
instance.data[13] = nucbuf[16];
instance.data[12] = nucbuf[15];
n->MID360.pos = instance.x[3];//
instance.data[19] = nucbuf[22];
instance.data[18] = nucbuf[21];
instance.data[17] = nucbuf[20];
instance.data[16] = nucbuf[19];
n->MID360.angle = instance.x[4];//
n->MID360.flag = nucbuf[23];//
break;
nuc->unc_online = true; // 设置为在线状态
return DEVICE_OK;
}
return DEVICE_OK;
}
error:
drop_message++;
NUC_Restart();
return DEVICE_ERR;
error:
drop_message++;
return DEVICE_ERR;
}
int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc)
int8_t NUC_HandleOffline(CMD_NUC_t *cmd)
{
if (cmd == NULL)
return DEVICE_ERR_NULL;
nuc->unc_online = false; // 设置为离线状态
memset(cmd, 0, sizeof(*cmd));
return 0;
if(cmd == NULL) return DEVICE_ERR_NULL;
memset(cmd, 0, sizeof(*cmd));
return 0;
}
int8_t NUC_PackIMU(NUC_t *nuc, const AHRS_Quaternion_t *quat, const AHRS_Accl_t *accl, const AHRS_Gyro_t *gyro){
nuc->to_nuc.imu.head = HEAD;
nuc->to_nuc.imu.id = IMU_ID;
nuc->to_nuc.imu.type = TYPE;
nuc->to_nuc.imu.end = TAIL;
memcpy((void *)&(nuc->to_nuc.imu.package.quat), (const void *)quat,sizeof(*quat));
memcpy((void *)&(nuc->to_nuc.imu.package.gyro), (const void *)gyro,sizeof(*gyro));
memcpy((void *)&(nuc->to_nuc.imu.package.accl), (const void *)accl,sizeof(*accl));
return DEVICE_OK;
}
int8_t NUC_PackCMD(NUC_t *nuc, const NUC_send_t *send){
nuc->to_nuc.cmd.head = HEAD;
nuc->to_nuc.cmd.id = IMU_ID;
nuc->to_nuc.cmd.end = TAIL;
memcpy((void *)&(nuc->to_nuc.cmd.package.status), (const void *)send,sizeof(*send));
/*在这里添加你们的控制命令*/
return DEVICE_OK;
}
int8_t NUC_StartSend(NUC_t *nuc, bool cmd_update){
if (cmd_update) {
// if (HAL_UART_Transmit_DMA(
// BSP_UART_GetHandle(BSP_UART_NUC), (uint8_t *)&(nuc->to_nuc.cmd),
// sizeof(nuc->to_nuc.cmd) + sizeof(nuc->to_nuc.imu)) == HAL_OK)
/*仅发送cmd数据*/
if (HAL_UART_Transmit_DMA(
BSP_UART_GetHandle(BSP_UART_NUC), (uint8_t *)&(nuc->to_nuc.cmd),
sizeof(nuc->to_nuc.cmd) ) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
}
// else {
// if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
// (uint8_t *)&(nuc->to_nuc.imu),
// sizeof(nuc->to_nuc.imu)) == HAL_OK)
// return DEVICE_OK;
// else
// return DEVICE_ERR;
// }
}

43
User/device/nuc.h
View File

@ -6,47 +6,21 @@
#include "bsp_usart.h"
#include "cmd.h"
#include "protocol.h"
#include "Algorithm/ahrs.h"
#include "Module/Chassis.h"
typedef struct
{
fp32 send;
}NUC_send_t;
typedef struct {
uint8_t id;
Protocol_UpPackageMCU_t package;
} NUC_UpPackageMCU_t;
typedef struct {
uint8_t head;
uint8_t type;
uint8_t id; // 0x01 IMU帧
Protocol_UpDataIMU_t package;
uint8_t end;
} NUC_UpPackageIMU_t;
typedef struct {
uint8_t head;
uint8_t id;
Protocol_UpDataCMD_t package;
uint8_t end;
} NUC_UpPackageCMD_t;
typedef struct {
// osThreadId_t thread_alert;
bool unc_online; //是否在线
Protocol_DownPackageChassis_t Nucfor_chassis; //接收的数据协议
// NUC_UpPackageMCU_t to_nuc; //发送的数据协议
struct {
NUC_UpPackageIMU_t imu;
NUC_UpPackageCMD_t cmd;
} to_nuc;
NUC_UpPackageMCU_t to_nuc; //发送的数据协议
uint8_t status;//控制状态
@ -55,14 +29,9 @@ typedef struct {
int8_t NUC_Init(NUC_t *nuc);
int8_t NUC_StartReceiving(void);
int8_t NUC_StartSending(fp32 *data) ;
bool_t NUC_WaitDmaCplt(void);
int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc);
int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc);
int8_t NUC_Restart(void);
int8_t NUC_PackIMU(NUC_t *nuc, const AHRS_Quaternion_t *quat, const AHRS_Accl_t *accl, const AHRS_Gyro_t *gyro);
int8_t NUC_PackCMD(NUC_t *nuc, const NUC_send_t *send);
int8_t NUC_StartSend(NUC_t *nuc, bool cmd_update);
int8_t NUC_RawParse(CMD_NUC_t *n);
int8_t NUC_HandleOffline(CMD_NUC_t *cmd);
#endif

6
User/device/rc.c
View File

@ -1,9 +1,5 @@
/*
遥控
dr16：dbus 乐迪：s-ubs
dr16掉线不输出信号，可以使用超时检测方式判断掉线
即RC_SBUS_WaitDmaCplt
乐迪掉线依然有信号输出，只是部分通道变为特定值，无法使用以上方式
DR16接收机
*/

4
User/device/vofa.c
View File

@ -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);
}

10
User/task/atti_esti.c
View File

@ -119,7 +119,7 @@ void Task_AttiEsti(void *argument) {
AHRS_GetEulr(&imu_eulr, &gimbal_ahrs);
detect_hook(IMU_TOE);
osKernelUnlock();
osMessageQueueReset(task_runtime.msgq.imu.accl);
osMessageQueuePut(task_runtime.msgq.imu.accl, &bmi088.accl, 0, 0);
@ -128,13 +128,7 @@ void Task_AttiEsti(void *argument) {
osMessageQueueReset(task_runtime.msgq.imu.eulr);
osMessageQueuePut(task_runtime.msgq.imu.eulr, &imu_eulr, 0, 0);
osMessageQueueReset(task_runtime.msgq.nuc.gyro);
osMessageQueuePut(task_runtime.msgq.nuc.gyro, &bmi088.gyro, 0, 0);
osMessageQueueReset(task_runtime.msgq.nuc.accl);
osMessageQueuePut(task_runtime.msgq.nuc.accl, &bmi088.accl, 0, 0);
osMessageQueueReset(task_runtime.msgq.nuc.quat);
osMessageQueuePut(task_runtime.msgq.nuc.quat, &gimbal_ahrs.quat, 0, 0);
osKernelUnlock();
/* PID控制IMU温度，PWM输出 */
BSP_PWM_Set(BSP_PWM_IMU_HEAT,

9
User/task/chassis_task.c
View File

@ -34,7 +34,7 @@ static Chassis_Ctrl_t ctrl;
#endif
fp32 freq;
/**
* \brief 底盘任务
@ -50,7 +50,6 @@ void Task_Chassis(void *argument)
Chassis_init(&chassis,&(task_runtime.config.config->chassis),TASK_FREQ_CHASSIS);
uint32_t tick = osKernelGetTickCount();
uint32_t last_tick=tick;
while(1)
@ -60,8 +59,6 @@ void Task_Chassis(void *argument)
task_runtime.freq.chassis=TASK_FREQ_CHASSIS;
task_runtime.last_up_time.chassis=tick;
#endif
freq=(tick-last_tick)/1000.0f;
last_tick =tick;
/*imu数据获取*/
osMessageQueueGet(task_runtime.msgq.imu.eulr, &chassis.pos088.imu_eulr, NULL, 0);
@ -91,7 +88,7 @@ void Task_Chassis(void *argument)
osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);//清空队列
osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor_CHASSIS3508, 0, 0); //发送数据
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc_send);//清空队列
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc_send, &chassis.to_nuc, 0, 0); //发送数据
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc_send, &chassis.NUC_send, 0, 0); //发送数据
vofa_send[0] = chassis.vofa_send[0];
vofa_send[1] = chassis.vofa_send[1];
vofa_send[2] = chassis.vofa_send[2];
@ -101,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);

11
User/task/cmd_task.c
View File

@ -40,15 +40,10 @@ void Task_cmd(void *argument){
/*将各任务接收到的原始数据解析为通用的控制命令*/
/*注意，不能将nuc和码盘导航一块使用*/
if((osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK)){ //nuc
}
else
{
memset(&Nuc, 0, sizeof(CMD_NUC_t));
}
CMD_ParseNuc(&cmd,&Nuc);
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK) //nuc
CMD_ParseNuc(&cmd,&Nuc);
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器

9
User/task/init.c
View File

@ -11,7 +11,7 @@
#include "task\user_task.h"
#include "can_use.h"
#include "cmd.h"
#include "nuc.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
@ -64,13 +64,6 @@ void Task_Init(void *argument) {
task_runtime.msgq.can.output.up_dribble_3508 =
osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL);
/*nuc*/
task_runtime.msgq.nuc.quat =
osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
task_runtime.msgq.nuc.accl =
osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL);
task_runtime.msgq.nuc.gyro =
osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
/* imu */

58
User/task/nuc_task.c
View File

@ -5,82 +5,54 @@
NUC_t nuc_raw;
CMD_NUC_t cmd_fromnuc;
AHRS_Quaternion_t quat;
AHRS_Accl_t accl;
AHRS_Gyro_t gyro;
NUC_send_t NUC_send;
#else
static NUC_t nuc_raw;
static CMD_NUC_t cmd_fromnuc;
static AHRS_Quaternion_t quat;
static AHRS_Accl_t accl;
static AHRS_Gyro_t gyro;
NUC_send_t NUC_send;
#endif
fp32 send[4]={11.0f,45.0,1.f,4.0f};
int a=0;
void Task_nuc(void *argument){
(void)argument; /**/
osDelay(TASK_INIT_DELAY_NUC);
// osDelay(TASK_INIT_DELAY_NUC);
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_NUC;
NUC_Init(&nuc_raw);
uint32_t tick = osKernelGetTickCount();
uint32_t last_online_tick = tick;
while (1)
{
#ifdef DEBUG
task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());
task_runtime .freq.nuc = TASK_FREQ_NUC;
task_runtime.last_up_time.nuc= tick;
#endif
NUC_StartReceiving();
NUC_StartSending(NUC_send.send);
// NUC_StartSending (send);
if (NUC_WaitDmaCplt()){
NUC_RawParse(&cmd_fromnuc, &nuc_raw);
last_online_tick = tick;
NUC_RawParse(&cmd_fromnuc);
}
else{
if (tick - last_online_tick > 300)
NUC_HandleOffline(&cmd_fromnuc, &nuc_raw);
NUC_HandleOffline(&cmd_fromnuc);
}
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc);
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0);
osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0);
if (nuc_raw.unc_online) {
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc);
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0);
}
else
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc);
osMessageQueueGet(task_runtime.msgq.nuc.quat, &(quat), NULL, 0);
osMessageQueueGet(task_runtime.msgq.nuc.accl, &(accl), NULL, 0);
osMessageQueueGet(task_runtime.msgq.nuc.gyro, &(gyro), NULL, 0);
bool cmd_update = (osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send,
&(NUC_send), NULL, 0) == osOK);
// NUC_PackIMU(&nuc_raw, &quat, &accl, &gyro);
if (cmd_update) {
NUC_PackCMD(&nuc_raw,&NUC_send);
NUC_StartSend(&nuc_raw,cmd_update);
}
// NUC_StartSend(&nuc_raw, cmd_update);
tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/
osDelayUntil(tick);
}
}

3
User/task/rc_task.c
View File

@ -33,7 +33,7 @@ static LD_Data_t LD;
* \brief dr16接收机
*
* \param argument 未使用
*/
*/int aaaaaaaaaaaaaaaaaaa=0;
void Task_rc(void *argument) {
(void)argument; /* 未使用，消除警告 */
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_RC;
@ -48,6 +48,7 @@ void Task_rc(void *argument) {
#endif
/* 开启DMA */
RC_StartDmaRecv();
aaaaaaaaaaaaaaaaaaa++;
if (RC_WaitDmaCplt(30)) {
RC_ParseRC(&dr16,&LD_raw,&LD, &cmd_rc);

7
User/task/up_task.c
View File

@ -91,11 +91,8 @@ 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 [2]=UP.vofa_send [2];
vofa_send [3]=UP.vofa_send [3];
vofa_send [4]=UP.vofa_send [4];
// vofa_send [0]=UP.vofa_send [0];
// vofa_send [1]=UP.vofa_send [1];
vofa_tx_main (vofa_send);
tick += delay_tick;

15
User/task/user_task.h
View File

@ -13,14 +13,14 @@
/* 所有任务都要定义自己的任务运行频率 */
// 分配的频率该如何给定?
#define TASK_FREQ_CHASSIS (900u)
#define TASK_FREQ_UP (900u)
#define TASK_FREQ_CTRL_CMD (100u)
#define TASK_FREQ_NUC (100u)
#define TASK_FREQ_CTRL_CMD (500u)
#define TASK_FREQ_NUC (500u)
#define TASK_FREQ_CAN (1000u)
#define TASK_FREQ_RC (100u)
#define TASK_FREQ_RC (1000u)
#define TASK_FREQ_ERROR_DTC (3u)
@ -54,18 +54,13 @@ typedef struct {
osMessageQueueId_t eulr; /* 姿态结算得到欧拉角 */
} imu;
/* 控制指令 */
struct {
osMessageQueueId_t quat; /* 姿态解算得到 */
osMessageQueueId_t accl; /* IMU读取 */
osMessageQueueId_t gyro; /* IMU读取 */
} nuc;
struct {
struct {
osMessageQueueId_t rc;
osMessageQueueId_t nuc; //
osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
osMessageQueueId_t nuc_send; //给nuc发
}raw;
/*控制分离*/