加了视觉

Core/Src/usart.c

@@ -102,7 +102,7 @@ void MX_USART6_UART_Init(void)
 
   /* USER CODE END USART6_Init 1 */
   huart6.Instance = USART6;
-  huart6.Init.BaudRate = 4000000;
+  huart6.Init.BaudRate = 115200;
   huart6.Init.WordLength = UART_WORDLENGTH_8B;
   huart6.Init.StopBits = UART_STOPBITS_1;
   huart6.Init.Parity = UART_PARITY_NONE;

MDK-ARM/.vscode/c_cpp_properties.json

@@ -3,26 +3,26 @@
         {
             "name": "R1-shooter",
             "includePath": [
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Core\\Inc",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Core\\Inc",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Drivers\\CMSIS\\Include",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Include",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\bsp",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\bsp",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\module",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\module",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\task",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\task",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\lib",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\lib",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\User\\device",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\device",
                 "D:\\keil\\ARM\\ARMCC\\include",
                 "D:\\keil\\ARM\\ARMCC\\include\\rw",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\MDK-ARM",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\MDK-ARM",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Core\\Src",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Core\\Src",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Src",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Src",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source",
-                "d:\\Desktop\\r1\\r1_upper - xiaomi\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
             ],
             "defines": [
                 "USE_HAL_DRIVER",

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

@@ -104,3 +104,23 @@
 [info] Log at : 2025/5/22|16:11:31|GMT+0800
 >>>>>>> 8fc2f50677ecabc32b37eac7a7df89d916fefef0
 
+[info] Log at : 2025/5/23|21:04:08|GMT+0800
+
+[info] Log at : 2025/5/25|14:26:08|GMT+0800
+
+[info] Log at : 2025/5/26|19:07:26|GMT+0800
+
+[info] Log at : 2025/5/26|20:16:52|GMT+0800
+
+[info] Log at : 2025/5/27|00:06:58|GMT+0800
+
+[info] Log at : 2025/5/27|19:35:30|GMT+0800
+
+[info] Log at : 2025/5/28|13:50:56|GMT+0800
+
+[info] Log at : 2025/5/29|19:03:54|GMT+0800
+
+[info] Log at : 2025/5/30|19:34:50|GMT+0800
+
+[info] Log at : 2025/5/31|14:39:23|GMT+0800
+

MDK-ARM/.vscode/uv4.log

@@ -1,8 +1,8 @@
 *** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
 Build target 'R1-shooter'
-compiling gimbalTask.cpp...
+compiling usart.c...
 linking...
-Program Size: Code=28656 RO-data=1824 RW-data=252 ZI-data=23940  
+Program Size: Code=30588 RO-data=2144 RW-data=276 ZI-data=24012  
 FromELF: creating hex file...
 "R1-shooter\R1-shooter.axf" - 0 Error(s), 0 Warning(s).
-Build Time Elapsed:  00:00:03
+Build Time Elapsed:  00:00:05

MDK-ARM/.vscode/uv4.log.lock

@@ -1 +1 @@
-2025/5/23 20:55:43
+2025/5/31 16:57:56

MDK-ARM/R1-shooter.uvoptx

@@ -103,7 +103,7 @@
         <bEvRecOn>1</bEvRecOn>
         <bSchkAxf>0</bSchkAxf>
         <bTchkAxf>0</bTchkAxf>
-        <nTsel>6</nTsel>
+        <nTsel>3</nTsel>
         <sDll></sDll>
         <sDllPa></sDllPa>
         <sDlgDll></sDlgDll>
@@ -114,7 +114,7 @@
         <tDlgDll></tDlgDll>
         <tDlgPa></tDlgPa>
         <tIfile></tIfile>
-        <pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
+        <pMon>BIN\CMSIS_AGDI.dll</pMon>
       </DebugOpt>
       <TargetDriverDllRegistry>
         <SetRegEntry>
@@ -140,7 +140,7 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGUARM</Key>
-          <Name>(105=-1,-1,-1,-1,0)</Name>
+          <Name></Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -153,40 +153,7 @@
           <Name>-U00160029510000164E574E32 -O2254 -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 -FO15 -FD20000000 -FC800 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
         </SetRegEntry>
       </TargetDriverDllRegistry>
-      <Breakpoint>
+      <Breakpoint/>
-        <Bp>
-          <Number>0</Number>
-          <Type>0</Type>
-          <LineNumber>169</LineNumber>
-          <EnabledFlag>1</EnabledFlag>
-          <Address>134235782</Address>
-          <ByteObject>0</ByteObject>
-          <HtxType>0</HtxType>
-          <ManyObjects>0</ManyObjects>
-          <SizeOfObject>0</SizeOfObject>
-          <BreakByAccess>0</BreakByAccess>
-          <BreakIfRCount>1</BreakIfRCount>
-          <Filename>..\User\device\djiMotor.c</Filename>
-          <ExecCommand></ExecCommand>
-          <Expression>\\R1_shooter\../User/device/djiMotor.c\169</Expression>
-        </Bp>
-        <Bp>
-          <Number>1</Number>
-          <Type>0</Type>
-          <LineNumber>174</LineNumber>
-          <EnabledFlag>1</EnabledFlag>
-          <Address>134235770</Address>
-          <ByteObject>0</ByteObject>
-          <HtxType>0</HtxType>
-          <ManyObjects>0</ManyObjects>
-          <SizeOfObject>0</SizeOfObject>
-          <BreakByAccess>0</BreakByAccess>
-          <BreakIfRCount>1</BreakIfRCount>
-          <Filename>..\User\device\djiMotor.c</Filename>
-          <ExecCommand></ExecCommand>
-          <Expression>\\R1_shooter\../User/device/djiMotor.c\174</Expression>
-        </Bp>
-      </Breakpoint>
       <WatchWindow1>
         <Ww>
           <count>0</count>
@@ -251,57 +218,57 @@
         <Ww>
           <count>12</count>
           <WinNumber>1</WinNumber>
-          <ItemText>ball_state,0x0A</ItemText>
+          <ItemText>shoot,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>13</count>
           <WinNumber>1</WinNumber>
-          <ItemText>flag,0x0A</ItemText>
+          <ItemText>speedm,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>14</count>
           <WinNumber>1</WinNumber>
-          <ItemText>shoot,0x0A</ItemText>
+          <ItemText>shoot_flag,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>15</count>
           <WinNumber>1</WinNumber>
-          <ItemText>speedm,0x0A</ItemText>
+          <ItemText>angle1,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>16</count>
           <WinNumber>1</WinNumber>
-          <ItemText>a1,0x0A</ItemText>
+          <ItemText>motor_5065</ItemText>
         </Ww>
         <Ww>
           <count>17</count>
           <WinNumber>1</WinNumber>
-          <ItemText>a2,0x0A</ItemText>
+          <ItemText>test_distance</ItemText>
         </Ww>
         <Ww>
           <count>18</count>
           <WinNumber>1</WinNumber>
-          <ItemText>shoot_flag,0x0A</ItemText>
+          <ItemText>xxx</ItemText>
         </Ww>
         <Ww>
           <count>19</count>
           <WinNumber>1</WinNumber>
-          <ItemText>angle1,0x0A</ItemText>
+          <ItemText>test_speed,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>20</count>
           <WinNumber>1</WinNumber>
-          <ItemText>rx_header</ItemText>
+          <ItemText>speedm,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>21</count>
           <WinNumber>1</WinNumber>
-          <ItemText>rx_data</ItemText>
+          <ItemText>Torque,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>22</count>
           <WinNumber>1</WinNumber>
-          <ItemText>motor_5065</ItemText>
+          <ItemText>triggerspeed,0x0A</ItemText>
         </Ww>
       </WatchWindow1>
       <WatchWindow2>
@@ -1113,7 +1080,7 @@
 
   <Group>
     <GroupName>User/module</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>

README.md

@@ -14,4 +14,15 @@ r1上层
 
 //PC6  接球光电
 
-//PI6  运球光电
+//PI6  运球光电
+
+## 上层过程
+
++ 一个按键切换 运球 /发射
++ 👈 运球 +
++ .3
+
+
+
+
+

User/device/GO_M8010_6_Driver.c

@@ -32,7 +32,7 @@ void GO_M8010_init (){
         
     }
     // HAL_UART_RegisterCallback(&huart1, HAL_UART_TX_COMPLETE_CB_ID, uartTxCB);
-       HAL_UART_RegisterCallback(&huart6, HAL_UART_TX_COMPLETE_CB_ID, uartTxCB);
+    //   HAL_UART_RegisterCallback(&huart6, HAL_UART_TX_COMPLETE_CB_ID, uartTxCB);
 }
 
 

User/device/djiMotor.c

@@ -78,11 +78,18 @@
 
   }
 
