整体代码

.gitignore

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

Core/Src/main.c

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

MDK-ARM/.vscode/c_cpp_properties.json

@@ -415,114 +415,6 @@
                 "__vfp_status(x,y)=0"
             ],
             "intelliSenseMode": "${default}"
-        },
-        {
-            "name": "R2",
-            "includePath": [
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Core\\Inc",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Include",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\User",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\bsp",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\device",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\task",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\Algorithm",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\Module",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\MDK-ARM",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\USB_DEVICE\\App",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\USB_DEVICE\\Target",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Inc",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Inc",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Lib\\ARM",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Core\\Src",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Src",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Src",
-                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Src"
-            ],
-            "defines": [
-                "USE_HAL_DRIVER",
-                "STM32F407xx",
-                "__CC_ARM",
-                "__arm__",
-                "__align(x)=",
-                "__ALIGNOF__(x)=",
-                "__alignof__(x)=",
-                "__asm(x)=",
-                "__forceinline=",
-                "__restrict=",
-                "__global_reg(n)=",
-                "__inline=",
-                "__int64=long long",
-                "__INTADDR__(expr)=0",
-                "__irq=",
-                "__packed=",
-                "__pure=",
-                "__smc(n)=",
-                "__svc(n)=",
-                "__svc_indirect(n)=",
-                "__svc_indirect_r7(n)=",
-                "__value_in_regs=",
-                "__weak=",
-                "__writeonly=",
-                "__declspec(x)=",
-                "__attribute__(x)=",
-                "__nonnull__(x)=",
-                "__register=",
-                "__breakpoint(x)=",
-                "__cdp(x,y,z)=",
-                "__clrex()=",
-                "__clz(x)=0U",
-                "__current_pc()=0U",
-                "__current_sp()=0U",
-                "__disable_fiq()=",
-                "__disable_irq()=",
-                "__dmb(x)=",
-                "__dsb(x)=",
-                "__enable_fiq()=",
-                "__enable_irq()=",
-                "__fabs(x)=0.0",
-                "__fabsf(x)=0.0f",
-                "__force_loads()=",
-                "__force_stores()=",
-                "__isb(x)=",
-                "__ldrex(x)=0U",
-                "__ldrexd(x)=0U",
-                "__ldrt(x)=0U",
-                "__memory_changed()=",
-                "__nop()=",
-                "__pld(...)=",
-                "__pli(...)=",
-                "__qadd(x,y)=0",
-                "__qdbl(x)=0",
-                "__qsub(x,y)=0",
-                "__rbit(x)=0U",
-                "__rev(x)=0U",
-                "__return_address()=0U",
-                "__ror(x,y)=0U",
-                "__schedule_barrier()=",
-                "__semihost(x,y)=0",
-                "__sev()=",
-                "__sqrt(x)=0.0",
-                "__sqrtf(x)=0.0f",
-                "__ssat(x,y)=0",
-                "__strex(x,y)=0U",
-                "__strexd(x,y)=0",
-                "__strt(x,y)=",
-                "__swp(x,y)=0U",
-                "__usat(x,y)=0U",
-                "__wfe()=",
-                "__wfi()=",
-                "__yield()=",
-                "__vfp_status(x,y)=0"
-            ],
-            "intelliSenseMode": "${default}"
         }
     ],
     "version": 4

MDK-ARM/.vscode/keil-assistant.log

@@ -451,39 +451,3 @@
 
 [info] Log at : 2025/3/28|21:46:16|GMT+0800
 
-[info] Log at : 2025/6/6|21:51:18|GMT+0800
-
-[info] Log at : 2025/6/7|17:45:08|GMT+0800
-
-[info] Log at : 2025/6/7|21:03:23|GMT+0800
-
-[info] Log at : 2025/6/12|16:54:50|GMT+0800
-
-[info] Log at : 2025/6/13|16:30:54|GMT+0800
-
-[info] Log at : 2025/6/14|15:14:16|GMT+0800
-
-[info] Log at : 2025/6/16|23:35:32|GMT+0800
-
-[info] Log at : 2025/6/18|17:47:04|GMT+0800
-
-[info] Log at : 2025/6/18|19:43:09|GMT+0800
-
-[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
-

MDK-ARM/.vscode/settings.json

@@ -96,8 +96,6 @@
     "limits": "c",
     "gimbal_task.h": "c",
     "gimgal.h": "c",
-    "navi.h": "c",
+    "navi.h": "c"
-    "up.h": "c",
-    "bsp_usart.h": "c"
   }
 }

MDK-ARM/R2.uvoptx

@@ -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>
+          <Key>DLGUARM</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>
+          <Name></Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
-          <Key>DLGUARM</Key>
+          <Key>CMSIS_AGDI</Key>
-          <Name></Name>
+          <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,67 @@
         <Ww>
           <count>0</count>
           <WinNumber>1</WinNumber>
-          <ItemText>tar_angle</ItemText>
+          <ItemText>Nor_Vx</ItemText>
         </Ww>
         <Ww>
           <count>1</count>
           <WinNumber>1</WinNumber>
-          <ItemText>huart-&gt;ErrorCode</ItemText>
+          <ItemText>Nor_Vy</ItemText>
         </Ww>
         <Ww>
           <count>2</count>
           <WinNumber>1</WinNumber>
-          <ItemText>error_code</ItemText>
+          <ItemText>count,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>3</count>
           <WinNumber>1</WinNumber>
-          <ItemText>posss</ItemText>
+          <ItemText>count</ItemText>
         </Ww>
         <Ww>
           <count>4</count>
           <WinNumber>1</WinNumber>
-          <ItemText>UP,0x0A</ItemText>
+          <ItemText>NUC_StartSending,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>5</count>
           <WinNumber>1</WinNumber>
-          <ItemText>chassis,0x0A</ItemText>
+          <ItemText>chassis</ItemText>
         </Ww>
         <Ww>
           <count>6</count>
           <WinNumber>1</WinNumber>
-          <ItemText>nuc_raw</ItemText>
+          <ItemText>PIAN</ItemText>
         </Ww>
         <Ww>
           <count>7</count>
           <WinNumber>1</WinNumber>
-          <ItemText>cmd_fromnuc,0x0A</ItemText>
+          <ItemText>shang_yaw</ItemText>
         </Ww>
         <Ww>
           <count>8</count>
           <WinNumber>1</WinNumber>
-          <ItemText>drop_message,0x0A</ItemText>
+          <ItemText>pian_yaw</ItemText>
         </Ww>
         <Ww>
           <count>9</count>
           <WinNumber>1</WinNumber>
-          <ItemText>\\R2\../User/task/chassis_task.c\chassis.param-&gt;M3508_param.d</ItemText>
+          <ItemText>pid,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>10</count>
           <WinNumber>1</WinNumber>
-          <ItemText>\\R2\../User/task/nuc_task.c\cmd_fromnuc</ItemText>
+          <ItemText>gyro_kp</ItemText>
+        </Ww>
+        <Ww>
+          <count>11</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>rc_ctrl,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>38</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>cmd_fromnuc</ItemText>
         </Ww>
       </WatchWindow1>
       <Tracepoint>
@@ -250,10 +265,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 +307,7 @@
 
   <Group>
     <GroupName>Application/User/Core</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -432,7 +447,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 +719,7 @@
 
   <Group>
     <GroupName>Drivers/CMSIS</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -724,7 +739,7 @@
 
   <Group>
     <GroupName>Middlewares/FreeRTOS</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -892,18 +907,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 +917,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 +929,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>52</FileNumber>
+      <FileNumber>51</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -938,7 +941,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>53</FileNumber>
+      <FileNumber>52</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -950,7 +953,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>54</FileNumber>
+      <FileNumber>53</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -962,7 +965,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>55</FileNumber>
+      <FileNumber>54</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -974,7 +977,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>56</FileNumber>
+      <FileNumber>55</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -986,7 +989,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>57</FileNumber>
+      <FileNumber>56</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -998,7 +1001,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>58</FileNumber>
+      <FileNumber>57</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1010,7 +1013,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>59</FileNumber>
+      <FileNumber>58</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1022,7 +1025,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>60</FileNumber>
+      <FileNumber>59</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1034,7 +1037,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>61</FileNumber>
+      <FileNumber>60</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1046,7 +1049,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>62</FileNumber>
+      <FileNumber>61</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1066,7 +1069,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 +1081,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>64</FileNumber>
+      <FileNumber>63</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1090,7 +1093,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>65</FileNumber>
+      <FileNumber>64</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1102,7 +1105,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>66</FileNumber>
+      <FileNumber>65</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1114,7 +1117,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>67</FileNumber>
+      <FileNumber>66</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1126,7 +1129,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>68</FileNumber>
+      <FileNumber>67</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1138,7 +1141,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>69</FileNumber>
+      <FileNumber>68</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1150,7 +1153,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>70</FileNumber>
+      <FileNumber>69</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1162,7 +1165,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>71</FileNumber>
+      <FileNumber>70</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1174,7 +1177,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>72</FileNumber>
+      <FileNumber>71</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1194,7 +1197,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 +1209,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>74</FileNumber>
+      <FileNumber>73</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1216,6 +1219,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>9</GroupNumber>
+      <FileNumber>74</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\Algorithm\kalman.c</PathWithFileName>
+      <FilenameWithoutPath>kalman.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
     <File>
       <GroupNumber>9</GroupNumber>
       <FileNumber>75</FileNumber>
@@ -1235,18 +1250,6 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\User\Algorithm\kalman.c</PathWithFileName>
-      <FilenameWithoutPath>kalman.c</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
-    <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>77</FileNumber>
-      <FileType>1</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
       <PathWithFileName>..\User\Algorithm\user_math.c</PathWithFileName>
       <FilenameWithoutPath>user_math.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
@@ -1254,7 +1257,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>78</FileNumber>
+      <FileNumber>77</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1274,7 +1277,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 +1289,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>80</FileNumber>
+      <FileNumber>79</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1298,7 +1301,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>81</FileNumber>
+      <FileNumber>80</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1310,7 +1313,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>82</FileNumber>
+      <FileNumber>81</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1322,7 +1325,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>83</FileNumber>
+      <FileNumber>82</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1334,7 +1337,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>84</FileNumber>
+      <FileNumber>83</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1346,7 +1349,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>85</FileNumber>
+      <FileNumber>84</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1366,7 +1369,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 +1381,7 @@
     </File>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>87</FileNumber>
+      <FileNumber>86</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1390,7 +1393,7 @@
     </File>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>88</FileNumber>
+      <FileNumber>87</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1410,7 +1413,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 +1427,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 +1445,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>91</FileNumber>
+      <FileNumber>90</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1454,7 +1457,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>92</FileNumber>
+      <FileNumber>91</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1466,7 +1469,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>93</FileNumber>
+      <FileNumber>92</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>

