将改后的运球和发射合并到up_branch

AUTO_CHASSIS.ioc

@@ -145,24 +145,23 @@ Mcu.Pin24=PA4
 Mcu.Pin25=PC4
 Mcu.Pin26=PC5
 Mcu.Pin27=PE9
-Mcu.Pin28=PE11
+Mcu.Pin28=PA7
-Mcu.Pin29=PA7
+Mcu.Pin29=PB0
 Mcu.Pin3=PB3
-Mcu.Pin30=PB0
+Mcu.Pin30=VP_CRC_VS_CRC
-Mcu.Pin31=VP_CRC_VS_CRC
+Mcu.Pin31=VP_FREERTOS_VS_CMSIS_V2
-Mcu.Pin32=VP_FREERTOS_VS_CMSIS_V2
+Mcu.Pin32=VP_SYS_VS_Systick
-Mcu.Pin33=VP_SYS_VS_Systick
+Mcu.Pin33=VP_TIM4_VS_ClockSourceINT
-Mcu.Pin34=VP_TIM4_VS_ClockSourceINT
+Mcu.Pin34=VP_TIM7_VS_ClockSourceINT
-Mcu.Pin35=VP_TIM7_VS_ClockSourceINT
+Mcu.Pin35=VP_TIM10_VS_ClockSourceINT
-Mcu.Pin36=VP_TIM10_VS_ClockSourceINT
+Mcu.Pin36=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
-Mcu.Pin37=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
 Mcu.Pin4=PA14
 Mcu.Pin5=PA13
 Mcu.Pin6=PB7
 Mcu.Pin7=PB6
 Mcu.Pin8=PD0
 Mcu.Pin9=PC11
-Mcu.PinsNb=38
+Mcu.PinsNb=37
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32F407IGHx
@@ -273,8 +272,6 @@ PD14.GPIOParameters=GPIO_Label
 PD14.GPIO_Label=Buzzer
 PD14.Locked=true
 PD14.Signal=S_TIM4_CH3
-PE11.Locked=true
-PE11.Signal=GPIO_Output
 PE9.GPIOParameters=PinState,GPIO_PuPd,GPIO_Label
 PE9.GPIO_Label=FlagForUpper
 PE9.GPIO_PuPd=GPIO_PULLUP

Core/Inc/stm32f4xx_hal_conf.h

@@ -190,7 +190,7 @@
 #define  USE_HAL_SMBUS_REGISTER_CALLBACKS       0U /* SMBUS register callback disabled     */
 #define  USE_HAL_SPI_REGISTER_CALLBACKS         0U /* SPI register callback disabled       */
 #define  USE_HAL_TIM_REGISTER_CALLBACKS         0U /* TIM register callback disabled       */
-#define  USE_HAL_UART_REGISTER_CALLBACKS        0U /* UART register callback disabled      */
+#define  USE_HAL_UART_REGISTER_CALLBACKS        1U /* UART register callback disabled      */
 #define  USE_HAL_USART_REGISTER_CALLBACKS       0U /* USART register callback disabled     */
 #define  USE_HAL_WWDG_REGISTER_CALLBACKS        0U /* WWDG register callback disabled      */
 

Core/Src/gpio.c

@@ -63,9 +63,6 @@ void MX_GPIO_Init(void)
   /*Configure GPIO pin Output Level */
   HAL_GPIO_WritePin(FlagForUpper_GPIO_Port, FlagForUpper_Pin, GPIO_PIN_SET);
 
-  /*Configure GPIO pin Output Level */
-  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_RESET);
-
   /*Configure GPIO pin Output Level */
   HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_SET);
 
@@ -102,13 +99,6 @@ void MX_GPIO_Init(void)
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
   HAL_GPIO_Init(FlagForUpper_GPIO_Port, &GPIO_InitStruct);
 
-  /*Configure GPIO pin : PE11 */
-  GPIO_InitStruct.Pin = GPIO_PIN_11;
-  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-  HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
-
   /*Configure GPIO pin : PtPin */
   GPIO_InitStruct.Pin = GYRO_CS_Pin;
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;

Core/Src/main.c

@@ -79,9 +79,7 @@ int main(void)
 
   /* MCU Configuration--------------------------------------------------------*/
 
-  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
+  /* Reset of all peripherals, Initializes the Flash interface and the Systick. *   HAL_Init();
-
-  HAL_Init();
 
   /* USER CODE BEGIN Init */
 
@@ -96,6 +94,7 @@ int main(void)
 
   /* Initialize all configured peripherals */
   MX_GPIO_Init();
+	
   MX_DMA_Init();
   MX_SPI1_Init();
   MX_TIM4_Init();

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

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

MDK-ARM/AUTO_CHASSIS.uvoptx

@@ -12,7 +12,7 @@
     <lExt>*.lib</lExt>
     <tExt>*.txt; *.h; *.inc; *.md</tExt>
     <pExt>*.plm</pExt>
-    <CppX>*.cpp; *.cc; *.cxx</CppX>
+    <CppX>*.cpp</CppX>
     <nMigrate>0</nMigrate>
   </Extensions>
 
@@ -22,11 +22,11 @@
   </DaveTm>
 
   <Target>
-    <TargetName>R2_UP</TargetName>
+    <TargetName>AUTO_CHASSIS</TargetName>
     <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>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
       </DebugOpt>
       <TargetDriverDllRegistry>
+        <SetRegEntry>
+          <Number>0</Number>
+          <Key>ST-LINKIII-KEIL_SWO</Key>
+          <Name>-U00160029510000164E574E32 -O206 -SF5000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO131091 -TC12000000 -TT12000000 -TP21 -TDS8005 -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,8 +139,8 @@
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
-          <Key>ST-LINKIII-KEIL_SWO</Key>
+          <Key>CMSIS_AGDI</Key>
-          <Name>-U-O206 -O206 -SF10000 -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 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
+          <Name>-X"Horco CMSIS-DAP" -U4626385832 -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC168000000 -TT168000000 -TP20 -TDS8010 -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,20 +158,97 @@
         <Ww>
           <count>0</count>
           <WinNumber>1</WinNumber>
-          <ItemText>aaa,0x0A</ItemText>
+          <ItemText>raw,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>1</count>
           <WinNumber>1</WinNumber>
+          <ItemText>UP,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>2</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>can,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>3</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>can_out,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>4</count>
+          <WinNumber>1</WinNumber>
           <ItemText>bbb,0x0A</ItemText>
         </Ww>
+        <Ww>
+          <count>5</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>aaa,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>6</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>CCC</ItemText>
+        </Ww>
+        <Ww>
+          <count>7</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>GO_motor_info</ItemText>
+        </Ww>
+        <Ww>
+          <count>8</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>cbuf[36]</ItemText>
+        </Ww>
+        <Ww>
+          <count>9</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>dr16,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>10</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>cmd,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>11</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>rc_ctrl,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>12</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>up_cmd,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>13</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>angle</ItemText>
+        </Ww>
+        <Ww>
+          <count>14</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>can_ZZ</ItemText>
+        </Ww>
+        <Ww>
+          <count>15</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>flaggg,0x0A</ItemText>
+        </Ww>
       </WatchWindow1>
+      <WatchWindow2>
+        <Ww>
+          <count>0</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>a</ItemText>
+        </Ww>
+      </WatchWindow2>
       <Tracepoint>
         <THDelay>0</THDelay>
       </Tracepoint>
       <DebugFlag>
         <trace>0</trace>
-        <periodic>0</periodic>
+        <periodic>1</periodic>
         <aLwin>1</aLwin>
         <aCover>0</aCover>
         <aSer1>0</aSer1>
@@ -204,18 +286,18 @@
       <pSingCmdsp></pSingCmdsp>
       <pMultCmdsp></pMultCmdsp>
       <DebugDescription>
-        <Enable>1</Enable>
+        <Enable>0</Enable>
         <EnableFlashSeq>0</EnableFlashSeq>
         <EnableLog>0</EnableLog>
         <Protocol>2</Protocol>
-        <DbgClock>10000000</DbgClock>
+        <DbgClock>5000000</DbgClock>
       </DebugDescription>
     </TargetOption>
   </Target>
 
   <Group>
     <GroupName>Application/MDK-ARM</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -247,7 +329,7 @@
 
   <Group>
     <GroupName>Application/User/Core</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -387,7 +469,7 @@
 
   <Group>
     <GroupName>Drivers/STM32F4xx_HAL_Driver</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -659,7 +741,7 @@
 
   <Group>
     <GroupName>Drivers/CMSIS</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -679,7 +761,7 @@
 
   <Group>
     <GroupName>Middlewares/FreeRTOS</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -886,6 +968,30 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
+      <PathWithFileName>..\User\bsp\gpio.c</PathWithFileName>
+      <FilenameWithoutPath>gpio.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>53</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\bsp\spi.c</PathWithFileName>
+      <FilenameWithoutPath>spi.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>54</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
       <PathWithFileName>..\User\bsp\pwm.c</PathWithFileName>
       <FilenameWithoutPath>pwm.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
@@ -893,7 +999,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>53</FileNumber>
+      <FileNumber>55</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -905,7 +1011,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>54</FileNumber>
+      <FileNumber>56</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -917,7 +1023,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>55</FileNumber>
+      <FileNumber>57</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -929,7 +1035,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>56</FileNumber>
+      <FileNumber>58</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -941,7 +1047,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>57</FileNumber>
+      <FileNumber>59</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -953,7 +1059,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>58</FileNumber>
+      <FileNumber>60</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -965,7 +1071,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>59</FileNumber>
+      <FileNumber>61</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -977,7 +1083,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>60</FileNumber>
+      <FileNumber>62</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -987,30 +1093,6 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
-    <File>
-      <GroupNumber>7</GroupNumber>
-      <FileNumber>61</FileNumber>
-      <FileType>1</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
-      <PathWithFileName>..\User\bsp\bsp_spi.c</PathWithFileName>
-      <FilenameWithoutPath>bsp_spi.c</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
-    <File>
-      <GroupNumber>7</GroupNumber>
-      <FileNumber>62</FileNumber>
-      <FileType>1</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
-      <PathWithFileName>..\User\bsp\bsp_gpio.c</PathWithFileName>
-      <FilenameWithoutPath>bsp_gpio.c</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
   </Group>
 
   <Group>
@@ -1074,8 +1156,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\User\task\nuc_task.c</PathWithFileName>
+      <PathWithFileName>..\User\task\error_detect_task.c</PathWithFileName>
-      <FilenameWithoutPath>nuc_task.c</FilenameWithoutPath>
+      <FilenameWithoutPath>error_detect_task.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -1086,8 +1168,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\User\task\error_detect_task.c</PathWithFileName>
+      <PathWithFileName>..\User\task\dr16_task.c</PathWithFileName>
-      <FilenameWithoutPath>error_detect_task.c</FilenameWithoutPath>
+      <FilenameWithoutPath>dr16_task.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -1122,8 +1204,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\User\task\dr16_task.c</PathWithFileName>
+      <PathWithFileName>..\User\task\nuc_task.c</PathWithFileName>
-      <FilenameWithoutPath>dr16_task.c</FilenameWithoutPath>
+      <FilenameWithoutPath>nuc_task.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -1282,8 +1364,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\User\device\GO_M8010_6_Driver.c</PathWithFileName>
+      <PathWithFileName>..\User\device\dr16.c</PathWithFileName>
-      <FilenameWithoutPath>GO_M8010_6_Driver.c</FilenameWithoutPath>
+      <FilenameWithoutPath>dr16.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -1294,8 +1376,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\User\device\dr16.c</PathWithFileName>
+      <PathWithFileName>..\User\device\GO_M8010_6_Driver.c</PathWithFileName>
-      <FilenameWithoutPath>dr16.c</FilenameWithoutPath>
+      <FilenameWithoutPath>GO_M8010_6_Driver.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -1347,7 +1429,7 @@
 
   <Group>
     <GroupName>Application/User/USB_DEVICE/Target</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -1367,7 +1449,7 @@
 
   <Group>
     <GroupName>Middlewares/USB_Device_Library</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>