+//小米
+#define ID_xiaomi 1		
+#define Pmax 1		
+#define speed_full_scale 200.0f
+#define current_full_scale 4.0f
+
+
 #if DEBUG == 1
 
 //电机回传数据结构体
 motor_measure_t motor_chassis[10];
-CAN_MotorFeedback_t motor_5065[2];
+CAN_MotorFeedback_t motor_5065[3];
 #else
 static motor_measure_t motor_chassis[5];
 #endif
@@ -93,16 +100,25 @@ static uint8_t              can_send_data_2ff[8];
 static CAN_TxHeaderTypeDef  tx_message_200;
 static uint8_t              can_send_data_200[8];
 
+//can1 dji
 CAN_RxHeaderTypeDef dji_rx_header;
 uint8_t dji_rx_data[8];
 
+CAN_RawTx_t raw_tx;
+
+//can2 vesc xiaomi
 CAN_RxHeaderTypeDef rx_header;
 uint8_t rx_data[8];
 
 static CAN_TxHeaderTypeDef vesc_tx_message;
 static uint8_t  vesc_send_data[4];
+//小米
+uint8_t can_send_data[8];
+// 定义 CAN 传输头部信息
+static CAN_TxHeaderTypeDef tx_message;
+
+MotorFeedback_t MotorFeedback;
 