MDK-ARM/R2.uvprojx

@@ -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>
@@ -1253,16 +1248,16 @@
               <FileType>1</FileType>
               <FilePath>..\User\Algorithm\ahrs.c</FilePath>
             </File>
-            <File>
-              <FileName>user_cmsis_os2.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\User\Algorithm\user_cmsis_os2.c</FilePath>
-            </File>
             <File>
               <FileName>kalman.c</FileName>
               <FileType>1</FileType>
               <FilePath>..\User\Algorithm\kalman.c</FilePath>
             </File>
+            <File>
+              <FileName>user_cmsis_os2.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\Algorithm\user_cmsis_os2.c</FilePath>
+            </File>
             <File>
               <FileName>user_math.c</FileName>
               <FileType>1</FileType>

Middlewares/Third_Party/Protocol/protocol.h

@@ -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

User/Algorithm/error_detect.h

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

User/Algorithm/user_math.c

@@ -216,21 +216,7 @@ void abs_limit_fp(fp32 *num, fp32 Limit)
         *num = -Limit;
     }
 }
-//改原始值，限制最大最小
+
-fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max)
-{
-    if (*num > Limit_max)
-    {
-			  *num=Limit_max;
-        return Limit_max;
-    }
-    else if (*num < Limit_min)
-    {
-			 *num=Limit_min;
-       return Limit_min;
-			
-    }
-}
 
 /* 移动向量 */
 MoveVector_t *mv;
@@ -405,19 +391,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);
 }

User/Algorithm/user_math.h

@@ -164,7 +164,5 @@ int read_flag_state(uint8_t flag) ;
 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

User/Module/Chassis.c

@@ -3,325 +3,284 @@
 #include "user_math.h"
 #include "bsp_buzzer.h"
 #include "bsp_delay.h"
-/*
+
-机器人坐标系，向前x，右y，上yaw
+
-不同于nuc (前x，左y，上yaw)
+
-            x
+
-            |
+
-              --y
+
+/*机器人坐标系，向前x，右y，上yaw
+   不同于nuc (前x，左y，上yaw)                          */
+/*          
+               x
+               |
+                 --y
+
+
 */
 
-static int8_t Chassis_SetCtrl(Chassis_t *c, CMD_t *ctrl) {
+static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){
-    if (c == NULL) return CHASSIS_ERR_NULL;
+	if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */
-    if (ctrl->CMD_CtrlType == c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode)
+  if (ctrl->CMD_CtrlType== c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode) return CHASSIS_OK; /*模式未改变直接返回*/
-        return CHASSIS_OK;
+   c->chassis_ctrl.ctrl =ctrl->CMD_CtrlType ;
-    c->chassis_ctrl.ctrl = ctrl->CMD_CtrlType;
+   c->chassis_ctrl.mode =ctrl->CMD_mode ;
-    c->chassis_ctrl.mode = ctrl->CMD_mode;
+   
-    return CHASSIS_OK;
+   return CHASSIS_OK;
-}
+}    //设置控制模式
+
+
+/*该函数用来更新can任务获得的电机反馈值*/
 
 int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
-    if (c == NULL) return CHASSIS_ERR_NULL;
+  if (c == NULL) return CHASSIS_ERR_NULL;
-    if (can == NULL) return CHASSIS_ERR_NULL;
+  if (can == NULL) return CHASSIS_ERR_NULL;
-    for (uint8_t i = 0; i < 4; i++) {
+  for (uint8_t i = 0; i < 4; i++) 
-        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;
+	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]) ;
+  return CHASSIS_OK;
-		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] );
-		
-    return CHASSIS_OK;
 }
 
-int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_freq) {
-    if (c == NULL) return CHASSIS_ERR_NULL;
-    c->param = param;
 
-    for (int i = 0; i < 3; i++) {
+int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
-        PID_init(&(c->pid.chassis_3508VecPID[i]), PID_POSITION_D, &(c->param->M3508_param));
+{
-    }
+   if (c == NULL) return CHASSIS_ERR_NULL;
-        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.Mid360AngleInPID), PID_POSITION, &(c->param->Mid360AngleInPID_param));
+   c->param = param; 	/*初始化参数 */
-    PID_init(&(c->pid.Mid360AngleOutPID), PID_POSITION, &(c->param->Mid360AngleOutPID_param));
+	
-		
+		 for(int i =0 ; i < 4 ; i++)
-		
+	{
-		
+		   PID_init(&(c->pid.chassis_3508VecPID[i]),PID_POSITION_D,&(c->param->M3508_param));  //带D项滤波
-    /*修正 */
+	}
-    PID_init(&(c->pid.Chassis_AngleAdjust), PID_POSITION, &(c->param->Chassis_AngleAdjust_param));	
 
-    LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波
+	    PID_init(&(c->pid.chassis_PICKWzPID_HIGN),PID_POSITION,&(c->param->chassis_PICKWzPID_HIGN_param));
-    LowPassFilter2p_Init(&(c->filled[1]), target_freq, 50.0f); 
+	
-    LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f); 
+		  PID_init(&(c->pid.chassis_PICKWzPID_LOW),PID_POSITION,&(c->param->chassis_PICKWzPID_LOW_param));
-    LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f); 
+
-    
+	
-		KalmanCreate(&c->extKalman[0],10,1); 
+	    PID_init(&(c->pid.sick_CaliforYPID),PID_POSITION,&(c->param->Sick_CaliYparam));
-		KalmanCreate(&c->extKalman[1],10,1); 
+	  
-		KalmanCreate(&c->extKalman[2],10,1); 
+	    PID_init(&(c->pid.sick_CaliforXPID),PID_POSITION,&(c->param->Sick_CaliXparam));
-		
+	
-		
+	    LowPassFilter2p_Init(&(c->filled[0]),target_freq,80.0f);  //给角加速度做滤波
-		c->sick_cali .sickparam=c->param ->sickparam ;
+	
-		
+	
-    c->ang_cail.is_open=1;
+	    LowPassFilter2p_Init(&(c->filled[1]),target_freq,80.0f); //给w 做滤波
-    return CHASSIS_OK;
+	
+	    LowPassFilter2p_Init(&(c->filled[2]),target_freq,80.0f);  //给y做滤波
+	
+	    LowPassFilter2p_Init(&(c->filled[3]),target_freq,80.0f); //给x 做滤波
+			
+		//
+	
+	
+	 return CHASSIS_OK;
 }
 
-//void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
+fp32 gyro_kp=1.0f;
-//    fp64 Nor_Vx, Nor_Vy;
+fp32 PIAN=0;
-//    normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy);
+fp32 shang_yaw=0;
-//    
+fp32 pian_yaw=0;
-//    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; // 右后
+void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw)  //底盘逆运动学的解算,
-//    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;  // 左前
+	  fp64 Nor_Vx,Nor_Vy;//归一化后的数据
+	  normalize_vector(Vx,Vy,&Nor_Vx,&Nor_Vy);
+	
+//		c->hopemotorout.OmniSpeedOut[0] = -Vx+Vy+Vw;//右前
+//		c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw;//右后
+//		c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后
+//		c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw;//左前	
+	if(Vw)
+	{	
+	  c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw  ;//右前
+		c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw  ;//右后
+		c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw   ;//左后
+		c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw   ;//左前
+	}
+	else {
+		c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw  + c->pos088 .bmi088.gyro.z*gyro_kp;//右前
+		c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw  + c->pos088 .bmi088.gyro.z*gyro_kp;//右后
+		c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw   + c->pos088 .bmi088.gyro.z*gyro_kp;//左后
+		c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw   + c->pos088 .bmi088.gyro.z*gyro_kp;//左前
+	}
 //	
-//		Chassis_AngleCompensate(c);
+//	if(Vx || Vy){
+//		PIAN = jiuzheng(c->pos088 .imu_eulr .yaw );	
+//		c->hopemotorout.OmniSpeedOut[0] -= PIAN   ;//右前
+//		c->hopemotorout.OmniSpeedOut[1] -= PIAN   ;//右后
+//		c->hopemotorout.OmniSpeedOut[2] -= PIAN   ;//左后
+//		c->hopemotorout.OmniSpeedOut[3] -= PIAN   ;//左前		
+//	}
+//	else 
+//	{
+//		PIAN = 0;
+//		shang_yaw=c->pos088 .imu_eulr .yaw ;
+//		pian_yaw=0;
+//	}
 
-//}
+//	
-void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
+	c->vofa_send[0]=c->pos088.bmi088.gyro.z;
-    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;  // 左前
 	
-		Chassis_AngleCompensate(c);
+	c->vofa_send[1]=PIAN;
-
+	c->vofa_send[2]=c->pos088.imu_eulr.yaw;
+//	c->vofa_send[3]=
+//	c->vofa_send[2]=c->motorfeedback .rotor_rpm3508 [1];
 }
-int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
-    if (c == NULL) return CHASSIS_ERR_NULL;
-    if (ctrl == NULL) return CHASSIS_ERR_NULL;
 
-    Chassis_SetCtrl(c, ctrl);
-    // IMU加滤波修正
-    c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);
 