MDK-ARM/AUTO_CHASSIS.uvprojx

@@ -7,7 +7,7 @@
 
   <Targets>
     <Target>
-      <TargetName>R2_UP</TargetName>
+      <TargetName>AUTO_CHASSIS</TargetName>
       <ToolsetNumber>0x4</ToolsetNumber>
       <ToolsetName>ARM-ADS</ToolsetName>
       <pArmCC>5060960::V5.06 update 7 (build 960)::.\ARMCC</pArmCC>
@@ -49,8 +49,8 @@
             <NotGenerated>0</NotGenerated>
             <InvalidFlash>1</InvalidFlash>
           </TargetStatus>
-          <OutputDirectory>.\R2_UP\</OutputDirectory>
+          <OutputDirectory>ELE_CHASSIS\</OutputDirectory>
-          <OutputName>R2_UP</OutputName>
+          <OutputName>AUTO_CHASSIS</OutputName>
           <CreateExecutable>1</CreateExecutable>
           <CreateLib>0</CreateLib>
           <CreateHexFile>0</CreateHexFile>
@@ -82,7 +82,7 @@
           </BeforeMake>
           <AfterMake>
             <RunUserProg1>0</RunUserProg1>
-            <RunUserProg2>0</RunUserProg2>
+            <RunUserProg2>1</RunUserProg2>
             <UserProg1Name></UserProg1Name>
             <UserProg2Name></UserProg2Name>
             <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
@@ -1128,6 +1128,16 @@
               <FileType>1</FileType>
               <FilePath>..\User\bsp\delay.c</FilePath>
             </File>
+            <File>
+              <FileName>gpio.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\bsp\gpio.c</FilePath>
+            </File>
+            <File>
+              <FileName>spi.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\bsp\spi.c</FilePath>
+            </File>
             <File>
               <FileName>pwm.c</FileName>
               <FileType>1</FileType>
@@ -1173,16 +1183,6 @@
               <FileType>1</FileType>
               <FilePath>..\User\bsp\bsp_buzzer.c</FilePath>
             </File>
-            <File>
-              <FileName>bsp_spi.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\User\bsp\bsp_spi.c</FilePath>
-            </File>
-            <File>
-              <FileName>bsp_gpio.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\User\bsp\bsp_gpio.c</FilePath>
-            </File>
           </Files>
         </Group>
         <Group>
@@ -1208,16 +1208,16 @@
               <FileType>1</FileType>
               <FilePath>..\User\task\can_task.c</FilePath>
             </File>
-            <File>
-              <FileName>nuc_task.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\User\task\nuc_task.c</FilePath>
-            </File>
             <File>
               <FileName>error_detect_task.c</FileName>
               <FileType>1</FileType>
               <FilePath>..\User\task\error_detect_task.c</FilePath>
             </File>
+            <File>
+              <FileName>dr16_task.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\task\dr16_task.c</FilePath>
+            </File>
             <File>
               <FileName>up_task.c</FileName>
               <FileType>1</FileType>
@@ -1280,9 +1280,9 @@
               </FileOption>
             </File>
             <File>
-              <FileName>dr16_task.c</FileName>
+              <FileName>nuc_task.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\User\task\dr16_task.c</FilePath>
+              <FilePath>..\User\task\nuc_task.c</FilePath>
             </File>
           </Files>
         </Group>
@@ -1349,16 +1349,16 @@
               <FileType>1</FileType>
               <FilePath>..\User\device\vofa.c</FilePath>
             </File>
-            <File>
-              <FileName>GO_M8010_6_Driver.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\User\device\GO_M8010_6_Driver.c</FilePath>
-            </File>
             <File>
               <FileName>dr16.c</FileName>
               <FileType>1</FileType>
               <FilePath>..\User\device\dr16.c</FilePath>
             </File>
+            <File>
+              <FileName>GO_M8010_6_Driver.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\GO_M8010_6_Driver.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -2044,7 +2044,7 @@
       <component Cclass="CMSIS" Cgroup="CORE" Cvendor="ARM" Cversion="5.3.0" condition="ARMv6_7_8-M Device">
         <package name="CMSIS" schemaVersion="1.3" url="http://www.keil.com/pack/" vendor="ARM" version="5.6.0"/>
         <targetInfos>
-          <targetInfo name="R2_UP"/>
+          <targetInfo name="AUTO_CHASSIS"/>
         </targetInfos>
       </component>
     </components>

README.md

@@ -9,4 +9,3 @@ r2上层代码
 3.16 up.c里3508需增加位置环，id1，2分别为2006，3508位置环，适应升降机构。
      go电机需换rs485模块测试，目前存在可控但收不到反馈数据的情况
 
-3.26 遥控器控制修改

User/Algorithm/navi.c

@@ -0,0 +1,216 @@
+#include "navi.h"
+
+
+//导航初始化一系列参数
+int8_t Action_init(ops_t *o , const ops_param_t *param , Action_POS_t *pos)
+{
+	if(o ==NULL)  return DEVICE_ERR;
+	if(param == NULL) return DEVICE_ERR;
+//	while(!Action_ready)
+//	{		
+//     TIM4->CCR3 = 9999;//等待全场定位初始化
+//	}
+	
+//    TIM4->CCR3 = 0;
+//	HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET);//接收到码盘数据后亮灯指示
+//	
+	
+//			osDelay(5);
+
+	o->param = param;
+	/*全场定位pid初始化*/ 
+	//全场定位跑路径的xy方向速度环pid
+	PID_init(&o->pid_PosSpeed_x, PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_x_param));
+	PID_init(&o->pid_PosSpeed_y, PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_y_param));
+	 /* 初始化角度纠正PID */
+	PID_init(&o->pid_OutAngle,PID_POSITION,(&o->param->ops_pid.pid_OutAngle_param));//外环
+	PID_init(&o->pid_InnerAngle,PID_POSITION,(&o->param->ops_pid.pid_InnerAngle_param));//内环
+	//底盘xy方向位置环pid初始化
+	//内环
+	PID_init(&o->pid_pos_x_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_x_inner_param));
+	PID_init(&o->pid_pos_y_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_y_inner_param));
+	//外环
+	PID_init(&o->pid_pos_x_out,PID_POSITION,(&o->param->ops_pid.pid_pos_x_out_param));
+	PID_init(&o->pid_pos_y_out,PID_POSITION,(&o->param->ops_pid.pid_pos_y_out_param));
+
+
+	
+	//底盘路径速度pid初始化
+	PID_init(&o->path_speed_pid,PID_POSITION,(&o->param->ops_pid.path_speed_pid_param));
+	//全场定位底盘坐标初始化
+	o->chassis_pos->pos_x=0;
+	o->chassis_pos->pos_y=0;	
+	o->chassis_pos->pos_lastX=0;
+	o->chassis_pos->pos_lastY=0;
+	o->chassis_map =param->path ;	
+	o->chassis_pos = pos;
+	
+	
+	//设置全场定位允许的误差范围
+	o->state.mistake = POS_ALLOW_MISTAKE;
+	o->state .angle_mistake =POS_ALLOW_ANGLE_MISTAKE ;
+	o->state.moveState = START;
+	o->state.points_num = param->path_num;
+	//标志位初始化
+	o->POS_IS_CPT = NO;
+  
+
+
+
+	return DEVICE_OK;
+}
+//利用C板imu纠正角度
+fp32 ops9_AngleCorr(ops_t *o,fp32 hope_angle)
+{
+	fp32 delta_angle,delta_w;
+	//外环角度纠正
+	delta_angle = PID_calc(&o->pid_OutAngle,o->ops_imu_pos.yaw,hope_angle);
+	//内环速度纠正
+	delta_w = -PID_calc(&o->pid_InnerAngle,o->ops_gyro.z,delta_angle);
+	return delta_w;
+}
+
+//底盘xy速度计算(全场定位)
+void POS_chassis_set(ops_t *o,fp32 vx_set, fp32 vy_set, fp32 yaw_angle_set){
+		o->final_out.vx = (PID_calc(&o->pid_PosSpeed_x, o->chassis_pos->pos_Vx,vx_set));//根据遥控器的方向选择正负
+		o->final_out.vy = PID_calc(&o->pid_PosSpeed_y, o->chassis_pos->pos_Vy,vy_set);
+	  o->final_out.yaw_angle = ops9_AngleCorr(o,yaw_angle_set);
+}
+
+
+//定点控制
+void hold_point(ops_t *o,point_t p,fp32 yaw_angle_set)
+{
+	fp32 delta_x,delta_y;
+	fp32 delta_vx,delta_vy;
+	fp32 vx_set,vy_set;//x,y,w目标速度
+	
+	//x
+	delta_x = PID_calc(&o->pid_pos_x_out,o->chassis_pos->pos_x,p.x);
+	delta_vx = PID_calc(&o->pid_pos_x_inner,o->chassis_pos->pos_Vx,delta_x);
+	
+	//y
+	delta_y = PID_calc(&o->pid_pos_y_out,o->chassis_pos->pos_y,p.y);
+	delta_vy = PID_calc(&o->pid_pos_y_inner,o->chassis_pos->pos_Vy,delta_y);
+	
+
+	
+	
+	vx_set = delta_vx;
+	vy_set = delta_vy;
+	
+	POS_chassis_set(o,vx_set,vy_set,yaw_angle_set);
+}
+
+
+  
+
+//判断是否到达函数(全场定位)，最最后判断
+bool isArrive(point_t p,ops_t *o)
+{
+		uint16_t xArrive = 0, yArrive = 0, zArrive=0;
+
+	xArrive = abs_float_double(p.x,o->chassis_pos->pos_x) < o->state.mistake ? 1:0;
+	yArrive = abs_float_double(p.y,o->chassis_pos->pos_y) < o->state.mistake ? 1:0;
+	
+	if(xArrive && yArrive) return true;
+	else return false;
+}
+
+
+
+
+//寻迹，跑点函数
+int8_t go_path(ops_t *o,CMD_ACTION_t *ops_out)
+{
+   static  fp32 distance;//两点之间的距离差
+   static fp32 chassis_speed_set;//底盘速度设置
+   static fp32 SIN,COS;//方向解算
+   static fp32 world_vx,world_vy;//x,y方向分解速度
+   static fp32 chassis_vx,chassis_vy;//底盘xy分速度
+   static int cnt =0 ;//计数标志位
+
+	 static int flag_update_num;//记录flag更新，防止cnt一直变化
+
+	/*数据同步*/
+	o->current_x =o->chassis_pos ->pos_x ;
+	o->current_y =o->chassis_pos ->pos_y ;	
+	o->next_mapx = o->chassis_map[cnt].x;
+	o->next_mapy =o->chassis_map[cnt].y;	
+  o->Navi_Mode .Trig_flag = ops_out ->flag ;	
+	
+	if(o == NULL) return DEVICE_ERR;
+	if (ops_out == NULL) return DEVICE_ERR;
+  
+// static fp32 yaw_angle_set = 0;	
+	if(o->POS_IS_CPT == NO)//路径未跑完
+	{
+		if(o->state.moveState ==START)
+		{
+			//更新路径状态
+			o->state.moveState =MOVING;			
+		}
+		
+		
+		distance = sqrt((o->chassis_map[cnt].x - o->chassis_pos->pos_x)*(o->chassis_map[cnt].x - o->chassis_pos->pos_x)
+										+(o->chassis_map[cnt].y - o->chassis_pos->pos_y)*(o->chassis_map[cnt].y - o->chassis_pos->pos_y));
+		
+		chassis_speed_set = PID_calc(&o->path_speed_pid,-distance,0);//此处由pid计算距离得出相应的速度
+		//速度分解
+		SIN = (o->chassis_map[cnt].y - o->chassis_pos->pos_y)/distance;
+		COS = (o->chassis_map[cnt].x - o->chassis_pos->pos_x)/distance;
+		
+		world_vx = chassis_speed_set * COS;
+		world_vy = chassis_speed_set * SIN;
+		
+//		chassis_vx = world_vx;
+//		chassis_vy = world_vy;
+		//世界坐标转机器坐标系转换
+		
+		chassis_vx = -world_vy*sin(o->ops_imu_pos.yaw) + world_vx*cos(o->ops_imu_pos.yaw);
+		chassis_vy = world_vy*cos(o->ops_imu_pos.yaw) + world_vx*sin(o->ops_imu_pos.yaw);
+	
+	}
+	
+/*下面的逻辑按需更改，根据cnt计数器自动跑点，遥控器触发跑点等等*/	
+	
+    		
+	  if(isArrive(o->chassis_map[cnt],o))			//判断是否到达(全场定位)，只涉及xy的判断
+	{ 	
+         /*到达点后，这里应该增加cnt增加或减小的触发方式*/
+          
+	         if (o->Navi_Mode .Trig_flag != flag_update_num) { // 只有flag发生变化时才更新cnt
+
+              if (o->Navi_Mode .Trig_flag==1){
+				cnt++;	
+		       }           
+		    }        
+            else if (o->Navi_Mode .Trig_flag == -1) {
+			cnt--;
+		     }          
+           flag_update_num = o->Navi_Mode .Trig_flag; // 更新prev_flag为当前的flag状态
+	}	
+    else
+	{
+		 o->POS_IS_CPT = NO;
+        hold_point(o,o->chassis_map[cnt],o->chassis_map[cnt].angle);	//调用该函数使临近误差值快速响应			
+	}					
+		if(cnt>3)//根据点数更改
+		{
+		  o->POS_IS_CPT =YES;
+			  //到达目的地
+	      o->final_out .vx =0;
+	      o->final_out .vy =0;
+	      o->final_out.yaw_angle=0;	
+		}	
+    
+
+    
+	  
+
+	ops_out->out.Vx= o->final_out .vx ;
+	ops_out->out.Vy = o->final_out .vy ;
+	ops_out->out.Vw = o->final_out.yaw_angle;
+
+	return DEVICE_OK;
+}