-CAN_RawTx_t raw_tx;
 /**
  * @brief           数据处理函数
  * @param[in]       none
@@ -168,15 +184,28 @@ void can2MotorEncode()
       // 存储消息到对应的电机结构体中
       VescMotor_Decode(&motor_5065[0], &rx_header,rx_data);
       break;
-
     case CAN_VESC5065_M2_MSG1:
       // 存储消息到对应的电机结构体中
       VescMotor_Decode(&motor_5065[1], &rx_header,rx_data);
+     case CAN_VESC5065_M3_MSG1:
+      // 存储消息到对应的电机结构体中
+      VescMotor_Decode(&motor_5065[2], &rx_header,rx_data);
       break;
-      
+
     default:
       break;
   }
+
+    switch (rx_data[0]) 
+  {
+				 case ID_xiaomi:
+				 Parse_CAN_Response (rx_data,&MotorFeedback);
+				 break ;
+  
+   default:
+    break;
+  }
+
 }
 
 #if FREERTOS_DJI == 0
@@ -387,7 +416,7 @@ return DEVICE_OK;
         uint32_t send_mail_box;
     
         id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8);
-        assert_param_rpm(&RPM);
+        //assert_param_rpm(&RPM);
         data = (int32_t)(RPM);
     
         vesc_tx_message.ExtId = id;
@@ -437,3 +466,69 @@ return DEVICE_OK;
  {
     return &motor_5065[i];
  }
+
+
+ //小米
+ void Parse_CAN_Response(uint8_t* rx_data, MotorFeedback_t* feedback) {
+  // 1. 解析位置
+  int16_t pos_code = (rx_data[1] << 8) | rx_data[2];
+  feedback->position_deg = ((int16_t)(pos_code - 0x8000) / 32768.0f) * 360.0f * Pmax;
+
+  // 2. 解析速度
+  int16_t speed_code = ((rx_data[3] << 4) | (rx_data[4] >> 4)) - 0x800;
+  feedback->speed_rads = (speed_code / 2048.0f) * speed_full_scale;
+
+  // 3. 解析电流
+  int16_t current_code = (((rx_data[4] & 0x0F) << 8) | rx_data[5]) - 0x800;
+  feedback->current_A = (current_code / 2048.0f) * current_full_scale;
+
+  // 记录电机ID
+  feedback->can_id = rx_data[0];
+}
+
+
+void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan)
+{
+    uint32_t send_mail_box;
+	  static int is_open=0;  //是否开启电机，根据手册，第一次使用需要ff开启
+    tx_message.StdId = id;
+    tx_message.IDE = CAN_ID_STD;
+    tx_message.RTR = CAN_RTR_DATA;
+    tx_message.DLC = 0x08;
+	if(is_open){
+
+		uint16_t pos_code = (uint16_t)((JZ_xiaomi->position  / (Pmax * 360.0f)) * 32768.0f + 32768.0f);
+		uint16_t speed_code = (uint16_t)((JZ_xiaomi->speed  / 200.0f) * 2048.0f + 2048.0f);
+		uint16_t current_code = (uint16_t)((JZ_xiaomi->K_C  / 4.0f) * 2048.0f + 2048.0f);
+
+		can_send_data[0] = (pos_code >> 8) & 0xFF;    // 位置高8位
+		can_send_data[1] = pos_code & 0xFF;            // 位置低8位
+
+		can_send_data[2] = (speed_code >> 4) & 0xFF;  // 速度高8位（取12位中的高8）
+		can_send_data[3] = ((speed_code & 0x0F) << 4) | ((JZ_xiaomi->K_P >> 8) & 0x0F); // 速度低4位 + Kp高4位
+
+		can_send_data[4] = JZ_xiaomi->K_P  & 0xFF;                 // Kp低8位
+
+		can_send_data[5] = (JZ_xiaomi->K_D  >> 4) & 0xFF;           // Kd高8位（取12位中的高8）
+		can_send_data[6] = ((JZ_xiaomi->K_D & 0x0F) << 4) | ((current_code >> 8) & 0x0F); // Kd低4位 + 电流高4位
+
+		can_send_data[7] = current_code & 0xFF;       // 电流低8位	
+			
+	}
+	else
+	{
+		can_send_data[0] = 0xFF;
+		can_send_data[1] = 0xFF;
+		can_send_data[2] = 0xFF;
+		can_send_data[3] = 0xFF;
+		can_send_data[4] = 0xFF;
+		can_send_data[5] = 0xFF;
+		can_send_data[6] = 0xFF;
+		can_send_data[7] = 0xFC;	
+		is_open=1;
+		}
+
+	
+  	HAL_CAN_AddTxMessage(hcan, &tx_message, can_send_data, &send_mail_box);
+}
+

User/device/djiMotor.h

@@ -10,7 +10,7 @@ extern "C"{
 #include "device.h"
 #include "can.h"
 #include <math.h>
-
+#include "string.h"
 
 typedef enum{
   GM6020 = 0,
@@ -44,6 +44,7 @@ enum{
   GM6020_2 = 0x208,
   CAN_VESC5065_M1_MSG1 =0x901,           //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
 	CAN_VESC5065_M2_MSG1 =0x902,
+  CAN_VESC5065_M3_MSG1 =0x903,
 };
 
 //vesc指令
@@ -170,6 +171,38 @@ void  CAN_VESC_HEAD(uint8_t controller_id);
 
 int8_t CAN_VESC_Control(int id,int speed ,CAN_HandleTypeDef*hcan);
 
+
+//小米
+
+typedef struct 
+{
+	int16_t position;
+	int16_t    speed;    
+	int16_t    K_P;
+	int16_t    K_D;
+	int16_t    K_C;
+	
+	int16_t    Pmax;//决定最大角度值，+-1 -> 最大+-360°
+	
+}JZ_xiaomi_t;
+
+typedef struct {
+	
+  uint8_t can_id;
+  float position_deg;
+  float speed_rads;
+  float current_A;
+	
+} MotorFeedback_t;
+
+
+
+void Parse_CAN_Response(uint8_t* rx_data, MotorFeedback_t* feedback) ;
+
+
+void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan);
+
+
 #ifdef __cplusplus
 }
 #endif

User/device/nuc.c

@@ -77,19 +77,12 @@ int8_t NUC_RawParse(NUC_t *n) {
   } instance; // 方便在浮点数和字符数组之间进行数据转换
 
   // 校验数据包头
-  if (nucbuf[0] != HEAD) {
+  if(nucbuf[0]!=HEAD) goto error;  //发送ID不是底盘
-      return DEVICE_ERR;
+  else
-  }
+  {
-  // 提取数据包中的 CRC 值（假设 CRC 值在数据包的最后两个字节）
-  uint16_t received_crc = (nucbuf[16] << 8) | nucbuf[17]; // 假设 CRC 在第 16 和 17 字节
-  uint16_t calculated_crc = do_crc_table(nucbuf, 16);     // 计算前 16 字节的 CRC
-  // 校验 CRC
-  if (received_crc == calculated_crc) {
-    
     n->status_fromnuc = nucbuf[1];
-  n->ctrl_status = nucbuf[2];
+    n->ctrl_status = nucbuf[2];
-
+ switch (n->status_fromnuc) {
-  switch (n->status_fromnuc) {
       case VISION:
           /* 协议格式 
              0xFF  HEAD
@@ -100,42 +93,103 @@ int8_t NUC_RawParse(NUC_t *n) {
              z    fp32
              0xFE  TAIL
           */