-    /*初始数据*/
+int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
+{
+	if(c ==NULL)  return CHASSIS_ERR_NULL;
+	if(ctrl ==NULL)  return CHASSIS_ERR_NULL;
+	   
 
-    c->to_nuc.send = 0;
+	  Chassis_SetCtrl(c,ctrl);
-    c->sick_cali.is_reach = 0;
+		 
-	  c->move_vec.Vw=0;
+	 //此处对imu加滤波做修正
-		c->move_vec.Vx = 0;
+	  c->pos088.bmi088.filtered_gyro.z =LowPassFilter2p_Apply(&(c->filled[0]),c->pos088.bmi088.gyro.z);
-		c->move_vec.Vy = 0;
+	  
-
+	switch (c->chassis_ctrl .ctrl)
-    
+	{
+	  case RCcontrol: //手动控制
+		
+/*     
+	在cmd里对数据进行处理 包括方向和油门	  
+	  6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同*/
+		if(c->chassis_ctrl .mode == Pitch){
+	    c->move_vec.Vw = 0;
+		  c->move_vec.Vx = 0;
+		  c->move_vec.Vy = 0;
+		}
+		else if(c->chassis_ctrl .mode == UP_Adjust)
+		{
+	    c->move_vec.Vw = ctrl->Vw*6000;
+		  c->move_vec.Vx = 0;
+		  c->move_vec.Vy = 0;			
+		}
+		else {
+	    c->move_vec.Vw = ctrl->Vw*6000;
+		  c->move_vec.Vx = ctrl->Vy*6000;
+		  c->move_vec.Vy = ctrl->Vx*6000;
+		}
+			
 	
-	
+
-	
+	  break;
-    switch (c->chassis_ctrl.ctrl) {
+
-        case RCcontrol:
+        case AUTO: // 自动模式
-					c->to_nuc.send = 0;
             switch (c->chassis_ctrl.mode) {
-                case Normal:
-                    c->move_vec.Vw = ctrl->Vw * 8000;
-                    c->move_vec.Vx = ctrl->Vy * 8000;
-                    c->move_vec.Vy = ctrl->Vx * 8000;
-                    break;
-                case Pitch:
-                     c->move_vec.Vw = ctrl->Vw * 6000;
-                    c->move_vec.Vx = ctrl->Vy * 6000;
-                    c->move_vec.Vy = ctrl->Vx * 6000;                  
-                    break;
-                case UP_Adjust:
-                    c->move_vec.Vw = ctrl->Vw * 6000;
-                    
-                    break;
-                case Chassis_Adjust_Sick:
-									
-								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);
+                case AUTO_MID360: // 自动雷达
-//                    c->to_nuc.send  = (c->sick_cali.is_reach == 1) ? 1 : 0;
+                    // 全向轮方向, 注意xy方向
-                    break;
+                    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;
 
-            }
-            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);
-								   
-                    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;
-
-                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);									
+                    abs_limit_fp(&c->move_vec.Vw, 5000.0f);
-								    break ;
+
-											 
+							      c->NUC_send .send [0]=0;
-									 }	
+                    break;
-									 break;
+
-										case RELAXED:
+                case AUTO_MID360_Pitch: 
-                   c->move_vec.Vw = 0;
+                    c->move_vec.Vw = 0;
-                   c->move_vec.Vx = 0;
+                    c->move_vec.Vy = 0;
-                   c->move_vec.Vy = 0;											
+                    c->move_vec.Vx = 0;
-                    break		;									
 								
+							     	c->NUC_send .send [0]=0;
+                    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;
 								
+							     	c->NUC_send .send [0]=1;
+                    break;
+                default:
+                    c->move_vec.Vw = 0;
+                    c->move_vec.Vx = 0;
+                    c->move_vec.Vy = 0;
+							     	c->NUC_send .send [0]=0;
 								
-								
+                    break;
-									break;
+            }
             break;
 
         default:
-
+            c->move_vec.Vw = 0;
+            c->move_vec.Vx = 0;
+            c->move_vec.Vy = 0;
             break;
     }
-		
 
     // 电机速度限幅
     abs_limit_fp(&c->move_vec.Vx, 6000.0f);
     abs_limit_fp(&c->move_vec.Vy, 6000.0f);
     abs_limit_fp(&c->move_vec.Vw, 6000.0f);
 
-    Chassis_speed_calculate(c, c->move_vec.Vx, c->move_vec.Vy, c->move_vec.Vw);
+  Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw);
-
-    for (uint8_t i = 0; i < 4; i++) {
-        c->final_out.final_3508out[i] = PID_calc(&(c->pid.chassis_3508VecPID[i]),
-                                                 c->motorfeedback.rotor_rpm3508[i],
-                                                 c->hopemotorout.OmniSpeedOut[i]);
-        out->motor_CHASSIS3508.as_array[i] = c->final_out.final_3508out[i];
-    }
-
-
-		
-    return CHASSIS_OK;
-		
-		
-		
-}
-
-
-
-/*
-sick0,下
-sick1,左2
-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 sick1 = c->sick_cali.sick_dis[1];
-    fp32 sick2 = c->sick_cali.sick_dis[2];
-	  
 	
-    const   sickparam_t *param = &c->sick_cali.sickparam;
-    
-	  
-     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));
+		for (uint8_t i = 0 ; i <4 ; i++)
-
+	{
-    delta_w=PID_calc(&c->pid.SickCaliWzOutPID, diff_yaw, 0);
+		 c->final_out.final_3508out[i] = PID_calc(&(c->pid.chassis_3508VecPID[i]),c->motorfeedback.rotor_rpm3508[i],c->hopemotorout.OmniSpeedOut[i]);
-    delta_x=PID_calc(&c->pid.SickCaliVxOutPID, diff_x, 0);
+	 
-    delta_y=PID_calc(&c->pid.SickCaliVyOutPID, diff_y,0);
+		 out->motor_CHASSIS3508 .as_array[i] = c->final_out.final_3508out[i];
-	
-	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);
-//	}
 	
-		
-    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) {
-            
-            c->sick_cali.is_reach = 1;
-            
-        }
-    } else {
-        reach_cnt = 0;
-			  c->sick_cali.is_reach = 0;
-    }	
-		
-    return CHASSIS_OK;
-}
 
-int8_t Chassis_AngleCompensate(Chassis_t *c) 
+
+//	c->vofa_send[0]=c->pos088.bmi088.gyro.z;
+	
+//	c->vofa_send[1]=c->motorfeedback .rotor_rpm3508 [0];
+//	c->vofa_send[2]=c->motorfeedback .rotor_rpm3508 [1];
+//	c->vofa_send[3]=c->motorfeedback .rotor_rpm3508 [2];
+//  c->vofa_send[4]=c->motorfeedback .rotor_rpm3508 [3];
+//	
+//  c->vofa_send[0]=c->hopemotorout .OmniSpeedOut [0];
+//  c->vofa_send[1]=c->hopemotorout .OmniSpeedOut [0];
+////  c->vofa_send[2]=c->hopemotorout .OmniSpeedOut [2];
+//  c->vofa_send[3]=c->hopemotorout .OmniSpeedOut [3];
+
+////	c->vofa_send[4]=c->motorfeedback .rotor_rpm3508 [0];
+//	c->vofa_send[0]=c->motorfeedback .rotor_rpm3508 [0];
+////	c->vofa_send[6]=c->motorfeedback .rotor_rpm3508 [2];
+//  c->vofa_send[2]=c->motorfeedback .rotor_rpm3508 [3];	
+//	
+//	  c->vofa_send[5]=c->pos088 .imu_eulr .yaw  ;
+
+	return CHASSIS_OK;
+
+}
+	 pid_type_def pid;
+
+	pid_param_t pid_param={ 
+		.p = 0.50f,        
+    .i = 0.0f,         
+    .d = 0.0f,         
+    .i_limit = 50.0f,
+    .out_limit = 100.0f
+	};
+fp32  jiuzheng(fp32 yaw)
 {
-	  if (c == NULL) return CHASSIS_ERR_NULL;
+	static int is=0;
-	  if(c->ang_cail.is_open==0) return 0;
+
 	
-    static fp32 pian_angel,cur_angle;
+	if(is==0)
-	  
+	{
-	  if((c->move_vec.Vx || c->move_vec.Vy) && (c->move_vec.Vw== 0))
+		PID_init (&pid,PID_POSITION ,&pid_param);
-    {
+		is=1;
-			c->ang_cail.ang_error=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);		
+	}
-		   
+	
-		}
+	pian_yaw+= (yaw - shang_yaw)*1000;	
-    
+	shang_yaw=yaw ;
-		else	{
+	
-			 c->ang_cail.ang_cur=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);			
+	return PID_calc (&pid,pian_yaw,0);
-       c->ang_cail.ang_error=0;
-		}
-    c->ang_cail.out = PID_calc(&c->pid.Chassis_AngleAdjust,pian_angel,0);
 }
 
-
-

User/Module/Chassis.h

@@ -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)     
@@ -59,12 +58,7 @@ typedef struct {
  AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
 }ChassisImu_t;
 
-typedef	struct {
+
-	int32_t w_error;//角度纠正时的允许误差量
-	int32_t xy_error;//xy允许误差量		
-	int32_t x_set;
-	int32_t y_set;
-}sickparam_t;
 
 /* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */
 typedef struct
@@ -72,9 +66,7 @@ typedef struct
 	
 	
 	/*该部分决定PID的参数整定在config中修改*/
-  pid_param_t M3508_param;
+  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;
@@ -82,23 +74,10 @@ typedef struct
   pid_param_t NaviVx_param;
   pid_param_t NaviVy_param;
   pid_param_t NaviVw_param;	
-	
+	pid_param_t Sick_CaliYparam;
-	pid_param_t SickCali_WInparam;
+	pid_param_t Sick_CaliXparam;
-	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;
-	
-
-	
-	sickparam_t sickparam;
 	
 }Chassis_Param_t;
 
@@ -112,6 +91,7 @@ typedef struct
 	fp32 mul;//油门倍率
 }ChassisMove_Vec;
 
+
 /**
  *  @brief  
  * 
@@ -161,65 +141,61 @@ typedef struct{
 	pid_type_def chassis_PICKWzPID_HIGN;
 		/*存在较低误差*/
 	pid_type_def chassis_PICKWzPID_LOW;
-	pid_type_def 	Chassis_AngleAdjust;
+		
+		
 	
-	/*sick */
+	pid_type_def	chassis_NaviWzPID;
-	  pid_type_def SickCaliWzInPID;
+		pid_type_def chassis_NaviVxPID;
-	  pid_type_def SickCaliWzOutPID;
+		pid_type_def chassis_NaviVyPID;
-	  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 sick_CaliforYPID;
+	pid_type_def sick_CaliforXPID;
 		
 	}pid;
 
    fp32 vofa_send[8];
-	/*卡尔曼滤波*/
-	 extKalman_t extKalman[3];
-	 LowPassFilter2p_t filled[8]; /* 输出滤波器滤波器数组 */
 	
-	/*角度偏转修正 */
+	 LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
-	struct{
-		
-		int is_open;
-		fp32 ang_error;
-		fp32 ang_cur; 
-		fp32 out;
-		
-		
-	}ang_cail;
 	 
-	struct {
-	fp32 sick_dis[4];   //获取到的sick激光值
-  sickparam_t sickparam;
-	int is_reach;
-	}sick_cali;
 	
-	struct {
+	int32_t sick_dis[4];   //获取到的sick激光值
-	fp32 send;
+	
-	}to_nuc;
 
 	
-	
 }Chassis_t;
 
-int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq);
+/**
+ * @brief 
+ *
+ * @param c 
+ * @param param 
+ * @param mech_zero 
+ * @param wheelPolar 
+ * @return 
+ */
+ int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq);
 
 
+/**
+ * \brief 
+
+ */
 int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can);
 
 
+/**
+ * \brief 
+
+ */
 int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out);
 
 
-int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) ;
+fp32  jiuzheng(fp32 yaw);
-
-int8_t Chassis_AngleCompensate(Chassis_t *c) ;
 
 
+
+
+/// @brief 
+/// @param c 
+void vesc_current_detection(Chassis_t *c);
 #endif

User/Module/config.c

@@ -1,4 +1,4 @@
-#include "config.h"
+ #include "config.h"
 #include "flash.h"
 #include "string.h"
 #define DEBUG