User/Algorithm/navi.h

@@ -0,0 +1,108 @@
+#ifndef NAVI_H
+#define NAVI_H
+
+
+#include "Action.h"
+#include "pid.h"
+#include "ahrs.h"
+#include "bmi088.h"
+#include "map.h"
+
+typedef enum {
+    AUTO_MODE, // 自动跑点模式
+    MANUAL_MODE ,  // 控制模式
+	  NO_MODE
+} Navi_Mode_t;
+
+typedef struct {
+	
+   Navi_Mode_t mode; 
+		 
+	 int Trig_flag;//触发沿，初始值为零，两种控制模式下，这个值变为1，会使cnt+1，为-1，使cnt-1
+	
+} Navi_COT_MODE_t;
+
+
+typedef struct{
+	float pos_x;
+	float pos_y;
+	float last_pos_x;
+  float	last_pos_y;
+}chassis_position_t;
+
+typedef enum{
+	YES,
+	NO,
+	NEXT1,
+	NEXT2
+}is_cpt_e;
+typedef struct{
+   pid_param_t pid_PosSpeed_x_param;                 
+   pid_param_t pid_PosSpeed_y_param;                
+   pid_param_t pid_pos_x_out_param;                   
+   pid_param_t pid_pos_x_inner_param;                 
+   pid_param_t pid_pos_y_out_param;                   
+   pid_param_t pid_pos_y_inner_param;                 
+   pid_param_t pid_OutAngle_param;                    
+   pid_param_t pid_InnerAngle_param;                   
+   pid_param_t path_speed_pid_param;                  
+
+}ops_pid_param_t;
+
+typedef struct {
+	ops_pid_param_t ops_pid;
+	const  point_t *path;
+	int8_t path_num;
+}ops_param_t;
+
+/**
+ * @brief 
+ *
+*/
+typedef struct {
+	Action_POS_t *chassis_pos;          
+	fp32 current_x;														 	
+	fp32 current_y;														
+
+	fp32 next_mapx;
+	fp32 next_mapy;
+  const point_t *chassis_map;		
+  const ops_param_t *param;	
+	PathState_t state;
+	Navi_COT_MODE_t Navi_Mode; 
+	
+	 AHRS_Eulr_t ops_imu_pos;
+   AHRS_Gyro_t ops_gyro;
+
+
+	
+	pid_type_def pid_PosSpeed_x;              
+	pid_type_def pid_PosSpeed_y;	         
+	pid_type_def pid_pos_x_out;
+	pid_type_def pid_pos_x_inner;
+	pid_type_def pid_pos_y_out;
+	pid_type_def pid_pos_y_inner;
+	pid_type_def pid_OutAngle;
+	pid_type_def pid_InnerAngle;
+	pid_type_def path_speed_pid;
+
+
+	is_cpt_e POS_IS_CPT;                     
+   
+		 struct 
+	 {
+		fp32 vx;
+		fp32 vy;
+		fp32 yaw_angle;
+	 }final_out;
+	 
+}ops_t;
+
+
+
+
+int8_t Action_init(ops_t *o,const ops_param_t *param,Action_POS_t *pos);
+
+int8_t go_path(ops_t *o,CMD_ACTION_t *ops_out);
+#endif
+

User/Algorithm/user_math.c

@@ -309,3 +309,16 @@ uint8_t average(uint8_t arr[], uint8_t n) {
     }
     return (float) sum / n;
 }
+
+
+bool is_arrived(float target, float current, float mistake) {
+    // 计算当前值与目标值的差值的绝对值
+    float diff = fabs(target - current);
+    
+    // 判断是否在误差范围内
+    if (diff <= mistake) {
+        return true; // 在误差范围内
+    } else {
+        return false; // 不在误差范围内
+    }
+}

User/Algorithm/user_math.h

@@ -157,4 +157,7 @@ PolarCoordinate_t addPolarVectors(PolarCoordinate_t v1, PolarCoordinate_t v2);
 /// @param n 元素数
 /// @return 平均值
 uint8_t average(uint8_t arr[], uint8_t n);
+/*判断是否在误差内*/
+bool is_arrived(float target, float current, float mistake) ;
+
 #endif

User/Module/config.c

@@ -7,18 +7,6 @@
 
 #define DEG_TO_RAD(x) ((x) * (3.141592653 / 180.0)) //角度转弧度
 
-//#ifdef DEBUG
-
-//ConfigParam_t param_up ={
-
-//#else	
-//static const ConfigParam_t param_up ={
-//#endif
-//	
-//	
-//	
-//	
-//};
 
 #ifdef DEBUG
 
@@ -31,7 +19,9 @@ static const ConfigParam_t param_chassis ={
 
 #endif
 	.up={
-
+		
+		
+		/*上层pid参数*/
 .M2006_angle_param = {  
     .p = 25.0f,        
     .i = 0.0f,         
@@ -46,20 +36,6 @@ static const ConfigParam_t param_chassis ={
     .i_limit = 2000.0f,
     .out_limit = 3000.0f,
 },
-.M3508_angle_param = {  
-    .p = 30.0f,        
-    .i = 0.0f,         
-    .d = 1.5f,         
-    .i_limit = 1000.0f,
-    .out_limit = 3000.0f,
-},
-.M3508_speed_param = {  
-    .p = 5.0f,         
-    .i = 0.3f,         
-    .d = 0.0f,         
-    .i_limit = 2000.0f,
-    .out_limit = 3000.0f,
-},
 .UP_GM6020_angle_param={
 			.p = 30.0f,
 			.i = 20.0f,
@@ -74,25 +50,34 @@ static const ConfigParam_t param_chassis ={
 			.i_limit = 200.0f,
 			.out_limit = 3000.0f  			
 },
-.M3508_speed_param={
+
-		 .p = 15.1f,
+.go_param[0]={
-		 .i = 0.02f,
-		 .d = 3.2f,
-		 .i_limit = 200.0f,
-		 .out_limit =6000.0f,
-},
-.go_param={
     .rev = 0,
 	  .T=0.1,
 	  .W=0.1,
-	  .K_P=0.2,
+	  .K_P=0.1,
-	  .K_W=0.05,
+	  .K_W=0.1,
-}
+},
+.go_param[1]={
+    .rev = 0,
+	  .T=0.1,
+	  .W=0.1,
+	  .K_P=0.10f,
+	  .K_W=0.1,
+},
+/*上层其他参数*/
+    /*运球*/
+    .DribbleConfig_Config = {
+    .m3508_init_angle = 0,
+    .m3508_high_angle = 900,
+    .go2_init_angle =  35,
+    .go2_flip_angle = -130,
+    .flip_timing = 200,
+    .release_threshold = -1.2f,
+},
 	
 	
 	},
-
-
 	 
    .can = {
 		   .pitch6020 =  BSP_CAN_1,

User/Module/config.h

@@ -25,6 +25,8 @@ void Config_Get(Config_t *cfg);
 
 void Config_Set(Config_t *cfg);
 
+//void set_ops_path(ConfigParam_t *config, const point_t *path, int8_t path_num) ;
+
 extern const ConfigParam_t *Config_ChassisGet(void);
 
 

User/Module/up.c

@@ -11,6 +11,26 @@
 #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编码值转轴角度
 
+/*投球*/
+#define M2006_INIT_ANGLE        (-120) //初始和发射
+#define GO1_INIT_POSITION        (0)   //go初始
+
+#define M2006_TRIGGER           (0)    //扳机
+#define GO_POSITION_TRIGGER       (-300)  //go发射控制值
+#define GO_POSITION_PITCH_FD    (-4.8f) //反馈检测
+
+
+/*运球*/
+
+#define M3508_INIT_ANGLE           (0)    //3508
+#define GO2_INIT_ANGLE             (0)    //go2的初始角度
+#define M3508_HIGH_ANGLE           (900)    //3508，升起角度
+#define GO2_Flip_timing            (200)   // go的翻转时机，以3508角度反馈值为准
+#define GO2_Flip_ANGLE            (-160)     //go2翻转角度
+#define BALL_REL_TIME               (-1.2)     //球放开时机，以go的反馈值为准
+// 定义继电器控制
+#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
+#define RELAY2_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
 
 int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
 {
@@ -32,12 +52,19 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
 		
 
 	for(int i=0;i<2;i++){		 //go初始位置设置为0 
-	    GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), i,u->param->go_param .rev ,u->param->go_param .T ,u->param->go_param .W ,0,u->param->go_param .K_P ,u->param->go_param .K_W );		
+	    GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), i,u->param->go_param[i] .rev ,u->param->go_param[i] .T ,u->param->go_param[i] .W ,0,u->param->go_param [i].K_P ,u->param->go_param[i] .K_W );		
 	}
-	/**/
+
-	u->state .Dribble_flag =Not_started_dri;
+	
-	u->state. Pitch_flag=Not_started_Pit;
+    // 初始化状态机
-	u->state .last_state = Not_started_dri;
+	if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机
+		  u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
+      u->DribbleContext .DribbleState  = STATE_GRAB_BALL;  //状态更新,开始夹球
+      u->DribbleContext .is_initialized = 1;
+    }
+
+
+
 
 }
 
@@ -67,12 +94,13 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
 }
 
 
-	 int8_t cnt=0;
 
 /*上层电机控制,使用can1的id1和2*/
 int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor) {
     // 获取当前编码器角度
+	int8_t cnt=0;
 	fp32 angle ,delta;
+
 	switch(motor)
 	{
 		case M2006 :  			
@@ -178,76 +206,239 @@ int8_t GM6020_control(UP_t *u,fp32 angle)
 int8_t GO_SendData(UP_t *u,int id,float pos)
 {
 	
-	 GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), id,u->param->go_param .rev ,u->param->go_param .T ,u->param->go_param .W ,AngleChange(RADIAN,pos),u->param->go_param .K_P ,u->param->go_param .K_W );
+	 GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), id,u->param->go_param[id] .rev ,u->param->go_param[id] .T ,u->param->go_param[id] .W ,AngleChange(RADIAN,pos),u->param->go_param [id].K_P ,u->param->go_param[id] .K_W );
 
 }
 
 
 