-          if (nucbuf[15] != TAIL) goto error;
+      if (nucbuf[15] != TAIL) goto error;
 
-          instance.data[3] = nucbuf[3];
+      instance.data[3] = nucbuf[3];
-          instance.data[2] = nucbuf[4];
+      instance.data[2] = nucbuf[4];
-          instance.data[1] = nucbuf[5];
+      instance.data[1] = nucbuf[5];
-          instance.data[0] = nucbuf[6];
+      instance.data[0] = nucbuf[6];
-          n->vision.x = instance.x[0];
+      n->vision.x = instance.x[0];
 
-          instance.data[7] = nucbuf[7];
+      instance.data[7] = nucbuf[7];
-          instance.data[6] = nucbuf[8];
+      instance.data[6] = nucbuf[8];
-          instance.data[5] = nucbuf[9];
+      instance.data[5] = nucbuf[9];
-          instance.data[4] = nucbuf[10];
+      instance.data[4] = nucbuf[10];
-          n->vision.y = instance.x[1];
+      n->vision.y = instance.x[1];
 
-          instance.data[11] = nucbuf[11];
+      instance.data[11] = nucbuf[11];
-          instance.data[10] = nucbuf[12];
+      instance.data[10] = nucbuf[12];
-          instance.data[9] = nucbuf[13];
+      instance.data[9] = nucbuf[13];
-          instance.data[8] = nucbuf[14];
+      instance.data[8] = nucbuf[14];
-          n->vision.z = instance.x[2];
+      n->vision.z = instance.x[2];
           break;
-
-
   }
-  return DEVICE_OK;
+   return DEVICE_OK;
- }
-  else 
-  {
-    return DEVICE_ERR;
-  
   }
 
+
 error:
   return DEVICE_ERR;
 }
 
 