@@ -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,
+    .i_limit = 2000.0f,
-    .out_limit = 20000.0f,
+    .out_limit = 3000.0f,
 },
 .Pitch_M2006_speed_param={
-    .p = 8.5f,         
+    .p = 5.0f,         
-    .i = 0.03f,         
+    .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,79 +93,53 @@ static const ConfigParam_t param ={
 },
 
 
-/*上层其他参数,一些只是初始值，在运行过程中会被更改*/			
+/*上层其他参数*/
+    /*运球*/
+    .DribbleConfig_Config = {
+		.dribble_flag=0,
+    .m3508_init_angle = -5,
+    .m3508_translate_angle = -930,
+    .m3508_dribble_Reverse_speed=-3500,
+    . m3508_dribble_speed= 4000, // 转动速度
+		.m3508_dribble_init_speed=0,
 
-    /*投球*/
+		.light_3508_flag=0,//3508平移处的光电，0初始，1到位置
-    .LaunchCfg = {
+	  .light_ball_flag=0,//检测球的flag			
-    .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 = {
-
-		.go_init = -50,       //仅用在go上电，初始位置
-	  .go_release_pos=-200,
-		.Pitch_angle  =66,
-    .go_up_speed  =15,
-		.go_down_speed =10,
-}, 
-		.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电机下降速度
 
-   
+},
-		},			
+    /*投球*/
+    .PitchConfig_Config = {
+    .m2006_init_angle =-100,    
+	  .m2006_trigger_angle =0,
+    .go1_init_position = -50, 
+		.go1_Receive_ball  = -5, //偏下	
+	  .go1_release_threshold =-210,
+		.m2006_Screw_init=0,	
+		.Pitch_angle  =58,
+},
+	
+	
 	},
 	 .chassis = {/**/
 		
-
+	 
 	 	 .M3508_param = {
-//		 .p = 15.1f,
+		 .p = 15.1f,
-//		 .i = 0.02f,
+		 .i = 0.02f,
-//		 .d = 3.2f,
-//		 .i_limit = 200.0f,
-//		 .out_limit =6000.0f,
-		 .p = 20.5f,
-		 .i = 0.00005f,
 		 .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 ={ 
+		 .chassis_PICKWzPID_HIGN_param ={ //高响应
 			.p = 1.0f,
 			.i = 0.03f,
 			.d = 0.03f,
 			.i_limit = 100.0f,
 			.out_limit =2000.0f,	 
 		 },
-		 	.chassis_PICKWzPID_LOW_param ={ 
+		 	.chassis_PICKWzPID_LOW_param ={ //高精度
 			.p = 0.5f,     //1.0 0.5
 			.i = 0.5f,    //0.01 0.04
 			.d = 0.0f,    //0.02 0.02
@@ -151,137 +147,6 @@ static const ConfigParam_t param ={
 			.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,   
-			.d = 0.0f,    
-			.i_limit = 0.0f,
-			.out_limit =1000.0f,			
-				
-			},
-					
-			.Mid360AngleInPID_param={
-			.p = 0.0f,     
-			.i = 0.0f,   
-			.d = 0.0f,    
-			.i_limit = 0.0f,
-			.out_limit =1000.0f,			
-				
-			},
-			.Mid360AngleOutPID_param={
-			.p = 10.0f,     
-			.i = 0.0f,   
-			.d = 0.0f,    
-			.i_limit = 0.0f,
-			.out_limit =1000.0f,			
-				
-			},
-			
-			
-			
-			
-			.sickparam={
-				.w_error=2,
-				.xy_error=3,
-				.x_set=10000,
-				.y_set=2000,
-				
-			},
-
 				 
    },
 

User/Module/up.c

@@ -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; 	/*初始化参数 */
@@ -39,27 +41,32 @@ 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  ;//赋值
+	if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球
-      u->PitchContext .PitchState   = PITCH_PREPARE;  //状态更新
+		  u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
-      u->PitchContext .is_init = 1;
+      u->DribbleContext .DribbleState  = DRIBBLE_PREPARE;  
+      u->DribbleContext .is_initialized = 1;
+		
     }
-   if(!u->PassContext.is_init)  {
+
-		  u->PassContext .PassCfg  = u->param ->PassCfg  ;//赋值
+	if (!u->PitchContext  .is_initialized) { 
-      u->PassContext .PassState   = PASS_STOP;  //状态更新
+		  u->PitchContext .PitchConfig  = u->param ->PitchConfig_Config  ;//赋值
-      u->PassContext .is_init = 1;		  
+      u->PitchContext .PitchState   = PITCH_PREPARE;  //状态更新,开始夹球
-	 }
+      u->PitchContext .is_initialized = 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层
+     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->DribbleContext .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port  ,LIGHT_C_Pin);
+	u->DribbleContext .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，
 */
@@ -117,8 +151,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;
 	
@@ -128,35 +161,79 @@ 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; //误差量
+
+//	GO_MotorData_t *GO_calibration;//校准前，原始数据
+	   u->motorfeedback .go_data = get_GO_measure_point() ;
+//	if(is_calibration==0)
+//	{
+//		is_calibration=HAL_GPIO_ReadPin (GPIOE ,GPIO_PIN_9 );
+//		u->go_cmd.Pos = -50; //上电之后跑		
+//	  error= GO_calibration->Pos ;
+//	}	  
+//    u->motorfeedback .go_data  = GO_calibration;
+//	  u->motorfeedback .go_data ->Pos= error  ;
+//    u->go_cmd.Pos = pos;
+
+    // 读取参数
+    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 );
-	
+//	DJ_Angle_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5] ,
-	/*俯仰2006，双环，内使用oid角度环，外电机速度环
+//	                 &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(
+	GO_SendData(u,u->motor_target .go_shoot,5 );
-	&u->motorfeedback.go_data,
-	&u->go_cmd,u->motor_target .go_shoot,
-	u->motor_target.go_pull_speed
-	);
 
+	
 	for(int i=0;i<4;i++){
 	out ->motor_UP3508_Dribble.as_array[i]=u->final_out.DJfinal_out [i] ;
 	}
@@ -172,44 +249,46 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
 }
 
 
-fp32 posss=0;
+int a=0;
+int b=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;
-	up_motor_target_t *target=&u->motor_target ;
-		
-
-	/*部分数据更新*/
 	static int is_pitch=1;
 	
+	
+
     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 ;
+		u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ;
-		target->Pitch_angle  =  LaunchCfg->Pitch_angle;		
+		u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
 		is_pitch=0;	
-		} //初始上电
+			} //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删
-		//LaunchCfg->go_up_speed=15;
+		u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
-//		target->Pitch_angle =LaunchCfg->Pitch_angle; 
+		u->PitchContext .PitchState   = PITCH_PREPARE;  //状态更新,开始夹球
-		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;
+		RELAY1_TOGGLE(0);//关闭气缸
+			for(int i=0;i<3;i++){
+	    	u->motor_target.Dribble_M3508_speed[i]=0;
+			}
+    u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_translate_angle;
+	  u->DribbleContext .DribbleState  = DRIBBLE_PREPARE; //重置最初状态
+			
+			
+
 				break;
 			
 		case Pitch :
@@ -218,253 +297,301 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
 		      u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
 				}
 
-			 Pitch_Process(u,out,c);
+			 Pitch_Process(u,out);
-
+//				if (u->PitchContext .PitchState  ==PITCH_PREPARE) 
+//				{
+//				     u->CoContext .CoState =CoTRANSLATE_OUT;
+//				}		
+//        Co_Process(u,out);
 
 				 break ;
-		case UP_Adjust: //测试用,手动使用pitch下的cfg
+		case UP_Adjust: //测试用
-			  
+			
-			  pitch_cfg ->go_release_pos += c->Vx ;
+				u->PitchContext.PitchConfig.go1_Receive_ball += c->Vx ;
-	      LaunchCfg->Pitch_angle += c->Vy/100;
+	      u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100;
 	
-		    target->go_shoot=pitch_cfg ->go_release_pos;
+		    u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_Receive_ball;
-		    target->Pitch_angle=LaunchCfg->Pitch_angle;
+		    u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
 			break ;
-				default:
+		case Dribble:
-			break;
+		{
+
+			if(u->DribbleContext.DribbleState== DRIBBLE_PREPARE){
+				u->DribbleContext .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;
-			
-			
-			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)
-{
-	/*简化*/
-	PitchCfg_t *cfg = &u->PitchContext.PitchConfig;
-	PitchState_t *state =&u->PitchContext .PitchState;
-  up_motor_target_t *target=&u->motor_target ;
-  LaunchContext_t *LaunchContext = &u->LaunchContext;
-	// 可根据实际需要传入不同的起始和末位置,起始：当前位置
-	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;
-		
-		    *state=PITCH_WAIT;
-		break;
-		
-		case PITCH_WAIT:
-			 
-    if(LaunchContext->LaunchState==Launch_DONE)
-         *state=PITCH_COMPLETE;
-		break;
-		case PITCH_COMPLETE:
-			
-			break;
-		default:
-			break;
-		
-	}
-  
-	
-    return 0;
-	
-}
-
-
-
-int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
-{
-
-    PassCfg_t *PassCfg = &u->PassContext.PassCfg;
-	  PassState_t *state = &u->PassContext.PassState;
-	  PassContext_t *PassContext = &u->PassContext;
-	   up_motor_target_t *target=&u->motor_target ;
-    LaunchContext_t *LaunchContext = &u->LaunchContext;
-	
-	  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->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);
-			}
-			else {
-					MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis)+0.1f,3.3,4.3,&u->MID360Context.Curve);
-				
-			}
-		}
-			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 	AUTO_MID360_Pitch:
-				if(MID360Context->IsLaunch==0){
+		 u->PitchContext .PitchConfig .go1_Receive_ball=LowPassFilter2p_Apply(&u->filled[0],c->pos);
-				MID360Context->IsLaunch=1;
+
-				LaunchContext->LaunchState = Launch_PREPARE;
+			if (u->PitchContext .PitchState  ==PITCH_PREPARE) 
+				{
+		      u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
 				}
-			  Pitch_Launch_Sequence(u,c,LaunchContext,u->motorfeedback.go_data.Pos,MID360Cfg->go_release_pos,out);
+				/*根据距离实时计算所需pos*/
-			
+				
+//       u->PitchContext .PitchConfig .go1_release_threshold=c->pos;
+			 Pitch_Process(u,out);
 			break ;
 				
 			case AUTO_MID360:
-					target->Shoot_M2006_angle=	LaunchContext->LaunchCfg.m2006_init;
+						u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
-				MID360Context->IsLaunch=0;
+
-//		   	u->cmd_from_nuc.lock=0;
+						u->PitchContext .PitchState   = PITCH_PREPARE;  
+
+			
+			
 				break ;
-					default:
-			break;
 			
 		}