-
-
 int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
 {
 	//电机控制	，传进can
 	
+	UP_angle_control(u,u->motor_target .M2006_angle ,M2006);
+	UP_angle_control(u,u->motor_target .M3508_angle ,M3508 );
+	
+	
+
+		GO_SendData(u,0 ,u->motor_target .go_shoot );
+	GO_SendData(u,1 ,u->motor_target .go_spin);
+
+	
 	for(int i=0;i<4;i++){
 	out ->motor3508 .as_array[i]=u->final_out.final_3508out [i] ;
 	}
 	
 	out ->chassis5065 .erpm [0]=  u->final_out .final_VESC_5065_M1out ;
 	out ->chassis5065 .erpm [1]=  -u->final_out .final_VESC_5065_M2out ;
-	
-	
-
 	out ->chassis6020 .as_array [2]=u->final_out .final_pitchout ;
 	
+	
+	
 }
 
 
-int8_t UP_control(UP_t *u,CAN_Output_t *out)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
 {
 	
 	if(u ==NULL)  return 0;
 	if(u ==NULL)  return 0;
 	
+    switch (c->CMD_CtrlType  )
+	{
+	case UP_RCcontrol:  //在手动模式下
+		
+   switch (c->CMD_UP_mode ) 
+			{
+				
+				case Normal :
+         
+				u->Oper_control_state .Pitch_flag  =Not_started_Pit;
+				u->Oper_control_state .last_state = Not_started_Pit;
+
+				u->motor_target .go_shoot =0;
+				u->motor_target .M2006_angle =M2006_INIT_ANGLE;
+				u->motor_target .go_spin =u->DribbleContext.DribbleConfig .go2_init_angle ;
+				u->motor_target .M3508_angle =0;
+				      u->DribbleContext .DribbleState  = STATE_GRAB_BALL;  //状态更新,开始夹球
+
+      	RELAY1_TOGGLE (1);//接球，1开0关
+      	RELAY2_TOGGLE (1);//夹球，0关1开
+    				
+				
+				break;
+				
+				case Pitch_pull :
+				 if(u->Oper_control_state .last_state == Not_started_Pit)					 
+				 {
+
+
+					 		u->motor_target .go_shoot =-2050;
+			       	u->motor_target .M2006_angle =-140;
+
+					 if(u->motorfeedback .GO_motor_info[0]->Pos < (-35.20)) //到达位置后再扣扳机
+					 		u->motor_target .go_shoot =GO_POSITION_TRIGGER;
+
+					 if(u->motorfeedback .GO_motor_info[0]->Pos < (GO_POSITION_PITCH_FD)) //到达位置后再扣扳机
+					 { 
+						  u->motor_target .M2006_angle =0;
+					   if(u->motorfeedback .M2006.total_angle>-5)	
+						 {//避免没勾上就拉
+							 u->motor_target .go_shoot =GO1_INIT_POSITION;
+							 u->Oper_control_state .Pitch_flag = Launch_Ready ;
+					     u->Oper_control_state .last_state = Launch_Ready;
+
+						 }
+						 
+					 }
+				 }
+
+				 break ;
+			  case Pitch_shoot :
+         if (u->Oper_control_state .last_state == Launch_Ready)
+				 {
  
+			       	u->motor_target .M2006_angle =M2006_INIT_ANGLE;
+							u->Oper_control_state .Pitch_flag = Done_Pit ;
+					    u->Oper_control_state .last_state = Done_Pit;
+					 
+				 }
+				 {	 
+
+				 }
+				 break;
+				 
+				 
+				 
+				 
+				 
+				 
+				 
+				 
+				 
+		case Dribble:
+		{
+			/*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收		*/
+			Dribble_Process(u,out);
 			
-//  switch(u->flag ){
+		}break ;
-//		case 0 :   
+						
-//			GO_SendData(u, 0,0);
-//			UP_angle_control(u, 0,M2006 );
-
-//		
-//		/*发射过程*/
-//		case 1 : 
-//			UP_angle_control(u, -130,M2006 );
-//			
-//			GO_SendData(u, 0,-1950);
 
 
 
-//		break ;  
-//			case 2 : 
-//			UP_angle_control(u, 0,M2006 );  
-//			
-//			if((u->motorfeedback .M2006 .total_angle )<10)
-//			
-//			GO_SendData(u, 0,0);
 
 
 
-//		break ;  
+
-//	
+
+
+					
+		
+			}
+			break;
+						
+	
+	default:
+		break;
+	}
+
 			
-	
+			
-//	
+
-//	}
+
-	
-	
-	
 	
 }
 
 
+int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
+{
+    
+    switch (u->DribbleContext.DribbleState) {
+    case STATE_GRAB_BALL://开始
+			
+			RELAY2_TOGGLE (0);//夹球
+		
+			u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_high_angle;//3508升起
+			u->motor_target.go_spin     =u->DribbleContext .DribbleConfig .go2_flip_angle;
+
+		
+		if((u->motorfeedback .M3508 .total_angle >400)) {
+				RELAY1_TOGGLE (0); 
+		
+
+		if((u->motorfeedback.GO_motor_info[1]->angle) <-53){
+
+				u->DribbleContext .DribbleState  = STATE_RELEASE_BALL;  //当go2到标准位置，标志位改变		
+			}
+		
+		}
+      break;
 
 
+    case STATE_RELEASE_BALL:
+				RELAY2_TOGGLE (1);//松球
+
+		
+		
+		
+		if((u->motorfeedback.GO_motor_info[1]->angle) <-60){
+				u->motor_target.go_spin     =u->DribbleContext .DribbleConfig .go2_init_angle ; //恢复go2位置	
+      	u->DribbleContext .DribbleState  = STATE_CATCH_PREP;  //当go2到标准位置，标志位改变		
+
+			}
+
+        break;
+
+    case STATE_CATCH_PREP:
+			if((u->motorfeedback.GO_motor_info[1]->angle) > -0){
+				u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle ;  //当go2到初始位置，3508降       
+		
+			  RELAY1_TOGGLE (1);//接球
+			}
+			if(u->motorfeedback .M3508 .total_angle <5){
+			  u->DribbleContext .DribbleState  = STATE_CATCH_DONE;
+			}
+        break;
+
+    case STATE_CATCH_DONE:
+				RELAY2_TOGGLE (1);//松球
+			  RELAY1_TOGGLE (0); 
+			
+
+        break;
+
+    default:
+        break;
+    }
+
+    return 0;
+}
+

User/Module/up.h

@@ -14,7 +14,38 @@
 #include "GO_M8010_6_Driver.h"
 #include "bsp_usart.h"
 
-	
+/*
+ 状态机
+
+状态结构体，触发标志位结构体
+
+配置层，-->config.c
+  |
+中间层，-->up.h,统一UP_t
+  |
+运行函数，switch(状态) 运行相应函数
+
+
+
+
+
+
+
+
+
+
+
+*/
+
+
+
+
+
+
+
+
+
+
 
 typedef enum
 {
@@ -25,25 +56,42 @@ typedef enum
 
 }Pitch_flag_t;
 
-typedef	enum{
+/*运球*/
+typedef enum {
+    STATE_GRAB_BALL,        // 夹球升起阶段
+    STATE_RELEASE_BALL,     // 释放球体
+    STATE_CATCH_PREP,       // 接球准备
+    STATE_CATCH_BALL,       // 接球动作
+	  STATE_CATCH_DONE        //完成
+} DribbleState_t;
+
+/* 参数配置结构体 */
+typedef struct {
+    fp32 m3508_init_angle;     // 3508初始角度
+    fp32 m3508_high_angle;     // 3508升起角度
+    fp32 go2_init_angle;       // GO2初始角度
+    fp32 go2_flip_angle;       // GO2翻转角度
+    fp32 flip_timing;       // 翻转触发时机
+    fp32 release_threshold;    // 释放球
+} DribbleConfig_t;
+
+/* 状态机上下文 */
+typedef struct {
+    DribbleState_t DribbleState;
+	  DribbleConfig_t DribbleConfig;
+	   
+    uint8_t is_initialized;
 	  
-	  Not_started_dri=0,//未开始
+} DribbleContext_t;
-	  No_ball =1,  //抓上无球    
-    Have_ball_F=2,  //刚开始有球
-    Have_ball_S=3,  //中途有球
-	  Done_dri =4         //已完成
-	
-}Dribble_flag_t;
 
 /*运行控制中的控制*/
+
 typedef struct{
 	
 	/*投球过程*/	
 	Pitch_flag_t Pitch_flag;
-	/*运球过程*/
+	
-	Dribble_flag_t Dribble_flag;
+	char last_state;	
- 
-	int last_state;
 	
 } Oper_control_state_t;
 
@@ -76,8 +124,7 @@ typedef struct {
 /*角度环控制电机类型*/
 typedef enum {
     M2006 = 1,
-    M3508
+    M3508	
-	
 } MotorType_t;
 
 
@@ -97,7 +144,9 @@ typedef struct
 	pid_param_t M3508_speed_param;	
 	pid_param_t M3508_angle_param;	
 	
-	GO_param_t go_param;
+	GO_param_t go_param[2];
+
+	DribbleConfig_t DribbleConfig_Config;
 	
 }UP_Param_t;
 
@@ -119,11 +168,22 @@ typedef struct{
    uint8_t up_task_run; 
    const	UP_Param_t *param;  
 	
+	int state;
+	
+	
+	
+		/*运球过程*/
+  DribbleContext_t DribbleContext;
+
+	
+	
+	
+	
 	 UP_Imu_t pos088; 
 	  
 	/*控制及状态*/
 	 CMD_t *cmd;	
-	 Oper_control_state_t state;//上层机构的运行状态
+	 Oper_control_state_t Oper_control_state;//上层机构的运行状态
 
 	 struct{					
 	  fp32 rotor_pit6020ecd;
@@ -172,8 +232,8 @@ typedef struct{
 		 pid_type_def M2006_angle;
 		 pid_type_def M2006_speed;
 		 
-		  pid_type_def M3508_angle;
+		 pid_type_def M3508_angle;
-		  pid_type_def M3508_speed;
+		 pid_type_def M3508_speed;
 				
 	}pid;
  
@@ -206,11 +266,12 @@ int8_t GM6020_control(UP_t *u,fp32 angle);
 int8_t VESC_M5065_Control(UP_t *u,fp32 speed);
 int8_t GO_SendData(UP_t *u,int id,float pos);
 int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor);
-int8_t UP_control(UP_t *u,CAN_Output_t *out);
+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 UP_M2006_angle(UP_t *u,fp32 target_angle);
 int8_t UP_M3508_speed(UP_t *u,fp32 speed);
 
+int8_t Dribble_Process(UP_t *u, CAN_Output_t *out);
 
 
 #endif

User/bsp/bsp_usart.c

@@ -8,8 +8,8 @@ static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void);
 static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
   if (huart->Instance == USART3)
     return BSP_UART_REMOTE;
-//  else if (huart->Instance == USART1)
+  else if (huart->Instance == USART1)
-//    return BSP_UART_RS485;
+    return BSP_UART_NUC;
   else if (huart->Instance == USART6)
     return BSP_UART_RS485;
   /*

User/bsp/gpio.c

@@ -1,5 +1,5 @@
 /* Includes ----------------------------------------------------------------- */
-#include "bsp\bsp_gpio.h"
+#include "bsp\gpio.h"
 
 #include <gpio.h>
 #include <main.h>

User/bsp/protocol.h

@@ -28,7 +28,7 @@ typedef struct __attribute__((packed)) {
    
 	int remain;
 } Protocol_UpDataMCU_t;