+// int8_t NUC_RawParse(NUC_t *n) {
+//   if (n == NULL) return DEVICE_ERR_NULL;
+//   union {
+//       float x[3];
+//       char data[12];
+//   } instance; // 方便在浮点数和字符数组之间进行数据转换
+
+//   // 校验数据包头
+//   if (nucbuf[0] != HEAD) {
+//       return DEVICE_ERR;
+//   }
+//   // 提取数据包中的 CRC 值（假设 CRC 值在数据包的最后两个字节）
+//   uint16_t received_crc = (nucbuf[16] << 8) | nucbuf[17]; // 假设 CRC 在第 16 和 17 字节
+//   uint16_t calculated_crc = do_crc_table(nucbuf, 16);     // 计算前 16 字节的 CRC
+//   // 校验 CRC
+//   if (received_crc == calculated_crc) {
+    
+//     n->status_fromnuc = nucbuf[1];
+//   n->ctrl_status = nucbuf[2];
+
+//   switch (n->status_fromnuc) {
+//       case VISION:
+//           /* 协议格式 
+//              0xFF  HEAD
+//              0x02  控制帧
+//              0x01  相机帧
+//              x    fp32
+//              y    fp32
+//              z    fp32
+//              0xFE  TAIL
+//           */
+//           if (nucbuf[15] != TAIL) goto error;
+
+//           instance.data[3] = nucbuf[3];
+//           instance.data[2] = nucbuf[4];
+//           instance.data[1] = nucbuf[5];
+//           instance.data[0] = nucbuf[6];
+//           n->vision.x = instance.x[0];
+
+//           instance.data[7] = nucbuf[7];
+//           instance.data[6] = nucbuf[8];
+//           instance.data[5] = nucbuf[9];
+//           instance.data[4] = nucbuf[10];
+//           n->vision.y = instance.x[1];
+
+//           instance.data[11] = nucbuf[11];
+//           instance.data[10] = nucbuf[12];
+//           instance.data[9] = nucbuf[13];
+//           instance.data[8] = nucbuf[14];
+//           n->vision.z = instance.x[2];
+//           break;
+
+
+//   }
+//   return DEVICE_OK;
+//  }
+//   else 
+//   {
+//     return DEVICE_ERR;
+  
+//   }
+
+// error:
+//   return DEVICE_ERR;
+// }
+
+
 int8_t NUC_HandleOffline(NUC_t *cmd)
 {
 	if(cmd == NULL) return DEVICE_ERR_NULL;

User/module/ball.cpp

@@ -7,8 +7,8 @@
 extern RC_ctrl_t rc_ctrl;
 extern int key;
 
-// C键 sw[4]👆 200   中1000  👇1800
+// C键 sw[5]👆 1800   中1000  👇200
-// D键 sw[5]👆 1800   👇200
+// D键 sw[6]👆 200   👇1800
 
 #define M_SPEED 4000
 
@@ -29,7 +29,20 @@ Ball ::Ball()
 {  
    detect_init();
 
- //两个伸缩6020
+ //三摩擦轮
+   for(int i = 0;i < MOTOR_NUM;i ++)
+ {
+    hand_Motor[i] = get_motor_point(i);  
+    if(i <=3)
+    {
+        hand_Motor[i]->type = M3508;//设置电机类型
+        PID_init(&speed_pid[i],PID_POSITION,M3508_speed_PID,16000, 10000);//pid初始化
+    }  
+    result[i] = 0;
+    speedSet[i] = 0;    
+ }
+
+  //两个伸缩6020
    Extern_Motor[0] = get_motor_point(6);
    Extern_Motor[1] = get_motor_point(7);
 
@@ -46,19 +59,13 @@ Ball ::Ball()
    angleSet[0] = 0;
    angleSet[1] = 0;
 
- //三摩擦轮
+    //小米电机初始化
-   for(int i = 0;i < MOTOR_NUM;i ++)
+    JZ_xiaomi.position = 0;    // 
- {
+    JZ_xiaomi.speed    = 20;   // 
-    hand_Motor[i] = get_motor_point(i);  
+    JZ_xiaomi.K_P      = 100;  // 
-    if(i <=3)
+    JZ_xiaomi.K_D      =20;    // 
-    {
+    JZ_xiaomi.K_C      = 2 ;  
-        hand_Motor[i]->type = M3508;//设置电机类型
+    JZ_xiaomi.Pmax     =1;
-        PID_init(&speed_pid[i],PID_POSITION,M3508_speed_PID,16000, 10000);//pid初始化
-    }  
-    result[i] = 0;
-    speedSet[i] = 0;    
- }
-
 
   //状态机状态初始化
   currentState1= BALL_IDLE;
@@ -66,6 +73,7 @@ Ball ::Ball()
 
 }
 
+
 void Ball ::Extend_control(int angle)
 {
     int16_t delta[2];
@@ -92,20 +100,8 @@ void Ball ::Extend_control(int angle)
     // e_result[1] = PID_calc(&extend_speed_pid, Extern_Motor[1]->speed_rpm, delta[1]);
 
 
-
-
 }
 
-// void Ball ::Extend_control(int angle)
-// {
-//     int16_t delta;
-//     angleSet = angle;
-
-//     delta = PID_calc(&extend_angle_pid, Extern_Motor->total_angle, angleSet);
-//     e_result = PID_calc(&extend_speed_pid, Extern_Motor->speed_rpm, delta);
-
-// }
-
 
 void Ball ::Spin(float speed)
 {
@@ -130,6 +126,7 @@ void Ball::ballDown(void)
 
 void Ball::ballStop(void)
 {
+    HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保闭合气缸
      speedm=0;
 
 }
@@ -150,77 +147,70 @@ void Ball::ballTake(void)
  
 void Ball::ballHadling(void)
 {
+    static int last_sw5 = 0; // 保存上一次拨杆状态
+    ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 有球 1 无球 0
 
-    ball_state =HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);//有球 1 无球 0
     switch (currentState1)
     {
     case BALL_IDLE:
-       HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
+        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保闭合气缸
-        if (rc_ctrl.sw[4] > 1000||key > 0) // 检测按键是否被按下,自动回弹的
+        // 只在拨杆从非200切到200时触发
+        if ((rc_ctrl.sw[5] == 200 && last_sw5 != 200) || key > 0)
         {
-            speedm=-4000;
+            speedm = -4000;
             currentState1 = BALL_FORWARD; // 切换到正转状态
         }
         break;
 
     case BALL_FORWARD:
-
+        if (hand_Motor[MOTOR_1]->speed_rpm >= 3980 && hand_Motor[MOTOR_1]->speed_rpm <= 4020 &&
-        if (  hand_Motor[MOTOR_1]->speed_rpm >= 3980&&hand_Motor[MOTOR_1]->speed_rpm <= 4020 &&
+            hand_Motor[MOTOR_2]->speed_rpm <= -3980 && hand_Motor[MOTOR_2]->speed_rpm >= -4020 &&
-              hand_Motor[MOTOR_2]->speed_rpm <= -3980&&hand_Motor[MOTOR_2]->speed_rpm >= -4020 &&          
+            hand_Motor[MOTOR_3]->speed_rpm <= -3980 && hand_Motor[MOTOR_3]->speed_rpm >= -4020)
-              hand_Motor[MOTOR_3]->speed_rpm <= -3980&&hand_Motor[MOTOR_3]->speed_rpm >= -4020 )
         {
-            HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);// 打开气缸
+            HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸
             currentState1 = BALL_DROP; // 切换到球下落状态
         }
         break;
 
     case BALL_DROP:
-
+        if (ball_state == 0) // 读光电 有球 1  无球 0
-        if (ball_state == 0) //读光电 有球 1  无球 0
         {
             osDelay(200); // 延时200ms
-            speedm=3500;
+            speedm = 3500;
             currentState1 = BALL_REVERSE; // 切换到反转状态
         }
-        
         break;
 
     case BALL_REVERSE:
-       
+        if (ball_state == 1)
-        if (ball_state == 1) 
         {
-            
+            speedm = 0; // 停止电机
-            speedm=0; // 停止电机
             currentState1 = BALL_CLOSE; // 切换到完成状态
         }
-     
+        break;
-     break;
 
     case BALL_CLOSE:
-
+        if (ball_state == 1)
-       if(ball_state == 1)
+        {
-       {
+            HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
-           HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
+            currentState1 = BALL_FINISH; // 切换到完成状态
-           currentState1 = BALL_FINISH; // 切换到完成状态
+        }
-       }
+        break;
-
-        break;    
 
     case BALL_FINISH:
-        osDelay(200); // 延时50ms
+        osDelay(200); // 延时200ms
-        key=0; // 重置按键状态
+        key = 0; // 重置按键状态
-        speedm=0;
+        speedm = 0;
         osThreadFlagsSet(task_struct.thread.shoot, BALL_OK);
-        currentState1 = BALL_IDLE; // 回到空闲状态
+        currentState1 = BALL_IDLE; // 直接回到空闲状态
         break;
 
     default:
-       currentState1 = BALL_IDLE; // 默认回到空闲状态
+        currentState1 = BALL_IDLE; // 默认回到空闲状态
         break;
     }
 
-
+    last_sw5 = rc_ctrl.sw[5]; // 更新上一次拨杆状态
-
 }
 
 void Ball::Send_control()