+			
+			
+return 0;
+
+	
+}
+
+
+}
+
+
+
+
+
+
+
+
+
+
+
+int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
+{
+
+	switch(u->PitchContext .PitchState){
+	
+		case PITCH_START:
+//					u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 
+
+			u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold;
 		
-	}
+		if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
+			u->motor_target .Shoot_M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0
+		
+			u->PitchContext .PitchState=PITCH_PULL_TRIGGER;
+}//更改标志位
+
+			break ;
+		case PITCH_PULL_TRIGGER:
+			
+		
+			if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1)  //当2006的总角度小于1，可以认为已经勾上,误差为1
+			{
+         u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_Receive_ball; 
+				if(is_reached (u->motorfeedback .go_data ->Pos ,u->motor_target .go_shoot ,1.0f))
+			  {
+				u->PitchContext .PitchState=PITCH_LAUNCHING;			
+			  }
+			}
+			break ;
+		
+		case PITCH_LAUNCHING: //等待发射	
+			u->PitchContext .PitchState=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;//是否共速
+	
+
+	switch (u->DribbleContext.DribbleState) {
+
+		case DRIBBLE_TRANSLATE:
+		u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 
+
+		if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主
+		{
+			u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态
+		}
+			break;
+		case DRIBBLE_PROCESS_DOWN:
+			
+			u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
+			u->motor_target.Dribble_M3508_speed[1]=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
+			u->motor_target.Dribble_M3508_speed[2]=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
+
+		  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,50)
+			) {
+   		  	RELAY1_TOGGLE(1);  //速度达到后打开气缸
+				  common_speed_flag =1;
+			}
+			if(common_speed_flag){
+				if(u->DribbleContext .DribbleConfig .light_ball_flag == 0){//球下落检测，反转
+				u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
+				u->motor_target.Dribble_M3508_speed[1]=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
+				u->motor_target.Dribble_M3508_speed[2]=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
+
+			  u->DribbleContext .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(u->DribbleContext .DribbleConfig .light_ball_flag == 1){
+			     u->DribbleContext .DribbleState=DRIBBLE_DONE;
+				   RELAY1_TOGGLE(0);  //关闭气缸
+					 }
+				 }
+				 break ;
+		case DRIBBLE_DONE:
+			common_speed_reverse_flag=0;
+			for(int i=0;i<3;i++){
+			  u->motor_target.Dribble_M3508_speed[i]=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0
+			}
+		
+        /*标志位清零*/
+		    u->DribbleContext.DribbleConfig.light_ball_flag=0;
+		    u->DribbleContext.DribbleConfig.light_3508_flag=0;
+		
+		
+			break;
+		default:
+			// 处理未知状态
+			break;
+	}
+    return 0;
+}
+//    CoPREPARE,            // 准备阶段
+//	  CoTRANSLATE_OUT,//上方平移,去运球
+//	  CoDRIBBLE,       //运球阶段
+//	  CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
+//	  CoPITCH,       //发射
+//	  CoDONE,        
+	  
+int8_t Co_Process(UP_t *u, CAN_Output_t *out)
+{
+	switch(u->CoContext .CoState )
+	{
+		case CoPREPARE:
+			
+		break ;
+		case CoTRANSLATE_OUT:
+			u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 
+
+		if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主
+			{
+				u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，运球转移状态
+				u->PitchContext .PitchState = PITCH_START;
+
+				u->CoContext .CoState =CoDRIBBLE;
+			}
+		break;
+
+		case CoDRIBBLE:
+			 Dribble_Process(u,out);  //状态停在DRIBBLE_DONE
+			 Pitch_Process(u,out);    //状态停在PITCH_PULL_TRIGGER
+		//状态停在对应位置时,平移去给发射送球
+		if(u->DribbleContext .DribbleState ==DRIBBLE_DONE && u->PitchContext.PitchState ==PITCH_LAUNCHING)
+		{
+			u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle;			
+		}
+		if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleContext .DribbleConfig.m3508_init_angle,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;
+
+			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->DribbleContext .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->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 
+			
+			}
+			
+		  if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主,卡到最右端
+			{
+				u->CoContext .CoState =CoPITCH;
+			}
+    break ;
+			
+    case CoPITCH:
+			
+		    u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; 
+
+			
+			break ;
+
+	}
+	
+	
+	
+
+
+}
+

User/Module/up.h

@@ -13,7 +13,6 @@
 #include "vofa.h"	
 #include "GO_M8010_6_Driver.h"
 #include "bsp_usart.h"
-#include "up_utils.h"
 
 /*
  状态机
@@ -28,105 +27,100 @@
 
 
 */
-/*共用发射，重复部分*/
+/*配合过程状态,co是合作的意思*/
+typedef enum {
+    CoPREPARE,            // 准备阶段
+	  CoTRANSLATE_OUT,//上方平移,去运球
+	  CoDRIBBLE,       //运球和蓄力阶段,再平移回去
+	  CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
+	  CoPITCH,       //发射
+	  CoDONE,        
+	  
+
+}CoState_t;
+/*总配合上下文*/
 typedef struct {
-	  fp32 m2006_init;   // M2006初始角度
+    CoState_t CoState;
-	  fp32 m2006_trig;   // M2006触发角度0,用来合并扳机
+
-	  fp32 go_init;
+    uint8_t is_initialized ;
-	  fp32 go_pull_pos; // go上升去合并扳机，扳机位置  
+} CoContext_t;
-    fp32 go_up_speed;	
+
-	  fp32 go_down_speed;
+
-	 
-	  fp32 Pitch_angle;
-} LaunchCfg_t;
-typedef enum  {
-	  Launch_Stop,
-    Launch_PREPARE,        //
-	  Launch_START,          //启动，从指定位置上升，扣动扳机
-    Launch_TRIGGER,        //拉动到指定位置  
-    Launch_SHOOT_WAIT,           // 发射等待
-	  Launch_SHOOT,
-	  Launch_DONE,
-    
-	
-}LaunchState_t;
-typedef struct {
-	
-    LaunchCfg_t LaunchCfg;
-	  LaunchState_t LaunchState;
-} LaunchContext_t;
 
 
 /* 投球状态定义 */
 typedef enum {
-	
     PITCH_PREPARE,          // 准备阶段
-	  PITCH_START,            //启动，拉扳机 
+	  PITCH_START,            //启动，拉扳机
-	  PITCH_WAIT,             // 发射等待
+    PITCH_PULL_TRIGGER,  
+    PITCH_LAUNCHING,        // 发射中
+	
     PITCH_COMPLETE          // 完成
 	
 } PitchState_t;
 
 /* 投球参数配置 */
 typedef struct {
-    
+    fp32 m2006_init_angle;      // M2006初始角度-140
-    fp32 go_init;     // GO电机触发位置,初始位置，后续更改用于发射的位置	  
+	  fp32 m2006_trigger_angle;   // M2006触发角度0,用来合并扳机
-	  fp32 Pitch_angle;  //俯仰角度，以oid为准	  
+    fp32 go1_init_position;     // GO电机触发位置,0,初始位置
-	  fp32 go_release_pos;//go松开，发射位置
+	  fp32 go1_release_threshold; // go上升去合并扳机，扳机位置
-	fp32 go_up_speed;
+	  fp32 go1_Receive_ball;      //用在配合过程，用来接平移后的球
-	fp32 go_down_speed;
+	  fp32 m2006_Screw_init;
+	  fp32 Pitch_angle;
 	
-} PitchCfg_t;
+} PitchConfig_t;
 
+/* 投球控制上下文 */
 typedef struct {
     PitchState_t PitchState;
-	  PitchCfg_t PitchConfig;
+	  PitchConfig_t PitchConfig;
-    uint8_t is_init ;
+
+    uint8_t is_initialized ;
 } PitchContext_t;
 
-/*传球过程 */
+
+
+
+
+
+
+
+/*运球*/
 typedef enum {
-	  PASS_STOP,         // 停止状态,未进入传球模式
+    DRIBBLE_PREPARE,        // 准备阶段
-	  PASS_IDLE,         // 空闲状态，进入未开始
+	  DRIBBLE_START,
-    PASS_PREPARE,      // 传球准备
+    DRIBBLE_TRANSLATE,      // 平移过程
-    PASS_START,        // 启动传球,拉动到等球位置
+		DRIBBLE_PROCESS_DOWN,        // 运球过程，球下落
-    PASS_POS_PREPARE,  // 传球位置准备，对准篮筐，go的位置
+	  DRIBBLE_PROCESS_UP,        // 运球过程，球上升
-    PASS_COMPLETE      // 发射
+    DRIBBLE_DONE            // 运球结束
-} PassState_t;
+} DribbleState_t;
+
+/* 参数配置结构体 */
 typedef struct {
+	int8_t dribble_flag;//当移动三摩擦后为1，否则为0，防止发射撞到
+	   
+    fp32 m3508_init_angle;     // 平移前位置
+    fp32 m3508_translate_angle;     // 平移后位置
+	  fp32 m3508_dribble_init_speed;
+    fp32 m3508_dribble_speed;  // 转动速度
+    fp32 m3508_dribble_Reverse_speed;
+
 	
-	fp32 go_wait;     // GO等待位置
+	/*光电标志位，初始值均为0，触发为1*/
-	fp32 go_release_pos; // GO电机发射位置
+		int light_3508_flag;//3508平移处的光电，0初始，1到位置
-	fp32 Pitch_angle;  // 俯仰角度，以oid为准
+	  int light_ball_flag;//检测球的flag
-	fp32 go_up_speed; // GO电机上升速度
-	fp32 go_down_speed; // GO电机下降速度
 	
-} PassCfg_t;
+} DribbleConfig_t;
+
+/* 状态机上下文 */
 typedef struct {
-    PassState_t PassState;
+    DribbleState_t DribbleState;
-	  PassCfg_t PassCfg;
+	  DribbleConfig_t DribbleConfig;
-    CurveType Curve;  //当前函数，雷达距离->pos
+	   
-    uint8_t is_init ;
+    uint8_t is_initialized;
-} 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;
-
 
+} DribbleContext_t;
 
 
 typedef struct {
@@ -150,28 +144,80 @@ typedef struct
 	pid_param_t Pitch_M2006_angle_param;
 
   pid_param_t Pitch_Angle_M2006_speed_param;
+	
+	pid_param_t Dribble_M3508_speed_param;//三摩擦用的速度环
+		 
+	pid_param_t Dribble_translate_M3508_speed_param;//平移用的速度环
+	pid_param_t Dribble_translate_M3508_angle_param;//平移用的角度环
 	 
-	LaunchCfg_t LaunchCfg;
+	
-	PitchCfg_t  PitchCfg;
+	DribbleConfig_t DribbleConfig_Config;
-	PassCfg_t   PassCfg;
+	PitchConfig_t  PitchConfig_Config;
-	MID360Cfg_t MID360Cfg;
 	
 	GO_MotorCmd_t go_cmd;
 }UP_Param_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;  