-/* 视觉 -> 电控  数据结构体*/
+/* 视觉 -> 电控  底盘数据结构体*/
 typedef struct __attribute__((packed)) {
 
   Protocol_ID_t recv_id;//作为帧头使用确认通信ID正确

User/bsp/spi.c

@@ -1,5 +1,5 @@
 /* Includes ----------------------------------------------------------------- */
-#include "bsp\bsp_spi.h"
+#include "bsp\spi.h"
 
 /* Private define ----------------------------------------------------------- */
 /* Private macro ------------------------------------------------------------ */

User/device/GO_M8010_6_Driver.c

@@ -38,13 +38,21 @@ void GO_M8010_init (void){
 
 
 //暂存接收数据
-static uint8_t Temp_buffer[16];
+uint8_t Temp_buffer[16];
 
 void USART6_RxCompleteCallback(void ) 
 {
 	 UART_HandleTypeDef *huart6 = BSP_UART_GetHandle(BSP_UART_RS485);
 	
     uint16_t crc = CRC16_Calc(Temp_buffer,sizeof(Temp_buffer)-2,0x0000);
+    if ((Temp_buffer[14] != (crc&0xFF)) || (Temp_buffer[15] != ((crc>>8) & 0xFF)))
+    { 
+//        HAL_GPIO_WritePin(LED_B_GPIO_Port, LED_B_Pin, GPIO_PIN_SET);    //蓝色灯亮      
+//        HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET);      //红色灯灭
+        return;
+    }
+//    HAL_GPIO_WritePin(GPIOH, GPIO_PIN_11, GPIO_PIN_SET);                 // indicate CRC correct
+//    HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET);      //红色灯灭
     GO_M8010_recv_data(Temp_buffer);
 
 }
@@ -157,6 +165,7 @@ void GO_M8010_recv_data(uint8_t* Temp_buffer)
     motor->Temp = Temp;
     motor->MError = Temp_buffer[12] & 0x7;
     
+		motor ->angle = AngleChange(DEGREE,motor ->Pos);
 }
 
 //力位混合控制

User/device/GO_M8010_6_Driver.h

@@ -8,6 +8,7 @@ extern "C"{
 #include "usart.h"
 #include "string.h"
 #include <math.h>
+#include "user_math.h"
 #define GO_NUM 2
 /**
  * @brief
@@ -62,11 +63,14 @@ typedef struct {
         float tar_w;                    
         float T;                       
         float W;                       
-        float Pos;                     
+        float Pos; 
+        float angle;
+	
         int footForce;                  
         uint8_t buffer[17];
         uint8_t Rec_buffer[16];
         ControlData_t  motor_send_data;  
+	
 }GO_Motorfield;
 
 

User/device/LD_remote.c

@@ -0,0 +1,176 @@
+/*
+
+ 乐迪遥控器
+
+*/
+/* Includes ----------------------------------------------------------------- */
+#include "LD_remote.h"
+
+#include <string.h>
+
+#include "bsp_usart.h"
+#include "error_detect.h"
+#include "LD_remote.h"
+
+
+
+
+
+
+
+//static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl);
+
+//RC_ctrl_t rc_ctrl;
+//static uint8_t sbus_rx_buf[2][RC_FRAME_LENGTH];
+//uint8_t remote_ready = 0;//Ò£¿ØÆ÷×¼±¸Íê³É 
+
+//void remote_control_init(void)
+//{
+//	RC_init(sbus_rx_buf[0], sbus_rx_buf[1], RC_FRAME_LENGTH);
+//}
+
+// int map_int(int x, int in_min, int in_max, int out_min, int out_max)		
+//{
+//	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+//}
+////´®¿ÚÖжÏ
+//void USART3_IRQHandler(void)
+//{
+//	//have received data  ???
+//	if(huart3.Instance->SR & UART_FLAG_RXNE)
+//	{
+//		__HAL_UART_CLEAR_FEFLAG(&huart3);
+//	}
+//	else if(USART3->SR & UART_FLAG_IDLE)
+//	{
+//	
+//    static uint16_t this_time_rx_len = 0;
+//		__HAL_UART_CLEAR_PEFLAG(&huart3);
+//		
+//		if( (hdma_usart3_rx.Instance->CR & DMA_SxCR_CT) == RESET)
+//		{
+//			//current memory buffer used is memory0
+//			
+//			//disable dma to change dma register
+//			__HAL_DMA_DISABLE(&hdma_usart3_rx);
+//			
+//			//get received data length, length = set_data_length - remain_length 
+//			this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
+
+//			//reset set_data_length
+//			hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
+//			
+//			//change memory0 to memory1
+//			hdma_usart3_rx.Instance->CR |= DMA_SxCR_CT;
+//			
+//			//enable dma
+//			__HAL_DMA_ENABLE(&hdma_usart3_rx);
+//			
+//			//1 frame length is correct data
+//			if(this_time_rx_len == RC_FRAME_LENGTH)
+//			{
+//				sbus_to_rc(sbus_rx_buf[0], &rc_ctrl);
+//			}
+//		}
+//			else
+//			{
+//				__HAL_DMA_DISABLE(&hdma_usart3_rx);
+//				
+//				this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
+//				
+//				hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
+//				
+//				//change memory1 to memory0
+//				DMA1_Stream1->CR &= ~(DMA_SxCR_CT);
+//				
+//				__HAL_DMA_ENABLE(&hdma_usart3_rx);
+//				
+//				if(this_time_rx_len == RC_FRAME_LENGTH)
+//				{
+//					sbus_to_rc(sbus_rx_buf[1], &rc_ctrl);
+//				}
+//			}
+//	}
+//}
+
+//static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
+//{
+//    if (sbus_buf == NULL || rc_ctrl == NULL)
+//    {
+//        return;
+//    }
+//		
+//		rc_ctrl->ch[0] = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff;        //!< Channel 0
+//    rc_ctrl->ch[1] = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //!< Channel 1
+//    rc_ctrl->ch[2] = ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) |          //!< Channel 2
+//                         (sbus_buf[5] << 10)) &0x07ff;
+//    rc_ctrl->ch[3] = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //!< Channel 3
+//    
+//		rc_ctrl->sw[0] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF;                  //!< Switch left
+//    rc_ctrl->sw[1] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF;                       //!< Switch right
+//    rc_ctrl->sw[2] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;;                    //!< Mouse X axis
+//    rc_ctrl->sw[3] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF;                    //!< Mouse Y axis
+//    rc_ctrl->sw[4] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF;                  //!< Mouse Z axis
+//    rc_ctrl->sw[5] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF;                                  //!< Mouse Left Is Press ?
+//    rc_ctrl->sw[6] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF;                                  //!< Mouse Right Is Press ?
+//    rc_ctrl->sw[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF;                    //!< KeyBoard value
+
+//		rc_ctrl->ch[0] -= RC_CH_VALUE_OFFSET;
+//    rc_ctrl->ch[1] -= RC_CH_VALUE_OFFSET;
+//    rc_ctrl->ch[2] -= RC_CH_VALUE_OFFSET;
+//    rc_ctrl->ch[3] -= RC_CH_VALUE_OFFSET;
+//    
+//		//µ÷ÕûÒ¡¸ËÖÐֵʹң¿ØÆ÷΢µ÷Á¿Îª0ʱҡ¸ËÖÐÖµ½Ó½ü0£¬ÓÍÃųýÍâ
+//		//µÚÒ»´Îµ÷³µÇ°ÏÈÈ·ÈÏͨµÀÖµÊÇ·ñÕýÈ·£¡£¡Èç²»·ûºÏ¸ù¾Ýʵ¼ÊͨµÀÖµ½øÐÐÐÞ¸Ä
+//		//Ð޸Ĵ˴¦¼°omni_chassis.cÖÐͨµÀÖµ²¿·Ö
+//		rc_ctrl->ch[0] += 24;	//y(-694,693)×óÓÒ
+//		rc_ctrl->ch[1] = -rc_ctrl->ch[1]-24;	//x(-693,694)Ç°ºó
+//		rc_ctrl->ch[2] = -rc_ctrl->ch[2]+764;		//m(95,1482)ÓÍÃÅ
+//		rc_ctrl->ch[3] += 24;	//w(-694,693)Ðýת
+//		
+//		rc_ctrl->ch[1] = map_int(rc_ctrl->ch[1],-693,694,-700,700);		//xÓ³Éäµ½(-700,700)
+//		rc_ctrl->ch[0] = map_int(rc_ctrl->ch[0],-694,693,-700,700);		//yÓ³Éäµ½(-700,700)
+//		rc_ctrl->ch[3] = 0.5*(rc_ctrl->ch[3]);		//w
+//		
+//		//ËÀÇø(-30,30)
+//		if(rc_ctrl->ch[0]>-30&&rc_ctrl->ch[0]<30) rc_ctrl->ch[0]=0;
+//		if(rc_ctrl->ch[1]>-30&&rc_ctrl->ch[1]<30) rc_ctrl->ch[1]=0;
+//		if(rc_ctrl->ch[2]>-30&&rc_ctrl->ch[2]<30) rc_ctrl->ch[2]=0;
+//		if(rc_ctrl->ch[3]>-30&&rc_ctrl->ch[3]<30) rc_ctrl->ch[3]=0;
+//    remote_ready = 1;
+//}                                                                            																																					
+
+
+
+//int map(int x, int in_min, int in_max, int out_min, int out_max)		//Ó³É亯Êý
+//{
+//	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+//}
+
+
+///*
+
+
+
+//  																																	
+//306																							  					  	  306
+// sw[]																						 	 					  		sw[7]
+//1694																							 					  		1694
+// 
+//306																														    	306
+//sw[6]                                                              sw[4]
+//1694                                                               1694
+
+//306         306                                        306        306   
+//sw[0]      sw[2]    sw[1]:306-1694   sw[5]:306-1694  sw[]1000   sw[3] 
+//1694       1694                                         1694       1694
+//                                      710
+//               688                   1425
+//                |                      |
+//                |                      |
+//   54    -616------ch[3]770   -354---------ch[0] 339  0
+//                |                      |
+//                |                      |
+//	             ch[2]                 ch[1]
+//               _699                   38   
+//*/

User/device/LD_remote.h

@@ -0,0 +1,31 @@
+#ifndef LD_H
+#define LD_H
+#include "user_math.h"
+
+#define SBUS_RX_BUF_NUM 50u
+
+#define RC_FRAME_LENGTH 25u
+
+#define RC_CH_VALUE_OFFSET  ((uint16_t)1024)
+
+typedef  struct
+{
+	int16_t ch[4];
+  int16_t sw[8]; 
+
+}__attribute__((packed)) RC_ctrl_t;
+
+
+int map_int(int x, int in_min, int in_max, int out_min, int out_max)	;	
+
+void remote_control_init(void);
+static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl);
+int map(int x, int in_min, int in_max, int out_min, int out_max);		
+
+
+
+
+
+
+#endif
+

User/device/bmi088.c

@@ -12,8 +12,8 @@
 #include <string.h>
 #include "user_math.h"
 #include "bsp\delay.h"
-#include "bsp\bsp_gpio.h"
+#include "bsp\gpio.h"
-#include "bsp\bsp_spi.h"
+#include "bsp\spi.h"
 #include "device\device.h"
 #include "bsp\pwm.h"
 #include "pid.h"

User/device/can_use.c

@@ -378,8 +378,7 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
 //    case CAN_VSEC5065_M4_MSG1:
 //      // 存储消息到对应的电机结构体中
 //      CAN_VescMotor_Decode_1(&(can->motor.chassis5065.as_array[3]), can_rx->rx_data);
-
+      break;
-																											break;
 
 
 

User/device/cmd.c

@@ -52,22 +52,25 @@ int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) {
 	else if(rc->sw_l==CMD_SW_UP)
 	{
 		cmd ->CMD_CtrlType =UP_RCcontrol;
-		if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Pitch; //左上，右上，投篮
+		if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Normal; //左上，右上，投篮
-		if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左上，右中，无模式
+		if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Pitch_pull; //左上，右中，无模式
-	  if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Dribble; //左上，右上，投篮
+	  if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Pitch_shoot; //左上，右上，投篮
 	}
 	
 	else if(rc->sw_l==CMD_SW_MID)
 	{
-				cmd ->CMD_CtrlType =MID_NAVI;
+				
+		if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_CtrlType =MID_NAVI;; //左中，右上，雷达
+		if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左中，右中，无模式
+	  if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Dribble; //左中，右上，运球
 
 	}
 	else if(rc->sw_l==CMD_SW_DOWN)
 	{
 		cmd ->CMD_CtrlType =PICK_t;
-		if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Pitch; //左下，右上，投篮
+		if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Normal; //左下，右上，投篮
 		if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左下，右中，无模式
-	  if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Dribble; //左下，右上，投篮
+	  if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Normal; //左下，右上，投篮
 	}	
 	
 	

User/device/cmd.h

@@ -22,7 +22,8 @@ typedef  enum{
 
 		Normal,    //无模式
     Dribble ,  //运球
-	  Pitch  ,    //投篮
+	  Pitch_pull  ,    //投篮
+	  Pitch_shoot  ,    //投篮
 
   }CMD_UP_mode_t;
 typedef struct {

User/device/nuc.c

@@ -45,13 +45,13 @@ bool_t NUC_WaitDmaCplt(void) {
 int8_t NUC_RawParse(CMD_NUC_t *n){
 	if(n ==NULL) return DEVICE_ERR_NULL;
 
-  if(nucbuf[0]!=HEAD) goto error;  
+//  if(nucbuf[0]!=HEAD) goto error;  
-  else{
+//  else{
     n->status_fromnuc =nucbuf[1];
 		n->ctrl_status =nucbuf[2];
-    switch (n->status_fromnuc)
+//    switch (n->status_fromnuc)
-    {
+//    {
-		case MID://控制帧0x09
+//		case MID://控制帧0x09
 				/* 协议格式 
 		   0xFF  HEAD
 		   0x0X  控制帧
@@ -62,7 +62,7 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
 		   0xFE  TAIL
 	使用的是串口1
 		  */
-		if(nucbuf[15]!=TAIL)goto error;
+//		if(nucbuf[15]!=TAIL)goto error;
        instance.data[3] = nucbuf[6];
        instance.data[2] = nucbuf[5];
        instance.data[1] = nucbuf[4];
@@ -78,42 +78,59 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
        instance.data[9] = nucbuf[12];
        instance.data[8] = nucbuf[11];
       n->navi.wz = instance.x[2];//
-      break;
+//      break;
-    case PICK:
+//    case PICK:
-			/* 协议格式 
+//			/* 协议格式 
-		   0xFF  HEAD
+//		   0xFF  HEAD
-		   0x06  控制帧
+//		   0x0X  控制帧
-		   0x01  相机帧
+//		   0x01  相机帧
-		   cmd   8位
+//		   cmd   8位
-		   dis   相机深度值
+//		   dis   相机深度值
-		   posx  相机yaw轴值
+//		   posx  相机yaw轴值
-		   posy  相机pitch轴值
+//		   posy  相机pitch轴值
-		   0xFE  TAIL
+//		   0xFE  TAIL
-		  */
+//		  */
-		  if(nucbuf[15]!=TAIL)goto error;
+//		  if(nucbuf[15]!=TAIL)goto error;
-       instance.data[3] = nucbuf[6];
+//       instance.data[3] = nucbuf[6];
-       instance.data[2] = nucbuf[5];
+//       instance.data[2] = nucbuf[5];
-       instance.data[1] = nucbuf[4];
+//       instance.data[1] = nucbuf[4];
-       instance.data[0] = nucbuf[3];
+//       instance.data[0] = nucbuf[3];
-      n->pick.posx = instance.x[0]; //距离球中心的角度值
+//      n->pick.posx = instance.x[0]; //距离球中心的角度值
-       instance.data[7] = nucbuf[10];
+//       instance.data[7] = nucbuf[10];
-       instance.data[6] = nucbuf[9];
+//       instance.data[6] = nucbuf[9];
-       instance.data[5] = nucbuf[8];
+//       instance.data[5] = nucbuf[8];
-       instance.data[4] = nucbuf[7];
+//       instance.data[4] = nucbuf[7];
-		 n->pick.posy = instance.x[1];// 相机yaw轴
+//		 n->pick.posy = instance.x[1];// 相机yaw轴
-		   instance.data[11]  = nucbuf[14];
+//		   instance.data[11]  = nucbuf[14];
-		   instance.data[10]  = nucbuf[13];
+//		   instance.data[10]  = nucbuf[13];
-		   instance.data[9]  = nucbuf[12];
+//		   instance.data[9]  = nucbuf[12];
-		   instance.data[8]  = nucbuf[11];
+//		   instance.data[8]  = nucbuf[11];
-		 n->pick.posw= instance.x[2];// 暂未用到
+//		 n->pick.posw= instance.x[2];// 暂未用到
-       break;
+//       break;
-
+//		case SICK_CAIL:
-    }
+//			if(nucbuf[15]!=TAIL)goto error;
-     return DEVICE_OK;
+//       instance.data[3] = nucbuf[14];
-  }
+//       instance.data[2] = nucbuf[13];
-    error:
+//       instance.data[1] = nucbuf[12];
-    drop_message++;
+//       instance.data[0] = nucbuf[11];
-    return DEVICE_ERR;
+//      n->sick_cali.angle = instance.x[0]; //
+//       instance.data[7] = nucbuf[10];
+//       instance.data[6] = nucbuf[9];
+//       instance.data[5] = nucbuf[8];
+//       instance.data[4] = nucbuf[7];
+//		 n->sick_cali.isleft = instance.x[1];// 
+//		   instance.data[11]  = nucbuf[14];
+//		   instance.data[10]  = nucbuf[13];
+//		   instance.data[9]  = nucbuf[12];
+//		   instance.data[8]  = nucbuf[11];
+//		 n->pick.posw= instance.x[2];// 暂未用到
+//		   break;
+//    }
+//     return DEVICE_OK;
+//  }
+//    error:
+//    drop_message++;
+//    return DEVICE_ERR;
 }
 
 int8_t NUC_HandleOffline(CMD_NUC_t *cmd)

User/device/r12ds.c

@@ -0,0 +1,216 @@
+/*
+  乐迪r12ds遥控器。
+  需要根据实际的遥控器通道修改解析的函数 该遥控器采用sbus  用的uart3
+
+*/
+
+
+///* Includes ----------------------------------------------------------------- */
+//#include "r12ds.h"
+//#include "main.h"
+//#include "error_detect.h"
+//#include <string.h>
+
+
+//extern UART_HandleTypeDef huart3;
+//extern DMA_HandleTypeDef hdma_usart3_rx;
+
+
+
+//osThreadId_t thread_alert;
+
+//int buf0ready =0;
+//int buf1ready =0;
+
+///* Private function  -------------------------------------------------------- */
+//static void R12DS_IdleCallback(void) {
+//	static uint16_t this_time_rx_len = 0;
+//	if( (hdma_usart3_rx.Instance->CR & DMA_SxCR_CT) == RESET)
+//		{
+//			//current memory buffer used is memory0
+//			
+//			//disable dma to change dma register
+//			__HAL_DMA_DISABLE(&hdma_usart3_rx);
+//			
+//			//get received data length, length = set_data_length - remain_length 
+//			this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
+
+//			//reset set_data_length
+//			hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
+//			
+//			//change memory0 to memory1
+//			hdma_usart3_rx.Instance->CR |= DMA_SxCR_CT;
+//			
+//			//enable dma
+//			__HAL_DMA_ENABLE(&hdma_usart3_rx);
+//			
+//			//1 frame length is correct data
+//			if(this_time_rx_len == RC_FRAME_LENGTH)
+//			{
+////         	osThreadFlagsSet(thread_alert,SIGNAL_R12DS_BUF0_REDY);
+//				buf0ready = 1;
+//				detect_hook(R12DS_TOE);
+//			}
+//		}
+//			else
+//			{
+//				__HAL_DMA_DISABLE(&hdma_usart3_rx);
+//				
+//				this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
+//				
+//				hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
+//				
+//				//change memory1 to memory0
+//				DMA1_Stream1->CR &= ~(DMA_SxCR_CT);
+//				
+//				__HAL_DMA_ENABLE(&hdma_usart3_rx);
+//				
+//				if(this_time_rx_len == RC_FRAME_LENGTH)
+//				{
+////           	osThreadFlagsSet(thread_alert,SIGNAL_R12DS_BUF1_REDY);
+//					buf1ready = 1;
+//					detect_hook(R12DS_TOE);
+//				}
+//			}
+//}
+
+///* Exported functions ------------------------------------------------------- */
+
+
+//int8_t R12ds_DMA_Init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num)
+//{
+//	    if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
+//	//enable the dma transfer for the receiver request
+//	SET_BIT(huart3.Instance->CR3, USART_CR3_DMAR);
+//	
+//	//enable idle interrupt
+//	__HAL_UART_ENABLE_IT(&huart3, UART_IT_IDLE);
+//	
+//	//disable dma, to change the dma register
+//	__HAL_DMA_DISABLE(&hdma_usart3_rx);
+//	
+//	//disable dma again but why?
+//	//what's the condition?
+//	while(hdma_usart3_rx.Instance->CR & DMA_SxCR_EN)
+//	{
+//		__HAL_DMA_DISABLE(&hdma_usart3_rx);
+//	}
+//	
+//	//??
+//	hdma_usart3_rx.Instance->PAR = (uint32_t) & (USART3->DR);
+//	
+//	//memory buffer 1
+//	hdma_usart3_rx.Instance->M0AR = (uint32_t)(rx1_buf);
+//	
+//	//momory buffer 2
+//	hdma_usart3_rx.Instance->M1AR = (uint32_t)(rx2_buf);
+//	
+//	//data length
+//	hdma_usart3_rx.Instance->NDTR = dma_buf_num;
+//	
+//	//enable double memory buffer
+//	SET_BIT(hdma_usart3_rx.Instance->CR, DMA_SxCR_DBM);
+//	
+//	
+//	//enable dma
+//	__HAL_DMA_ENABLE(&hdma_usart3_rx);
+//	
+//	  BSP_UART_RegisterCallback(BSP_UART_R12DS, BSP_UART_IDLE_LINE_CB,
+//                           R12DS_IdleCallback);
+
+//  return 1;
+//}
+
+//int map(int x, int in_min, int in_max, int out_min, int out_max)		//ӳʤگ˽
+//{
+//	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+//}
+
+
+//int8_t sbus_to_rc(volatile const uint8_t *sbus_buf, CMD_RC_t *rc_ctrl)
+//{
+//    if (sbus_buf == NULL || rc_ctrl == NULL)
+//    {
+//        return 0;
+//    }
+//		
+//		rc_ctrl->ch_x = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff;        //Channel 1  x
+//    rc_ctrl->mul= ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //Channel 2   mul
+//    rc_ctrl->ch_y= ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) |          //Channel 3  y
+//                         (sbus_buf[5] << 10)) &0x07ff;
+//    rc_ctrl->ch_w = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //Channel 4  w
+//    
+//		rc_ctrl->key[0] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF;                   								//Channel 5
+//    rc_ctrl->key[1] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF;    	//Channel 6
+//    rc_ctrl->key[2] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;;                    							//Channel 7
+//    rc_ctrl->key[3] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF;                    							//Channel 8
+//    rc_ctrl->key[4] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF;                  								//Channel 9
+//    rc_ctrl->key[5] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF;                               	//Channel 10
+//    rc_ctrl->key[6] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF;	//Channel 11
+//    rc_ctrl->key[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF;                    							//Channel 12
+
+//		rc_ctrl->ch_y -= RC_CH_VALUE_OFFSET;
+//    rc_ctrl->ch_x -= RC_CH_VALUE_OFFSET;
+//    rc_ctrl->mul  -= RC_CH_VALUE_OFFSET;
+//    rc_ctrl->ch_w -= RC_CH_VALUE_OFFSET;
+//    
+
+
+//		rc_ctrl->ch_y -= 4;	//y(-694,693)
+//		rc_ctrl->ch_x += 3;	//x(-693,694)
+//		rc_ctrl->mul = rc_ctrl->mul;		//m(-518,843)
+//		rc_ctrl->ch_w += 4;		//w(-694,693)
+//	
+//    rc_ctrl->key[2] = map(rc_ctrl->key[2],306,1694,1694,306);	
+//    rc_ctrl->key[3] = map(rc_ctrl->key[3],306,1694,1694,306);	
+//		rc_ctrl->ch_x = map(rc_ctrl->ch_x,700,-800,-700,700);		//x
+//		rc_ctrl->ch_y = map(rc_ctrl->ch_y,-900,796,700,-700);	//y
+//		rc_ctrl->mul = map(rc_ctrl->mul,-632,901,25,0);		//m
+//		rc_ctrl->ch_w = map(rc_ctrl->ch_w,-820,780,-700,700);  
+//		rc_ctrl->ch_w = 0.5f*(rc_ctrl->ch_w);
+//    rc_ctrl->ch_x = -0.5f*(rc_ctrl->ch_x);
+//    rc_ctrl->ch_y = -0.5f*(rc_ctrl->ch_y);		
+////		
+////̀死区(-5,5)
+//		if(rc_ctrl->ch_y>-15&&rc_ctrl->ch_y<15) rc_ctrl->ch_y=0;              
+//		if(rc_ctrl->ch_x>-15&&rc_ctrl->ch_x<15) rc_ctrl->ch_x=0;                
+//		if(rc_ctrl->mul>=0&&rc_ctrl->mul<=3) rc_ctrl->mul=0;              
+//		if(rc_ctrl->ch_w>-15&&rc_ctrl->ch_w<15) rc_ctrl->ch_w=0;        
+
+//   return 0;
+//}
+
+//bool_t R12Buf0_WaitDmaCplt(int32_t timeout) {
+//	
+//	    if(buf0ready == 1)
+//			{
+//				buf0ready =0;
+//				return 1;
+//			}
+//			else 
+//				return 0;
+////  return(osThreadFlagsWait(SIGNAL_R12DS_BUF0_REDY, osFlagsWaitAll,timeout) ==
+////          SIGNAL_R12DS_BUF0_REDY);
+//}
+
+//bool_t R12Buf1_WaitDmaCplt(int32_t timeout) {
+//		    if(buf1ready == 1)
+//			{
+//				buf1ready =0;
+//				return 1;
+//			}
+//			else 
+//				return 0;
+////  return(osThreadFlagsWait(SIGNAL_R12DS_BUF1_REDY, osFlagsWaitAll,timeout) ==
+////          SIGNAL_R12DS_BUF1_REDY);
+//	    
+//}
+
+//void R12ds_HandleOffline(void) {
+//	 CMD_RC_t *rc;
+//	 rc = R12ds_DataGet();
+
+//  memset(rc, 0, sizeof(*rc));
+//  rc->key[3] = 2000;//用作遥控器断电后急停使用
+//}
+

User/device/r12ds.h

@@ -0,0 +1,32 @@
+#ifndef _R12DS_H
+#define _R12DS_H 
+
+#include <cmsis_os2.h>
+#include "device.h"
+#include "bsp_usart.h"
+#include "cmd.h"
+
+#define RC_FRAME_LENGTH 25u
+
+
+#define RC_CH_VALUE_OFFSET  ((uint16_t)1024)
+
+
+
+int8_t R12ds_DMA_Init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num);
+
+int8_t sbus_to_rc(volatile const uint8_t *sbus_buf,CMD_RC_t *rc_ctrl);
+
+
+bool_t R12Buf0_WaitDmaCplt(int32_t timeout);
+
+
+bool_t R12Buf1_WaitDmaCplt(int32_t timeout);
+
+void R12ds_HandleOffline(void);
+
+
+
+
+#endif
+

User/device/remote_control.c

@@ -0,0 +1,185 @@
+/**
+  ****************************(C) COPYRIGHT 2019 DJI****************************
+  * @file       remote_control.c/h
+  * @brief      遥控器处理，遥控器是通过类似SBUS的协议传输，利用DMA传输方式节约CPU
+  *             资源，利用串口空闲中断来拉起处理函数，同时提供一些掉线重启DMA，串口
+  *             的方式保证热插拔的稳定性。
+  * @note       该任务是通过串口中断启动，不是freeRTOS任务
+  * @history
+  *  Version    Date            Author          Modification
+  *  V1.0.0     Dec-01-2019     RM              1. 完成
+  *
+  @verbatim
+  ==============================================================================
+
+  ==============================================================================
+  @endverbatim
+  ****************************(C) COPYRIGHT 2019 DJI****************************
+  */
+  
+#include "remote_control.h"
+#include "main.h"
+
+
+extern UART_HandleTypeDef huart3;
+extern DMA_HandleTypeDef hdma_usart3_rx;
+
+static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl);
+
+RC_ctrl_t rc_ctrl;
+static uint8_t sbus_rx_buf[2][RC_FRAME_LENGTH];
+uint8_t remote_ready = 0;//遥控器准备完成 
+
+void remote_control_init(void)
+{
+	RC_init(sbus_rx_buf[0], sbus_rx_buf[1], RC_FRAME_LENGTH);
+}
+
+
+//串口中断
+void USART3_IRQHandler(void)
+{
+	//have received data  ???
+	if(huart3.Instance->SR & UART_FLAG_RXNE)
+	{
+		__HAL_UART_CLEAR_FEFLAG(&huart3);
+	}
+	else if(USART3->SR & UART_FLAG_IDLE)
+	{
+	
+    static uint16_t this_time_rx_len = 0;
+		__HAL_UART_CLEAR_PEFLAG(&huart3);
+		
+		if( (hdma_usart3_rx.Instance->CR & DMA_SxCR_CT) == RESET)
+		{
+			//current memory buffer used is memory0
+			
+			//disable dma to change dma register
+			__HAL_DMA_DISABLE(&hdma_usart3_rx);
+			
+			//get received data length, length = set_data_length - remain_length 
+			this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
+
+			//reset set_data_length
+			hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
+			
+			//change memory0 to memory1
+			hdma_usart3_rx.Instance->CR |= DMA_SxCR_CT;
+			
+			//enable dma
+			__HAL_DMA_ENABLE(&hdma_usart3_rx);
+			
+			//1 frame length is correct data
+			if(this_time_rx_len == RC_FRAME_LENGTH)
+			{
+				sbus_to_rc(sbus_rx_buf[0], &rc_ctrl);
+			}
+		}
+			else
+			{
+				__HAL_DMA_DISABLE(&hdma_usart3_rx);
+				
+				this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
+				
+				hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
+				
+				//change memory1 to memory0
+				DMA1_Stream1->CR &= ~(DMA_SxCR_CT);
+				
+				__HAL_DMA_ENABLE(&hdma_usart3_rx);
+				
+				if(this_time_rx_len == RC_FRAME_LENGTH)
+				{
+					sbus_to_rc(sbus_rx_buf[1], &rc_ctrl);
+				}
+			}
+	}
+}
+
+static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
+{
+    if (sbus_buf == NULL || rc_ctrl == NULL)
+    {
+        return;
+    }
+		
+		rc_ctrl->ch[1] = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff;        //Channel 1
+    rc_ctrl->ch[2] = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //Channel 2
+    rc_ctrl->ch[0] = ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) |          //Channel 3
+                         (sbus_buf[5] << 10)) &0x07ff;
+    rc_ctrl->ch[3] = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //Channel 4
+    
+		rc_ctrl->sw[0] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF;                   								//Channel 5
+    rc_ctrl->sw[1] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF;    	//Channel 6
+    rc_ctrl->sw[2] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;;                    							//Channel 7
+    rc_ctrl->sw[3] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF;                    							//Channel 8
+    rc_ctrl->sw[4] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF;                  								//Channel 9
+    rc_ctrl->sw[5] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF;                               	//Channel 10
+    rc_ctrl->sw[6] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF;	//Channel 11
+    rc_ctrl->sw[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF;                    							//Channel 12
+
+	rc_ctrl->ch[0] -= RC_CH_VALUE_OFFSET;
+    rc_ctrl->ch[1] -= RC_CH_VALUE_OFFSET;
+    rc_ctrl->ch[2] -= RC_CH_VALUE_OFFSET;
+    rc_ctrl->ch[3] -= RC_CH_VALUE_OFFSET;
+    
+
+//		//新遥控器
+		rc_ctrl->ch[0] += 20;	//y(-694,693)
+		rc_ctrl->ch[1] += 20;	//x(-693,694)
+		rc_ctrl->ch[2] = -rc_ctrl->ch[2]+144;		//m(-518,843)
+		rc_ctrl->ch[3] += 4;		//w(-694,693)
+	
+    rc_ctrl->sw[2] = map(rc_ctrl->sw[2],306,1694,1694,306);	
+    rc_ctrl->sw[3] = map(rc_ctrl->sw[3],306,1694,1694,306);	
+//		rc_ctrl->ch[1] = map(rc_ctrl->ch[1],656,-656,-700,700);		//x
+		rc_ctrl->ch[0] = map(rc_ctrl->ch[0],-800,796,700,-700);	//y
+		rc_ctrl->ch[2] = map(rc_ctrl->ch[2],-632,901,25,0);		//m
+		rc_ctrl->ch[3] = map(rc_ctrl->ch[3],-820,780,-700,700);  
+		rc_ctrl->ch[3] = 0.5*(rc_ctrl->ch[3]);
+    rc_ctrl->ch[1] = 0.5*(rc_ctrl->ch[1]);
+    rc_ctrl->ch[0] = 0.5*(rc_ctrl->ch[0]);		
+//		
+		//死区(-30,30)
+		if(rc_ctrl->ch[0]>-14&&rc_ctrl->ch[0]<10) rc_ctrl->ch[0]=0;              
+		if(rc_ctrl->ch[1]>-30&&rc_ctrl->ch[1]<20) rc_ctrl->ch[1]=0;                
+		if(rc_ctrl->ch[2]>=0&&rc_ctrl->ch[2]<=3) rc_ctrl->ch[2]=0;              
+		if(rc_ctrl->ch[3]>-22&&rc_ctrl->ch[3]<22) rc_ctrl->ch[3]=0;        
+
+    remote_ready = 1;
+}                                                                            																																					
+
+
+
+int map(int x, int in_min, int in_max, int out_min, int out_max)		//映射函数
+{
+	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+
+/*
+
+
+
+  																																	
+306																							  					  	  306
+ sw[]																						 	 					  		sw[7]
+1694																							 					  		1694
+ 
+306																														    	306
+sw[6]                                                              sw[4]
+1694                                                               1694
+
+306         306                                        306        306   
+sw[0]      sw[2]    sw[1]:306-1694   sw[5]:306-1694  sw[]1000   sw[3] 
+1694       1694                                         1694       1694
+                                      710
+               688                   1425
+                |                      |
+                |                      |
+   54    -616------ch[3]770   -354---------ch[0] 339  0
+                |                      |
+                |                      |
+	             ch[2]                 ch[1]
+               _699                   38   
+*/

User/device/remote_control.h

@@ -0,0 +1,42 @@
+/**
+  ****************************(C) COPYRIGHT 2016 DJI****************************
+  * @file       remote_control.c/h
+  * @brief      遥控器处理，遥控器是通过类似SBUS的协议传输，利用DMA传输方式节约CPU
+  *             资源，利用串口空闲中断来拉起处理函数，同时提供一些掉线重启DMA，串口
+  *             的方式保证热插拔的稳定性。
+  * @note       
+  * @history
+  *  Version    Date            Author          Modification
+  *  V1.0.0     Dec-26-2018     RM              1. 完成
+  *
+  @verbatim
+  ==============================================================================
+
+  ==============================================================================
+  @endverbatim
+  ****************************(C) COPYRIGHT 2016 DJI****************************
+  */
+#ifndef REMOTE_CONTROL_H
+#define REMOTE_CONTROL_H
+#include "struct_typedef.h"
+#include "bsp_rc.h"
+
+#define SBUS_RX_BUF_NUM 50u
+
+#define RC_FRAME_LENGTH 25u
+
+#define RC_CH_VALUE_OFFSET  ((uint16_t)1024)
+
+typedef  struct
+{
+	int16_t ch[4];
+  int16_t sw[8]; 
+
+}__attribute__((packed)) RC_ctrl_t;
+
+extern void remote_control_init(void);
+static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl);
+int map(int x, int in_min, int in_max, int out_min, int out_max);		//映射函数
+
+
+#endif