User/module/ball.hpp

@@ -55,11 +55,14 @@ public:
     void ballStop(void);
     void ballTake(void);
     void Extend_control(int angle);
-
+  
     BallState_t currentState1; // 当前状态
     int flag;//暂时还没用到
 	  int ball_state ;//光电检测
     
+    //小米电机伸缩
+    JZ_xiaomi_t JZ_xiaomi;
+
     //伸缩6020
     int16_t e_result[2];
     motor_measure_t *  Extern_Motor[2];
@@ -72,15 +75,13 @@ public:
     volatile uint32_t flag_thread;//接收传回的线程通知
 
 private:  
-    //滤波一下
-  
 
     //三个摩擦轮
     static const float M3508_speed_PID[3];
     static const float M3508_angle_PID[3];
 
     static const float Extend_speed_PID[3];
-    static const float Extend_angle_PID[3];
+    static const float Extend_angle_PID[3]; 
 
     //电机速度pid结构体
     pid_type_def speed_pid[MOTOR_NUM];

User/module/shoot.cpp

@@ -8,24 +8,32 @@
 #include "vofa.h"
 
 extern RC_ctrl_t rc_ctrl;
+NUC_t nuc_v;
+
 float vofa[8];
 #define SHOOT_SPEED 30500
 #define SHOOT_SPEED_BACK -2500
 #define Error 50
 
-int test_speed =30500;
+int triggerspeed = -5000; // 扳机速度
+int test_speed =35200;
+float test_distance =1;
+float  xxx;
 
 #define STOP 0
 
+int Torque = -5000; // 扳机恒力
 #define Trigger_Torque -5000
+#define Trigger_Slow 2000//扳机慢速 不用加负号
 
 //sw[7]👆 1694  中 1000 👇306
 //sw[2]👆 1694   👇306
 
-//F键 sw[0]👆 1800  中 1000 👇200
+//E键 sw[1]👆 200 shoot  中 1000 stop 👇1800  error
-//E键 sw[1]👆 1800   👇200
+//F键 sw[0]👆 1800 开   👇200 关
+//B键 sw[3]👆 200 开   👇1800 关
 
-const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0};
+const fp32 Shoot:: M2006_speed_PID[3] = {50, 0, 0};
 int t=0;
 
 Shoot::Shoot()
@@ -33,8 +41,8 @@ Shoot::Shoot()
     //扳机初始化
     trigger_Motor= get_motor_point(4);
     trigger_Motor->type=M2006;
-    PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,6000, 1000);//pid初始化
+    PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,8000, 1000);//pid初始化
-    speedSet = 0; 
+    t_speedSet = 0; 
     result = 0;
    
     //发射摩擦轮
@@ -46,80 +54,84 @@ Shoot::Shoot()
 
 void Shoot::trigger_control()
 {
-    ///speedSet=speed;
+   // result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet);
-    //result = PID_calc(&speed_pid, trigger_Motor->speed_rpm, speedSet);
-    //result = Trigger_Torque;
     CAN_cmd_1FF(result,0,0,0,&hcan1);
-
-}
-
-void Shoot::shootThree()
-{
-    if((rc_ctrl.sw[7]==1694))
-    {
-
-        speed_5065 = SHOOT_SPEED;
-
-    }
-    if((rc_ctrl.sw[7]==1000))
-    {
-		   speed_5065=STOP;
-     //发一次会堵塞另一个      
-//       CAN_VESC_HEAD(1); 
-//       CAN_VESC_HEAD(2); 
-    }
-     if(rc_ctrl.sw[7]==306)
-    {
-		speed_5065=SHOOT_SPEED_BACK;
-        
-    }
-        CAN_VESC_Control(1,speed_5065,&hcan2);
-        // CAN_VESC_RPM(1, speed_5065); 
-        // CAN_VESC_RPM(2, speed_5065);
-
-        // vofa[0] = motor_5065[1]->rotor_speed;
-        // vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
-        // vofa_tx_main(vofa); // 发送数据到虚拟串口
-}
-
-void Shoot::shootBack()
-{
-      speed_5065=SHOOT_SPEED_BACK;      
-      CAN_VESC_Control(1,speed_5065,&hcan2);
-    
 }
 
 void Shoot::shootStop()
 {
-      speed_5065=STOP;      
+      speed_5065=STOP;   
-      CAN_VESC_Control(1,speed_5065,&hcan2);
+      result=STOP;//扳机慢速归位   
-  
+    
+
 }
 