+   CoContext_t	CoContext;
+	/*运球过程*/
+  DribbleContext_t DribbleContext;
+    /*投篮过程*/
+	PitchContext_t PitchContext;
+		  
+	 CMD_t *cmd;	
+
+	 struct{	
+		 
+	 struct{	
+	  fp32 VESC_5065_M1_rpm;
+	  fp32 VESC_5065_M2_rpm;
+	 }VESC;
+
+	  GO_MotorData_t *go_data;//存放go电机数据
+
+    DJmotor_feedback_t DJmotor_feedback[6];
+		
+    struct { 
+	   uint32_t ecd;
+	   float angle;
+     }Encoder;	
+	}motorfeedback;
+ 
+
+	
+	 struct{
 
 	  fp32 VESC_5065_M1_rpm;
 	  fp32 VESC_5065_M2_rpm;
 
 	  fp32 Shoot_M2006_angle;
+		fp32 Pitch_M2006_angle;
 		 
 		fp32 go_shoot;
-    fp32 go_pull_speed;
+		fp32 Dribble_M3508_speed[3];//运球转速
-	  fp32 Pitch_angle;  //以oid的角度为准,目标俯仰角
+		fp32 Dribble_translate_M3508_angle;//平移用的3508转动角度
-		 
-	}up_motor_target_t;
 
-typedef struct{
+		fp32 Shoot_Pitch_angle;
+		 
+	}motor_target;
+  
+	 struct{
 		 
 		 
 		 pid_type_def VESC_5065_M1;
@@ -185,47 +231,18 @@ typedef struct{
 		 
 		 pid_type_def Pitch_Angle_M2006_speed;		 
 		 
+		 pid_type_def Dribble_M3508_speed[3];//三摩擦用的速度环
 		 
-	}up_pid_t;
+		 pid_type_def Dribble_translate_M3508_speed;//平移用的速度环
-
+		 pid_type_def Dribble_translate_M3508_angle;//平移用的角度环
-typedef struct{
+	 
-	
-  uint8_t up_task_run; 
-  const	UP_Param_t *param; 
-  LaunchContext_t LaunchContext;	
-    /*投篮过程*/
-	PitchContext_t PitchContext;
-	/*传球过程*/
-	PassContext_t PassContext;
-  /*自动过程*/	
-	MID360Context_t MID360Context;
-	 CMD_t *cmd;	
-
-	 struct{	
 		 
-	 struct{	
+	}pid;
-	  fp32 VESC_5065_M1_rpm;
+ 	   GO_MotorCmd_t go_cmd;
-	  fp32 VESC_5065_M2_rpm;
-	 }VESC;
-	  GO_MotorData_t go_data;//存放go电机数据
 
-    DJmotor_feedback_t DJmotor_feedback[6];
+		 
-		
-    struct { 
-	   uint32_t ecd;
-	   float angle;
-     }Encoder;	
-	
-	}motorfeedback;
- 
 
 	
-
-  up_pid_t pid;
-  up_motor_target_t motor_target;
-
- 	GO_MotorCmd_t go_cmd;
-	
 		/*经PID计算后的实际发送给电机的实时输出值*/
     struct 
 	{
@@ -235,32 +252,29 @@ typedef struct{
 		
 
 	}final_out;
-//	    struct { 
+	
-//	   float angle;
-//		 float dis;
-//		uint8_t lock;
-//     }cmd_from_nuc;	
 	 LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
 	
 	fp32 vofa_send[8];
 	
+
+	
 }	UP_t;
 
 
 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 Dribble_Process(UP_t *u, CAN_Output_t *out);
-int8_t Pass_Sequence_Check(UP_t *u,CMD_t *c);
+int8_t Pitch_Process(UP_t *u, CAN_Output_t *out);
-int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c);
+int8_t Co_Process(UP_t *u, CAN_Output_t *out);
-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);
 
 
 #endif

User/Module/up_utils.c

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

User/Module/up_utils.h

@@ -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

User/bsp/protocol.h

@@ -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

User/device/can_use.c

@@ -1,10 +1,6 @@
-
 /*
   CAN总线上的设备1到7
   将所有CAN总线上挂载的设备抽象成单个设备进行管理和控制
-	
-	
-	底盘4个3508，3个dt35接收板，终端电阻勿开，会出现数据收发异常情况
 */
 
 /* Includes ----------------------------------------------------------------- */
@@ -14,9 +10,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)
 
@@ -32,7 +32,7 @@
 #define CAN_M3508_MAX_ABS_CUR (20)
 #define CAN_M2006_MAX_ABS_CUR (10)
 
-#define CAN_ENCODER_RESOLUTION (4096) //欧艾迪编码器分辨率
+#define CAN_ENCODER_RESOLUTION (32768) //欧艾迪编码器分辨率
 
  CAN_RawRx_t raw_rx1;//原始的can数据
  CAN_RawRx_t raw_rx2;
@@ -56,39 +56,37 @@ 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,uint16_t id,
 	                            const uint8_t *raw) {
 	if (feedback == NULL || raw == NULL) return;	
-		static	uint16_t sick_rec[4];
+  switch 	(id)
-   switch 	(id)
 	{
-	 	case CAN_SICK_ID_1:
+		case CAN_SICK_ID_1:
-	 		feedback->raw_dis[0] = (uint16_t)(raw[0] << 8| raw[1]);	
+			feedback->raw_dis[0] = (uint16_t)(raw[0] << 8| raw[1]);	
 
-	 		break ;
+			break ;
 		
-	 	case CAN_SICK_ID_2:
+		case CAN_SICK_ID_2:
-	 		feedback->raw_dis[1] = (uint16_t)(raw[0] << 8| raw[1]);	
+			feedback->raw_dis[1] = (uint16_t)(raw[0] << 8| raw[1]);	
 			
-	 		break ;
+			break ;
 						
-	 	case CAN_SICK_ID_3:
+		case CAN_SICK_ID_3:
-	 		feedback->raw_dis[2] = (uint16_t)(raw[0] << 8| raw[1]);	
+			feedback->raw_dis[2] = (uint16_t)(raw[0] << 8| raw[1]);	
 
-	 		break ;	
+			break ;	
 						
-	 	case CAN_SICK_ID_4:
+		case CAN_SICK_ID_4:
-	 		feedback->raw_dis[3] = (uint16_t)(raw[0] << 8| raw[1]);	
+			feedback->raw_dis[3] = (uint16_t)(raw[0] << 8| raw[1]);	
-
-	 		break ;
-	 }	
 
+			break ;
+	}		
 }
 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
@@ -294,7 +292,26 @@ void CAN_Encoder_Control(CAN_t *can)
       break;
   }
   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;
@@ -368,19 +385,18 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
         detect_hook(CHASSIS3508M1_TOE + index);
         break;
         case CAN_SICK_ID_1:
- 					   CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_1, can_rx->rx_data);	
+					   CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_1, can_rx->rx_data);	
-         break;
+        break;
-         case CAN_SICK_ID_2:
+        case CAN_SICK_ID_2:
-				 	   CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_2, can_rx->rx_data);		
+					   CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_2, can_rx->rx_data);		
-         break;
+        break;
-         case CAN_SICK_ID_3:
+        case CAN_SICK_ID_3:
-				 	   CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_3, can_rx->rx_data);		
+					   CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_3, can_rx->rx_data);		
-         break;
+        break;
-         case CAN_SICK_ID_4:
+        case CAN_SICK_ID_4:
-              CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_4, can_rx->rx_data);
+             CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_4, can_rx->rx_data);
 		
-         break;
+        break;
-
       default:
         break;
     }

User/device/can_use.h

@@ -22,7 +22,6 @@ typedef enum {
 	CAN_SICK_ID_2=0x302,
 	CAN_SICK_ID_3=0x303,
 	CAN_SICK_ID_4=0x304,
-
 	
 	CAN_Encoder_ID=0x01,
 	
@@ -195,10 +194,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,uint16_t id,
 	                            const uint8_t *raw) ;
 #endif

User/device/cmd.c

@@ -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->cmd_MID360.angle = n->MID360.angle;
+			cmd->pos =n->MID360 .pos ;
-			cmd->cmd_MID360.dis = n->MID360.dis;
+			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;
-		
-			
 						
 
-		
+		}
         return 0;
 }
 
@@ -137,27 +146,25 @@ 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 =Normal; //左上，右上，手动调整
-		
-		if(rc->dr16.res > 3000) cmd ->CMD_mode =UP_Adjust;
 	}
 	
 	else if(rc->dr16.sw_l==CMD_SW_MID)
 	{
 		
 		cmd ->CMD_CtrlType =AUTO;		
-		if(rc->dr16.sw_r ==CMD_SW_UP) 	cmd ->CMD_mode =AUTO_MID360_Pitch; //左中，右中，雷达	
+		if(rc->dr16.sw_r ==CMD_SW_UP) 	cmd ->CMD_mode =AUTO_MID360; //左中，右中，雷达	
 		
-		if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =AUTO_MID360; //左中，右中，无模式	
+		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 =AUTO_MID360_Pitch; //左中，右下，视觉
 		
 	}
 	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_UP) cmd ->CMD_mode =Normal; //左下，右上，投篮
-		if(rc->dr16.sw_r ==CMD_SW_MID) 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,21 +191,20 @@ 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; 
 	else if(rc->LD.key_E ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360_Pitch;
 	else if(rc->LD .key_E ==CMD_SW_MID ) cmd ->CMD_mode =AUTO_MID360;		
 	else cmd ->CMD_mode =Normal ;
-	    }
+	
 	}
 	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 cmd ->CMD_mode =Normal;
 	}
 }	

User/device/cmd.h

@@ -14,7 +14,6 @@
 typedef enum{
 	RCcontrol,//遥控器控制，左按键上，控制上层
 	AUTO,
-  PASS_BALL,//传球模式
 	RELAXED,//异常模式
 
  }CMD_CtrlType_t;