User/task/error_detect_task.c

@@ -51,6 +51,7 @@
   */
 #include "error_detect.h"
 #include "user_task.h"
+#include "r12ds.h"
 #include "nuc.h"
 /**
   * @brief          init error_list, assign  offline_time, online_time, priority.

User/task/init.c

@@ -33,6 +33,8 @@ void Task_Init(void *argument) {
   /* 任务*/
   task_runtime.thread.atti_esti =
       osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti);
+	task_runtime.thread.dr16 =
+      osThreadNew(Task_dr16,NULL,&attr_dr16);
   task_runtime.thread.can =
       osThreadNew(Task_can,NULL,&attr_can);
   task_runtime.thread.cmd =
@@ -42,8 +44,6 @@ void Task_Init(void *argument) {
 	task_runtime.thread.up=
       osThreadNew(Task_up,NULL,&attr_up);
 		
-	task_runtime.thread.dr16 =
-      osThreadNew(Task_dr16,NULL,&attr_dr16);
 			
 			
 	task_runtime.thread.error_detect =
@@ -81,9 +81,12 @@ void Task_Init(void *argument) {
       osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
 	
 		/*cmd */
-
+   task_runtime.msgq.cmd.raw.rc =
+      osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
    task_runtime.msgq.cmd.raw.nuc =
       osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL);	 
+   task_runtime.msgq.cmd.up_ctrl =
+      osMessageQueueNew(3u,sizeof(CMD_t), NULL);	 
 
   osKernelUnlock();
   osThreadTerminate(osThreadGetId()); /* 结束自身 */

User/task/nuc_task.c

@@ -6,6 +6,8 @@
 NUC_t nuc_raw;
 CMD_NUC_t cmd_fromnuc;
 #else 
+static NUC_t nuc_raw;
+static CMD_NUC_t cmd_fromnuc;
 
 
 #endif

User/task/r12ds_task.c

@@ -0,0 +1,56 @@
+
+#include "r12ds.h"
+#include "cmsis_os.h"
+#include "user_task.h"
+
+//#ifdef DEBUG
+
+//CMD_RC_t cmd_rc;
+
+//uint8_t sbus_rx_buf[2][RC_FRAME_LENGTH];
+
+//#else 
+
+//static CMD_RC_t cmd_rc;
+
+//#endif
+
+
+
+//CMD_RC_t *R12ds_DataGet(){
+//	return &cmd_rc; 
+//}
+//	
+//void Task_r12ds(void *argument)
+//{
+//   (void)argument;
+//  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_R12DS;
+//    R12ds_DMA_Init(sbus_rx_buf[0],sbus_rx_buf[1],50);
+//	
+//      uint32_t tick = osKernelGetTickCount();
+//   while (1)
+//   {
+//#ifdef DEBUG
+//    task_runtime.stack_water_mark.rs12ds =
+//        osThreadGetStackSpace(osThreadGetId());
+//#endif
+//		 
+//      if(R12Buf0_WaitDmaCplt(1))
+//			{
+//			 sbus_to_rc(sbus_rx_buf[0],&cmd_rc);
+//			}
+//		  else if(R12Buf1_WaitDmaCplt(1))	
+//			{
+//				sbus_to_rc(sbus_rx_buf[1],&cmd_rc);
+//			}
+//      osMessageQueueReset(task_runtime.msgq.cmd.raw.rc);
+//	    osMessageQueuePut(task_runtime.msgq.cmd.raw.rc,(&cmd_rc),0,0);
+//	 
+//	 	tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/
+//		 osDelay(10);
+//   }
+
+
+
+//    
+// }

User/task/r12ds_task.h

@@ -0,0 +1,16 @@
+#ifndef _R12DS_TASK_H
+#define _R12DS_TASK_H
+
+
+
+
+
+
+
+
+
+
+
+
+
+#endif

User/task/up_task.c

@@ -10,7 +10,7 @@
 
 #include "vofa.h"
 
-static CAN_t can;
+static  CAN_t can;
 
 
 #ifdef DEBUG
@@ -34,7 +34,6 @@ static CMD_t up_cmd;
 float  aaa=0;
 float bbb=0;
 float CCC=0;
-float DDD=0;
 
 
 /**
@@ -51,7 +50,6 @@ void Task_up(void *argument)
   uint32_t tick = osKernelGetTickCount();
 	up_init(&UP,&(task_runtime.config.chassis_config->up ),TASK_FREQ_UP);
 	
-	
 	while(1)
 		{
 #ifdef DEBUG
@@ -59,24 +57,29 @@ void Task_up(void *argument)
 #endif
 		UP_UpdateFeedback(&UP, &can,&up_cmd) ;
 //    GM6020_control(&UP, 100) ;
-//			UP_angle_control(&UP, bbb,M3508 );
-						UP_angle_control(&UP, bbb,M2006 );
-
 //			UP_M3508_speed(&UP, 500); 
 			
-			
+//				UP_angle_control(&UP,0,M2006);
-			
-			VESC_M5065_Control(&UP,  000);
 //			
-//			HAL_GPIO_WritePin (GPIOE ,GPIO_PIN_9 ,GPIO_PIN_SET  );
+//			
-//		
+//			VESC_M5065_Control(&UP,  20000);
-//			HAL_GPIO_WritePin (GPIOE ,GPIO_PIN_11 ,GPIO_PIN_SET  );
 			
+			
+//			
 //			GO_SendData(&UP, 1,CCC);
-			GO_SendData(&UP, 0,aaa);
+//			GO_SendData(&UP, 0,aaa);
 
-		ALL_Motor_Control(&UP,&UP_CAN_out);
+      UP_control(&UP,&UP_CAN_out,&up_cmd);
-//		UP_control(&UP,&UP_CAN_out);
+//			UP.motor_target .go_shoot =aaa;
+//			UP.motor_target .M2006_angle =bbb ;
+
+//      UP_control(&UP,&UP_CAN_out,&up_cmd);
+//			UP.motor_target .M3508_angle =CCC;
+//			UP.motor_target .go_shoot =aaa;
+//			UP.motor_target .M2006_angle =bbb;
+//			UP.motor_target .go_spin  =ddd;
+			
+			ALL_Motor_Control(&UP,&UP_CAN_out);
 
 		osDelay(1);
 			
@@ -89,10 +92,9 @@ void Task_up(void *argument)
 	osMessageQueueGet(task_runtime.msgq.imu.accl, &UP.pos088.bmi088.accl,NULL, 0);
 	/*can上设备数据获取*/	
 	osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
+					
+		osMessageQueueGet(task_runtime.msgq.cmd .up_ctrl , &up_cmd, NULL, 0);
 		
- 		
-	osMessageQueueGet(task_runtime.msgq.cmd.up_ctrl, &up_cmd, NULL, 0);
-	
 	/*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/		
 	osKernelLock();
 	

User/task/user_task.c

@@ -25,6 +25,21 @@ const osThreadAttr_t attr_atti_esti = {
     .priority = osPriorityRealtime,
     .stack_size = 256 * 4,
 };
+//const osThreadAttr_t attr_ops_9pos = {
+//	  .name = "action",
+//	  .priority = osPriorityRealtime,
+//	  .stack_size = 256 *4,
+//};
+//const osThreadAttr_t attr_chassis = {
+//    .name = "chassis",
+//    .priority = osPriorityAboveNormal,
+//    .stack_size = 512 * 4,
+//};
+const osThreadAttr_t attr_r12ds = {
+    .name = "r12ds",
+    .priority = osPriorityRealtime,
+    .stack_size = 128 * 4,
+};
 
 const osThreadAttr_t attr_can = {
     .name = "can",
@@ -50,13 +65,20 @@ const osThreadAttr_t attr_error_detect = {
 	  .stack_size = 128 *4,
 };
 
-const osThreadAttr_t attr_up = {
-	   .name = "up",
-	   .priority = osPriorityRealtime,
-	   .stack_size = 512 * 4,
-};
 const osThreadAttr_t attr_dr16 = {
 	   .name = "dr16",
 	   .priority = osPriorityRealtime,
 	   .stack_size = 128 *4,
 };
+
+//const osThreadAttr_t attr_navi = {
+//	   .name = "navi",
+//	   .priority = osPriorityRealtime,
+//	   .stack_size = 128 *4,
+//};
+
+const osThreadAttr_t attr_up = {
+	   .name = "up",
+	   .priority = osPriorityRealtime,
+	   .stack_size = 512 * 4,
+};

User/task/user_task.h

@@ -19,7 +19,7 @@
 #define TASK_FREQ_NUC (500u)
 #define TASK_FREQ_CAN (1000u)
 #define TASK_FREQ_NAVI (500u)
-#define TASK_FREQ_DR16 (1000u)
+#define TASK_FREQ_R12DS (1000u)
 #define TASK_FREQ_ACTION (500u)
 
 #define TASK_FREQ_UP (500u)  //新加的上层
@@ -44,10 +44,10 @@ typedef struct {
     osThreadId_t atti_esti;
 		osThreadId_t dr16;
 		osThreadId_t up;
+    osThreadId_t nuc;
   
     osThreadId_t can;
 		osThreadId_t cmd;
-    osThreadId_t nuc;
 		osThreadId_t error_detect;
   } thread;
 
@@ -62,11 +62,9 @@ typedef struct {
     struct {
       struct {
         osMessageQueueId_t rc;
-        osMessageQueueId_t nuc;
+	      osMessageQueueId_t nuc;
+			
         osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
-        osMessageQueueId_t Action;
-				osMessageQueueId_t ops_9_out;
-				
 		 
       }raw;
       osMessageQueueId_t up_ctrl;
@@ -104,11 +102,9 @@ typedef struct {
     UBaseType_t can;
     UBaseType_t atti_esti;
 		UBaseType_t dr16;
-    UBaseType_t action;
     UBaseType_t cmd;
-    UBaseType_t nuc;
-		UBaseType_t navi;
 		UBaseType_t error_detect;
+    UBaseType_t nuc;
 		
 		UBaseType_t up;
 		
@@ -117,9 +113,8 @@ typedef struct {
   struct {
     float can;
     float atti_esti;
-    float dr16;
+    float r12ds;
     float cmd;
-    float nuc;
 		float error_detect;
 		
 		float up;
@@ -128,9 +123,8 @@ typedef struct {
   struct {
     float can;
     float atti_esti;
-    float dr16;
+    float r12ds;
     float cmd;
-    float nuc;
 		float error_detect;
 		float up;
 		
@@ -149,12 +143,12 @@ extern const osThreadAttr_t attr_can;
 
 extern const osThreadAttr_t attr_cmd;
 
-extern const osThreadAttr_t attr_nuc;
-
 extern const osThreadAttr_t attr_error_detect;
 
 extern const osThreadAttr_t attr_dr16;
 
+extern const osThreadAttr_t attr_nuc;
+
 extern const osThreadAttr_t attr_up;
 /* Exported functions prototypes -------------------------------------------- */
 void Task_Init(void *argument);
@@ -165,12 +159,11 @@ void Task_can(void *argument);
 
 void Task_cmd(void *argument);
 
-void Task_nuc(void *argument);
-
 void Task_up(void *argument);
 
-void Task_dr16(void *argument);
+void Task_nuc(void *argument);
 
 void Task_error_detect(void *argument);
 
+void Task_dr16(void *argument);
 #endif

keilkill.bat

@@ -1,27 +0,0 @@
-del *.bak /s
-del *.ddk /s
-del *.edk /s
-del *.lst /s
-del *.lnp /s
-del *.mpf /s
-del *.mpj /s
-del *.obj /s
-del *.omf /s
-::del *.opt /s  ::不允许删除JLINK的设置
-del *.plg /s
-del *.rpt /s
-del *.tmp /s
-del *.__i /s
-del *.crf /s
-del *.o /s
-del *.d /s
-del *.axf /s
-del *.tra /s
-del *.dep /s           
-del JLinkLog.txt /s
-
-del *.iex /s
-del *.htm /s
-del *.sct /s
-del *.map /s
-exit