-void Shoot::shootStateMachine() {
+void Shoot::errorControl()
+{
+        result=Trigger_Slow;//扳机慢速归位
+        speed_5065=SHOOT_SPEED_BACK; //摩擦归位     
+       
+    
+}
 
-     distance =3.5;
+
+void Shoot :: distanceGet(const NUC_t &nuc_v)
+{
+
+    test_distance=nuc_v.vision.x; // 获取自瞄数据
+
+}
+
+void Shoot::vesc_send()
+{
+    CAN_VESC_Control(1, speed_5065, &hcan2);
+}
+
+void Shoot::vofaWatch()
+{
+    vofa[0] = motor_5065[1]->rotor_speed;
+    vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
+    vofa[2] = speed_5065; // 发送电机速度数据
+    vofa[4] = trigger_Motor->speed_rpm; // 发送扳机电机速度数据
+    vofa_tx_main(vofa); // 发送数据到虚拟串口
+}
+
+
+void Shoot::shootStateMachine() {
+    
+     xxx=polynomial(test_distance);
+     distance =test_distance;
     switch (currentState) {
         case SHOOT_IDLE: {
             speed_5065=STOP;
-            result=STOP;
+            t_speedSet=STOP;
             // 空闲状态，等待遥控器输入
-            if((rc_ctrl.sw[0]==1800)) {
+            if((rc_ctrl.sw[1]==200)) {
-                currentState = SHOOT_FIRE; // 切换到发射状态
+                currentState = SHOOT_READY; // 切换到发射状态
             }
             break;
         }
 
-        case SHOOT_FIRE: {
+        case SHOOT_READY:
-            // 发射状态，控制电机发射
+        {
-          // speed_5065 = test_speed;
+           // 发射状态，控制电机发射
-            speed_5065 =distanceToSpeed(distance);
+           //speed_5065 = test_speed;
+            speed_5065 =polynomial(distance);
         if(motor_5065[0]->rotor_speed>=speed_5065-Error && motor_5065[0]->rotor_speed<=speed_5065+Error &&
 		   motor_5065[1]->rotor_speed>=speed_5065-Error && motor_5065[1]->rotor_speed<=speed_5065+Error)
 		{
-			result=Trigger_Torque;//恒力扳机
+             if((rc_ctrl.sw[3]==1800)) {
+                currentState = SHOOT_FIRE; // 切换到发射状态
+            }
+		
 		}
+        break;
 
+        }
+
+        case SHOOT_FIRE: {
+
+            t_speedSet=triggerspeed;// 扳机速度影响？
             // 检测光电传感器是否触发
             if (IS_PHOTOELECTRIC_TRIGGERED()) {
                 currentState = SHOOT_BACK; // 切换到光电触发状态
@@ -127,11 +139,13 @@ void Shoot::shootStateMachine() {
             break;
         }
 
+
+
         case SHOOT_BACK: {
             // 光电触发状态，停止发射
-             result=STOP;
+             t_speedSet=STOP;
              speed_5065=STOP;
-
+            
             // 切换到返回状态
             currentState = SHOOT_RETURN;
             break;
@@ -140,12 +154,12 @@ void Shoot::shootStateMachine() {
         case SHOOT_RETURN: {
             // 自动返回状态，控制电机反转
             speed_5065=SHOOT_SPEED_BACK;
-            result=-Trigger_Torque;
+            t_speedSet=2000;
 
             // 检测返回完成（可以通过时间或其他传感器判断）
-            if (rc_ctrl.sw[0]==1000) { // 假设遥控器开关控制返回完成
+            if (rc_ctrl.sw[1]==1000) { // 假设遥控器开关控制返回完成
                 speed_5065=SHOOT_SPEED_BACK;
-                result=STOP;
+                t_speedSet=STOP;
                 currentState = SHOOT_IDLE; // 切换回空闲状态
             }
             break;
@@ -157,21 +171,17 @@ void Shoot::shootStateMachine() {
             break;
         }
     }
-     CAN_VESC_Control(1,speed_5065,&hcan2);
+      result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet);
-     
+   //  CAN_VESC_Control(1,speed_5065,&hcan2);
-        vofa[0] = motor_5065[1]->rotor_speed;
+          
-        vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
-        vofa[2] = speed_5065; // 发送电机速度数据
-
-        vofa_tx_main(vofa); // 发送数据到虚拟串口
-     
 }
 
-//拟合函数
+//MATLAB拟合函数
-float distanceToSpeed(float x) {
+float polynomial(float x) {
-    return -2.3958f * x * x * x * x
+    return 6348.2377f * powf(x, 5.0f)
-           + 59.615f * x * x * x
+        + -128275.8296f * powf(x, 4.0f)
-           - 525.63f * x * x
+        + 1030651.0231f * powf(x, 3.0f)
-           + 2136.4f * x
+        + -4115199.9362f * powf(x, 2.0f)
-           + 28001.0f;
+        + 8167973.1254f * x
+        + -6420648.3555f;
 }

User/module/shoot.hpp

@@ -2,10 +2,12 @@
 #define SHOOT_HPP
 #include "djiMotor.h"
 #include "pid.h"
+#include "nuc.h"
 
 // 定义状态枚举
 typedef enum {
     SHOOT_IDLE,        // 空闲状态
+    SHOOT_READY,
     SHOOT_FIRE,        // 发射状态
     SHOOT_BACK,   // 光电触发状态
     SHOOT_RETURN       // 自动返回状态
@@ -28,16 +30,19 @@ class Shoot
 {
 public:
     Shoot();
-    void shootThree(void);
     void shootStop(void);
-    void shootBack(void);
     void shootStateMachine(void);
     void trigger_control(void);
+    void errorControl(void);
+    void vofaWatch(void);
+    void vesc_send(void);
+    void distanceGet(const NUC_t &nuc);
     float speed_5065;
     float speed_trigger;
     ShootState_t currentState;
 
     int16_t result;
+    float t_speedSet;
     //暂时放在公共,task里使用
      CAN_MotorFeedback_t *motor_5065[2];
     //==========================公共变量==========================
@@ -54,7 +59,7 @@ private:
     //电机速度pid结构体
     pid_type_def speed_pid;
     motor_measure_t *trigger_Motor;
-    float speedSet;
+
 
 
 };
@@ -62,4 +67,8 @@ private:
 
 float distanceToSpeed(float x);
 
+float polynomial(float x);
+
+float polynomial2(float x);
+
 #endif

User/task/ballTask.cpp

@@ -10,10 +10,10 @@ extern RC_ctrl_t rc_ctrl;
 Ball ball;
 //float vofa[8];
 
-// 左旋钮 sw[2] 左1800 右200
+
-// 右旋钮 sw[3] 左1800 右200
+// C键 sw[5]👆 1800   中1000  👇200
-// C键 sw[4]👆 200   中1000  👇1800
+// D键 sw[6]👆 200 外伸   👇1800 归位  //三摩擦架子
-// D键 sw[5]👆 1800   👇200
+// H键 sw[7]👆 200   👇1800  //取球
 
 int angle1=34;
 int angle2=23;
@@ -26,7 +26,7 @@ int speed_set=0;
 int speed_feedback=0;
 
 extern int speedm;
-extern int speedm1;
+
 
 void FunctionBall(void *argument)
 {
@@ -44,44 +44,47 @@ void FunctionBall(void *argument)
   abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
 
 
-   if(rc_ctrl.sw[2]>=1200)
+   if(rc_ctrl.sw[6] == 200)
     {
         angle1=75;
     }
-    if(rc_ctrl.sw[2]<1200)
+    if(rc_ctrl.sw[6]==1800)
     {
         angle1=0;
       
     }
+
      //运球
-        if(rc_ctrl.sw[3]>=1200)
+    if(rc_ctrl.sw[5]==200)
-        {
+    {
-					 a1=1;
+        //👇
-           //👇
+        ball.ballHadling(); // 处理摩擦轮转动
-           ball.ballHadling(); // 处理摩擦轮转动
+    }
-        }
      //下球     
-        if(rc_ctrl.sw[4]==200)
+    if(rc_ctrl.sw[5]==1800)
+    {
+        //👆
+        ball.ballDown();
+    }
+    //中间
+    if(rc_ctrl.sw[7]==200)
+    {
+         if(rc_ctrl.sw[5]==1000)
         {
-            //👆
+            //中
-            ball.ballDown();
-        }
-     //回弹停止
-        if(rc_ctrl.sw[3]<1000)
-        {
-            //👆
             ball.ballStop();
+            ball.ballHadling(); // 处理摩擦轮转动
         }
-        if(rc_ctrl.sw[7]==1800)
+    }
-        {
-            ball.ballTake();
 
-        }
+    if(rc_ctrl.sw[7]==1800)
+    {
+        //ball.ballTake();
 
-    
+    }
 
+	// CAN_XiaoMi(1,&ball.JZ_xiaomi,&hcan2);
 
-    ball.Extend_control(angle1); 
     ball.Spin(speedm);
     ball.Send_control();
 

User/task/nucTask.cpp

@@ -22,7 +22,6 @@ void Function_nuc(void *argument)
 
     NUC_Init(&cmd_fromnuc);
     
-
    uint32_t tick = osKernelGetTickCount();
 
     while(1)

User/task/shootTask.cpp

@@ -15,10 +15,8 @@ int shoot_flag = 0;
 
 int a2;
 
-// sw[0]2 下306上1694 sw[5]3前306后1694 sw[4]4前1694后306 sw[1]xuan1 sw[3]xuan2 
+//E键 sw[0]👆 200  中 1000 👇1800
-
+//F键 sw[1]👆 1800   👇200
-//F键 sw[0]👆 1800  中 1000 👇200
-//E键 sw[1]👆 1800   👇200
 
 void FunctionShoot(void *argument)
 {
@@ -40,35 +38,46 @@ while(1)
 	if(shoot.flag_thread & BALL_OK)
 	{
 		a2=2;
-	//  shoot.shootThree();
 	}
 
+
 	shoot_flag=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
 
 	if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
 	{
-		// 使用接收到的视觉数据调整云台
+		shoot.distanceGet(nucData);
-		//gimbal.gimbalVision(nucData);
 	}
-	if(rc_ctrl.sw[1]>1000)
+
+
+	if(rc_ctrl.sw[0]>1000)
 	{
-		shoot.shootStateMachine();
+
-		if(rc_ctrl.sw[0]==200)
+		if(rc_ctrl.sw[1]==1800)
 		{
-			shoot.shootBack();
+			shoot.errorControl();
 		}
-		if(rc_ctrl.sw[0]==1000)
+		if(rc_ctrl.sw[1]==1000)
 		{
 			shoot.shootStop();
+			shoot.shootStateMachine();//shoot
+		}
+		if(rc_ctrl.sw[1]==200)
+		{
+			shoot.shootStateMachine();//shoot
 		}
 	}
-	if(rc_ctrl.sw[1]==200)
+	if(rc_ctrl.sw[0]==200)
 	{
 		shoot.shootStop();
 
 	}
 
-	 shoot.trigger_control();
+	shoot.vesc_send(); // 发送电机速度数据
+
+	shoot.trigger_control();
+
+	shoot.vofaWatch();
+
 	osDelay(2);
 
 	tick += delay_tick; /* 计算下一个唤醒时刻 */