@@ -26,28 +25,28 @@ typedef  enum{
 	
 	  AUTO_MID360,
 	  AUTO_MID360_Pitch,
-	  REPOSITION,//雷达调整
+	  AUTO_MID360_Adjust,//雷达调整
 	
 	  UP_Adjust,//上层调整
-   	Chassis_Adjust_Sick,
-	/*传球占三个,逻辑在up.h里，简化就这样写了*/
-	  PB_UP,
-    PB_MID,
-    PB_DOWN,
 
-   
+    Dribble ,  //运球
-	  Pitch,     
+	  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 +141,9 @@ typedef struct {
 			}cmd_pick;
 				struct 
 		{
-			 fp32 angle;
+			 fp32 posx;
-			 fp32 dis;
+			 fp32 posy;
-
+			 fp32 posw;
 			}cmd_MID360;
 		 
 

User/device/device.h

@@ -28,7 +28,8 @@ extern "C" {
 #define SIGNAL_IST8310_MAGN_RAW_REDY (1u << 9)
 #define SIGNAL_ACTION_RAW_REDY (1u << 10)
 #define SIGNAL_OPSTIMER_REDY  (1u << 11)
-#define SIGNAL_RC_RAW_REDY  (1u <<13)
+#define SIGNAL_R12DS_BUF1_REDY (1u << 12)
+#define SIGNAL_DR16_RAW_REDY  (1u <<13)
 
 #define SIGNAL_AI_RAW_REDY (1u << 15)
 #define SIGNAL_KEY_REDY (1u << 16)

User/device/nuc.c

@@ -2,145 +2,169 @@
 #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];
+uint8_t nucbuf[31];
-char SendBuffer[7];
 
-static void NUC_CBLTCallback(void)
+
-{
+static void NUC_IdleCallback(void) {	
-  osThreadFlagsSet(thread_alert, SIGNAL_NUC_RAW_REDY);
+	osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
-  // detect_hook(NUC_TOE);
+	detect_hook(NUC_TOE);
 }
-static void NUC_ERRORCALLBACK(void)
+int8_t NUC_Init(NUC_t *nuc){
-{
+    if(nuc == NULL) return DEVICE_ERR_NULL;
-  NUC_Restart();
+    if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
-  uint32_t error_code = HAL_UART_GetError(BSP_UART_GetHandle(BSP_UART_NUC));
+   BSP_UART_RegisterCallback(BSP_UART_NUC,BSP_UART_IDLE_LINE_CB,
-  // osThreadFlagsSet(thread_alert, SIGNAL_NUC_ERROR);
+                            NUC_IdleCallback);
+	
+   return DEVICE_OK;
 }
-int8_t NUC_Init(NUC_t *nuc)
+int8_t NUC_StartReceiving() {
-{
+  if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_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),
                            (uint8_t *)nucbuf,
                            sizeof(nucbuf)) == HAL_OK)
     return DEVICE_OK;
   return DEVICE_ERR;
 }
-int8_t NUC_Restart(void)
+  char SendBuffer[19];
-{
+
+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)
+bool_t NUC_WaitDmaCplt(void) {
-{
+  return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) ==
-  return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll, 0) ==
           SIGNAL_NUC_RAW_REDY);
 }
 
-int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc)
+
-{
+int8_t NUC_RawParse(CMD_NUC_t *n){
-  if (n == NULL)
+	if(n ==NULL) return DEVICE_ERR_NULL;
-    return DEVICE_ERR_NULL;
+ 	union 
-  union
+	{
-  {
+		float x[5];
-    float x[2];
+		char data[20];
-    char data[8];
+	}instance;
-  } instance;
+  if(nucbuf[0]!=HEAD) goto error;  //发送ID不是底盘
-  if (nucbuf[0] != HEAD)
+  else{
-    goto error; // 发送ID不是底盘
+    n->status_fromnuc =nucbuf[1];
-  else
+		n->ctrl_status =nucbuf[2];
-  {
+    switch (n->status_fromnuc)
-      if (nucbuf[9] != TAIL)
+    {
-        goto error;
+		case VISION://控制帧0x02
-			
+				/* 协议格式 
-			
+		   0xFF  HEAD
-      instance.data[3] = nucbuf[4];
+		   0x02  控制帧
-      instance.data[2] = nucbuf[3];
+		   0x01  相机帧
-      instance.data[1] = nucbuf[2];
+		   vx    fp32
-      instance.data[0] = nucbuf[1];
+		   vy    fp32
-      n->MID360.angle = instance.x[0]; //
+		   wz    fp32
-      instance.data[7] = nucbuf[8];
+		   0xFE  TAIL
-      instance.data[6] = nucbuf[7];
+		  */
-      instance.data[5] = nucbuf[6];
-      instance.data[4] = nucbuf[5];
-      n->MID360.dis = instance.x[1]; //
 
 
-    nuc->unc_online = true; // 设置为在线状态
+		instance.data[3] = nucbuf[3];
-    return DEVICE_OK;
+       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;
+
+
+    }
+     return DEVICE_OK;
   }
-error:
+    error:
-  drop_message++;
+    drop_message++;
-  NUC_Restart();
+    return DEVICE_ERR;
-  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)
+	if(cmd == NULL) return DEVICE_ERR_NULL;
-    return DEVICE_ERR_NULL;
+	 memset(cmd, 0, sizeof(*cmd));
-  nuc->unc_online = false; // 设置为离线状态
+	return 0;
-  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;
-//  }
-}
 

User/device/nuc.h

@@ -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; //发送的数据协议
+  NUC_UpPackageMCU_t to_nuc; //发送的数据协议
-
-  struct {
-    NUC_UpPackageIMU_t imu;
-    NUC_UpPackageCMD_t cmd;
-  } 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_RawParse(CMD_NUC_t *n);
-int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc);
+int8_t NUC_HandleOffline(CMD_NUC_t *cmd);
-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);
-
 #endif
 

User/device/rc.c

@@ -1,15 +1,11 @@
 /*
-       遥控
+       DR16接收机
-       dr16：dbus       乐迪：s-ubs
-			 dr16掉线不输出信号，可以使用超时检测方式判断掉线
-			 即RC_SBUS_WaitDmaCplt
-			 乐迪掉线依然有信号输出，只是部分通道变为特定值，无法使用以上方式
 
 */
 
 /* Includes ----------------------------------------------------------------- */
 #include "rc.h"
-
+#include "cmd.h"
 #include <string.h>
 
 #include "bsp_usart.h"
@@ -42,14 +38,14 @@ int8_t RC_SBUS_Init(void )
   if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
 
   BSP_UART_RegisterCallback(BSP_UART_REMOTE, BSP_UART_IDLE_LINE_CB,
-                            RC_RxCpltCallback);
+                            RC_SBUS_RxCpltCallback);
 	memset(cbuf, 0, sizeof(cbuf));//初始化清空数据包
 	
   return DEVICE_OK;
 }
 
-static void RC_RxCpltCallback(void) {
+static void RC_SBUS_RxCpltCallback(void) {
-  osThreadFlagsSet(thread_alert, SIGNAL_RC_RAW_REDY);
+  osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY);
 //	detect_hook(DR16_TOE);
 }
 int8_t RC_SBUS_Restart(void) {
@@ -57,7 +53,7 @@ int8_t RC_SBUS_Restart(void) {
   __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_REMOTE));
   return DEVICE_OK;
 }
-int8_t RC_StartDmaRecv(void) {	
+int8_t RC_SBUS_StartDmaRecv(void) {	
 	
   if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_REMOTE),
                            (uint8_t *)cbuf,
@@ -65,9 +61,9 @@ int8_t RC_StartDmaRecv(void) {
     return DEVICE_OK;
     return DEVICE_ERR;
 }
-bool RC_WaitDmaCplt(uint32_t timeout) {
+bool RC_SBUS_WaitDmaCplt(uint32_t timeout) {
-  return (osThreadFlagsWait(SIGNAL_RC_RAW_REDY, osFlagsWaitAll, timeout) ==
+  return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
-          SIGNAL_RC_RAW_REDY);
+          SIGNAL_DR16_RAW_REDY);
 }
 /*乐迪数据解析 */
 int8_t LD_ParseRaw( LD_raw_t *raw, LD_Data_t *LD)
@@ -104,15 +100,15 @@ int8_t LD_ParseRaw( LD_raw_t *raw, LD_Data_t *LD)
 		
 		
 if (raw->ch[0] < 0)
-    raw->map_ch[0] = map_fp32(raw->ch[0], -696, 2, -1, 0);
+    raw->map_ch[0] = map_fp32(raw->ch[0], -700, -2, -1, 0);
 else
-    raw->map_ch[0] = map_fp32(raw->ch[0], 2, 704, 0, 1);
+    raw->map_ch[0] = map_fp32(raw->ch[0], -2, 700, 0, 1);
 
 // ch[1]
 if (raw->ch[1] < 0)
-    raw->map_ch[1] = map_fp32(raw->ch[1], -638, 5, -1, 0);
+    raw->map_ch[1] = map_fp32(raw->ch[1], -700, 4, -1, 0);
 else
-    raw->map_ch[1] = map_fp32(raw->ch[1], 5, 762, 0, 1);
+    raw->map_ch[1] = map_fp32(raw->ch[1], 4, 700, 0, 1);
 
     raw->map_ch[2] = map_fp32(raw->ch[2], 2, 1377, 0, 1);
 
@@ -201,7 +197,7 @@ int8_t DR16_ParseRaw(DR16_t *dr16)
 	 dr16->data.ch_l_y = ((cbuf[4] >>1) | (cbuf[5] <<7));
 	 dr16->data.sw_r = ((cbuf[5] >>4));
    dr16->data.sw_l = ((cbuf[5] >> 4) & 0x000C) >> 2;             
-	 dr16->data.res=  (fp32)((cbuf[16]|cbuf[17] << 8));
+	
 	return 1;
 }
 int8_t RC_ParseRC( DR16_t *dr16,LD_raw_t *LD_raw, LD_Data_t *LD, CMD_RC_t *rc) {
@@ -224,11 +220,13 @@ int8_t RC_ParseRC( DR16_t *dr16,LD_raw_t *LD_raw, LD_Data_t *LD, CMD_RC_t *rc) {
   rc->dr16.ch_l_x = 2 * ((float)dr16->data.ch_l_x - RC_CH_VALUE_MID) / full_range;
   rc->dr16.ch_l_y = 2 * ((float)dr16->data.ch_l_y - RC_CH_VALUE_MID) / full_range;
 
+//  rc->dr16.sw_l = (CMD_SwitchPos_DR16_t)dr16->data.sw_l;
+//  rc->dr16.sw_r = (CMD_SwitchPos_DR16_t)dr16->data.sw_r;
   rc->dr16.sw_l = (CMD_SwitchPos_t)dr16->data.sw_l;
   rc->dr16.sw_r = (CMD_SwitchPos_t)dr16->data.sw_r;
 
   rc->dr16.key = dr16->data.key;
-  rc->dr16.res = ((float)dr16->data.res - RC_CH_VALUE_MID) ;
+
 //  rc->ch_res = ((float)dr16->data.res - DR16_CH_VALUE_MID) / full_range;
 	
 	

User/device/rc.h

@@ -11,10 +11,6 @@
 /* Exported constants ------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */
 /* Exported types ----------------------------------------------------------- */
-
-
-
-
 typedef  __packed struct  
 {
   uint16_t ch_r_x : 11;
@@ -73,10 +69,10 @@ typedef  __packed struct
 
 
 int8_t RC_SBUS_Init(void );
-static void RC_RxCpltCallback(void) ;
+static void RC_SBUS_RxCpltCallback(void) ;
-int8_t RC_Restart(void) ;
+int8_t RC_SBUS_Restart(void) ;
-int8_t RC_StartDmaRecv(void) ;
+int8_t RC_SBUS_StartDmaRecv(void) ;
-bool RC_WaitDmaCplt(uint32_t timeout) ;
+bool RC_SBUS_WaitDmaCplt(uint32_t timeout) ;
 int8_t LD_ParseRaw( LD_raw_t *raw, LD_Data_t *LD);
 static bool DR16_DataCorrupted(const DR16_t *dr16) ;
 int8_t DR16_ParseRaw(DR16_t *dr16);

User/device/vofa.c

@@ -5,61 +5,35 @@
 //用来对需要的数据进行串口绘图  （未添加接收函数）
 
 
-//float vofa_send[8];
-
-////发送函数
-//void vofa_tx_main(float *data)
-//{
-//  float fdata[8]={0};
-//	uint8_t tail[4];
-//	tail[0]=0x00;
-//	tail[1]=0x00;
-//	tail[2]=0x80;
-//	tail[3]=0x7f;
-//	/*在下面添加发送的数据*/
-//	fdata[0] = data[0];
-//	fdata[1] = data[1];
-//	fdata[2] = data[2];
-//	fdata[3] = data[3];
-//	fdata[4] = data[4];
-//	fdata[5] = data[5];
-//	fdata[6] = data[6];
-//	fdata[7] = data[7];
-//	
-//		
-//	/*在下面使用对应的串口发送函数*/
-////	CDC_Transmit_FS( ( uint8_t *)fdata, sizeof(fdata));
-////	osDelay(1);
-////	CDC_Transmit_FS( tail, 4);
-////	HAL_UART_Transmit_DMA(&huart1, ( uint8_t *)fdata, sizeof(fdata));
-////	osDelay(1);
-//	HAL_UART_Transmit_DMA(&huart1, tail, 4);
-//	osDelay(1);
-
-//		
-//}
-
 float vofa_send[8];
 
 //发送函数
 void vofa_tx_main(float *data)
 {
-    float fdata[8] = {0};
+  float fdata[8]={0};
-    uint8_t tail[4] = {0x00, 0x00, 0x80, 0x7f};
+	uint8_t tail[4];
+	tail[0]=0x00;
+	tail[1]=0x00;
+	tail[2]=0x80;
+	tail[3]=0x7f;
+	/*在下面添加发送的数据*/
+	fdata[0] = data[0];
+	fdata[1] = data[1];
+	fdata[2] = data[2];
+	fdata[3] = data[3];
+	fdata[4] = data[4];
+	fdata[5] = data[5];
+	fdata[6] = data[6];
+	fdata[7] = data[7];
+	
+		
+	/*在下面使用对应的串口发送函数*/
 
-   
+	HAL_UART_Transmit_DMA(&huart6, ( uint8_t *)fdata, sizeof(fdata));
-    for (int i = 0; i < 8; i++) {
+	osDelay(1);
-        fdata[i] = data[i];
+	HAL_UART_Transmit_DMA(&huart6, tail, 4);
-    }
+	osDelay(1);
 
- 
+		
-    uint8_t packet[8 * sizeof(float) + 4];
+}
-    memcpy(packet, fdata, 8 * sizeof(float));         
-    memcpy(packet + 8 * sizeof(float), tail, 4);      
 
-    /*在下面使用对应的串口发送函数*/
-//		HAL_UART_Transmit_DMA(&huart6, packet, sizeof(packet));
-    CDC_Transmit_FS( packet, sizeof(packet));
-      osDelay(1);
-   
-}

User/task/atti_esti.c

@@ -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);
-    
+    osKernelUnlock();
-    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);
 		
     /* PID控制IMU温度，PWM输出 */
     BSP_PWM_Set(BSP_PWM_IMU_HEAT,

User/task/can_task.c

@@ -47,6 +47,7 @@ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
       
     
 		 //一问一答sick数据指令
+//	  CAN_Sick_Control(&can);
     CAN_Encoder_Control(&can);
 
 		/*can设备数据存入队列*/

User/task/chassis_task.c

@@ -34,7 +34,7 @@ static Chassis_Ctrl_t ctrl;
 
 #endif
 
-fp32 freq;
+
 
 /**
  * \brief 底盘任务
@@ -50,18 +50,13 @@ 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)
 		{
 #ifdef DEBUG
     task_runtime.stack_water_mark.chassis = osThreadGetStackSpace(osThreadGetId());
-			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);
@@ -90,18 +85,17 @@ 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);  //发送数据		
 	vofa_send[0] = chassis.vofa_send[0];
-  vofa_send[1] = chassis.vofa_send[1];
+   	vofa_send[1] = chassis.vofa_send[1];
 	vofa_send[2] = chassis.vofa_send[2];
 	vofa_send[3] = chassis.vofa_send[3];
 	vofa_send[4] = chassis.vofa_send[4];
-  vofa_send[5] = chassis.vofa_send[5];
+   	vofa_send[5] = chassis.vofa_send[5];
 	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);
 		 

User/task/cmd_task.c

@@ -30,9 +30,8 @@ void Task_cmd(void *argument){
    while(1){
 #ifdef DEBUG
     /*记录任务所需要的栈空间*/
-    task_runtime.stack_water_mark.cmd =osThreadGetStackSpace(osThreadGetId());	 
+    task_runtime.stack_water_mark.cmd =
-		 task_runtime.last_up_time.cmd=tick;
+        osThreadGetStackSpace(osThreadGetId());	 
-		 task_runtime.freq.cmd=TASK_FREQ_CTRL_CMD;
 #endif
 
     osKernelLock(); /*锁住RTOS内核调度*/
@@ -40,15 +39,10 @@ void Task_cmd(void *argument){
     /*将各任务接收到的原始数据解析为通用的控制命令*/
 		 
 		 
-
+		 /*注意，不能将nuc和码盘导航一块使用*/
 		 
-		if((osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK)){ //nuc	
+		if(osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK) //nuc
-		}
+      CMD_ParseNuc(&cmd,&Nuc);
-		else
-		{
-		   memset(&Nuc, 0, sizeof(CMD_NUC_t));
-		}
-	   CMD_ParseNuc(&cmd,&Nuc);
 		
 		
 		if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器

User/task/init.c

@@ -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 */
@@ -92,9 +85,7 @@ void Task_Init(void *argument) {
 	
    task_runtime.msgq.cmd.raw.nuc =
       osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL);	 
-   task_runtime.msgq.cmd.raw.nuc_send =
+
-	    osMessageQueueNew(3u,sizeof(NUC_send_t), NULL);	
-			
   osKernelUnlock();
   osThreadTerminate(osThreadGetId()); /* 结束自身 */
 }

User/task/nuc_task.c

@@ -5,82 +5,47 @@
 
 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.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());
 #endif
 			NUC_StartReceiving();
-      if (NUC_WaitDmaCplt()){ 
+//			NUC_StartSending(NUC_send.send);
-         NUC_RawParse(&cmd_fromnuc, &nuc_raw);
+      if (NUC_WaitDmaCplt()){
-         last_online_tick = tick;
+         NUC_RawParse(&cmd_fromnuc);
       }
       else{
-        if (tick - last_online_tick > 300) 
+        NUC_HandleOffline(&cmd_fromnuc);
-					NUC_HandleOffline(&cmd_fromnuc, &nuc_raw);
       }
-
+     osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc);
-      if (nuc_raw.unc_online) {
+     osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0);
-        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);
+			
 		 }
-		
+		 
+		 
+		 
 }
 	 

User/task/rc_task.c

@@ -20,9 +20,7 @@ LD_Data_t  LD;
 #else
 static DR16_t dr16;
 static CMD_RC_t cmd_rc;
-static LD_raw_t LD_raw;
+ static RC_ctrl_t cc;
-static LD_Data_t  LD;
-
 
 #endif
 
@@ -36,9 +34,7 @@ static LD_Data_t  LD;
  */
 void Task_rc(void *argument) {
   (void)argument; /* 未使用，消除警告 */
-	const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_RC;
 
-  uint32_t tick = osKernelGetTickCount();
   RC_SBUS_Init(); /* 初始化 */
 
   while (1) {
@@ -47,25 +43,17 @@ void Task_rc(void *argument) {
     task_runtime.stack_water_mark.rc = osThreadGetStackSpace(osThreadGetId());
 #endif
     /* 开启DMA */
-    RC_StartDmaRecv();
+    RC_SBUS_StartDmaRecv();
-    if (RC_WaitDmaCplt(30)) {
+
+    if (RC_SBUS_WaitDmaCplt(30)) {
       			
       RC_ParseRC(&dr16,&LD_raw,&LD, &cmd_rc);
     } else {
-      /* 处理遥控器离线*/
+      /* 处理dr16遥控器离线 ,乐迪不可用*/
-			
+      DR16_HandleOffline(&dr16, &cmd_rc);
-			
+			LD_HandleOffline(&LD,&cmd_rc);
-
-     DR16_HandleOffline(&dr16, &cmd_rc);
-     LD_HandleOffline(&LD,&cmd_rc);
-     
-			
-			
     }
     osMessageQueueReset(task_runtime.msgq.cmd.raw.rc);
     osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0);
-			tick += delay_tick;
-        osDelayUntil(tick);
-		 
   }
 }

User/task/up_task.c

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

User/task/user_task.h

@@ -13,14 +13,15 @@
 
 /* 所有任务都要定义自己的任务运行频率 */
 
-
+// 分配的频率该如何给定?
 #define TASK_FREQ_CHASSIS (900u)
 #define TASK_FREQ_UP (900u)
 
-#define TASK_FREQ_CTRL_CMD (100u)
+#define TASK_FREQ_CTRL_CMD (500u)
-#define TASK_FREQ_NUC (100u)
+#define TASK_FREQ_NUC (500u)
 #define TASK_FREQ_CAN (1000u)
-#define TASK_FREQ_RC (100u)
+#define TASK_FREQ_R12DS (1000u)
+#define TASK_FREQ_RC (1000u)
 
 #define TASK_FREQ_ERROR_DTC (3u)
 
@@ -54,23 +55,20 @@ 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 nuc;
         osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
-				osMessageQueueId_t nuc_send; //给nuc发
+				
+		 
       }raw;
-
+			
 			/*控制分离*/
       osMessageQueueId_t UP_cmd_ctrl_t;
 			osMessageQueueId_t CHASSIS_cmd_ctrl_t;
+
+				
 			
       osMessageQueueId_t status;
     } cmd;