完结撒花

Core/Src/gpio.c

@@ -64,7 +64,7 @@ void MX_GPIO_Init(void)
   /*Configure GPIO pin : BALL_Pin */
   GPIO_InitStruct.Pin = BALL_Pin;
   GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
-  GPIO_InitStruct.Pull = GPIO_PULLUP;
+  GPIO_InitStruct.Pull = GPIO_PULLDOWN;
   HAL_GPIO_Init(BALL_GPIO_Port, &GPIO_InitStruct);
 
   /*Configure GPIO pin : up_ball_Pin */

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

@@ -74,3 +74,93 @@
 
 [info] Log at : 2025/6/25|19:23:57|GMT+0800
 
+[info] Log at : 2025/6/27|11:41:11|GMT+0800
+
+[info] Log at : 2025/6/27|15:32:00|GMT+0800
+
+[info] Log at : 2025/6/27|18:43:18|GMT+0800
+
+[info] Log at : 2025/6/28|01:19:54|GMT+0800
+
+[info] Log at : 2025/6/28|14:29:33|GMT+0800
+
+[info] Log at : 2025/6/28|16:39:11|GMT+0800
+
+[info] Log at : 2025/6/28|16:39:57|GMT+0800
+
+[info] Log at : 2025/6/28|18:37:20|GMT+0800
+
+[info] Log at : 2025/6/29|15:33:54|GMT+0800
+
+[info] Log at : 2025/6/30|10:24:59|GMT+0800
+
+[info] Log at : 2025/7/1|10:57:51|GMT+0800
+
+[info] Log at : 2025/7/2|02:59:15|GMT+0800
+
+[info] Log at : 2025/7/2|17:15:33|GMT+0800
+
+[info] Log at : 2025/7/4|09:10:46|GMT+0800
+
+[info] Log at : 2025/7/6|22:01:26|GMT+0800
+
+[info] Log at : 2025/7/6|22:02:13|GMT+0800
+
+[info] Log at : 2025/7/6|23:12:04|GMT+0800
+
+[info] Log at : 2025/7/7|04:53:38|GMT+0800
+
+[info] Log at : 2025/7/7|08:44:07|GMT+0800
+
+[info] Log at : 2025/7/7|14:20:11|GMT+0800
+
+[info] Log at : 2025/7/8|02:49:12|GMT+0800
+
+[info] Log at : 2025/7/8|13:15:42|GMT+0800
+
+[info] Log at : 2025/7/8|18:00:22|GMT+0800
+
+[info] Log at : 2025/7/8|20:39:07|GMT+0800
+
+[info] Log at : 2025/7/9|08:39:28|GMT+0800
+
+[info] Log at : 2025/7/9|11:36:02|GMT+0800
+
+[info] Log at : 2025/7/9|16:01:34|GMT+0800
+
+[info] Log at : 2025/7/9|23:48:02|GMT+0800
+
+[info] Log at : 2025/7/10|01:59:23|GMT+0800
+
+[info] Log at : 2025/7/10|15:33:45|GMT+0800
+
+[info] Log at : 2025/7/10|17:26:59|GMT+0800
+
+[info] Log at : 2025/7/10|22:12:10|GMT+0800
+
+[info] Log at : 2025/7/11|10:11:25|GMT+0800
+
+[info] Log at : 2025/7/11|17:39:11|GMT+0800
+
+[info] Log at : 2025/7/12|23:45:28|GMT+0800
+
+[info] Log at : 2025/7/13|11:08:55|GMT+0800
+
+[info] Log at : 2025/7/13|12:29:41|GMT+0800
+
+[info] Log at : 2025/7/13|16:33:17|GMT+0800
+
+[info] Log at : 2025/7/13|16:42:25|GMT+0800
+
+[info] Log at : 2025/7/13|22:38:15|GMT+0800
+
+[info] Log at : 2025/7/14|07:52:29|GMT+0800
+
+[info] Log at : 2025/7/14|12:31:22|GMT+0800
+
+[info] Log at : 2025/7/14|13:56:03|GMT+0800
+
+[info] Log at : 2025/7/16|22:15:00|GMT+0800
+
+[info] Log at : 2025/7/16|22:26:06|GMT+0800
+

MDK-ARM/.vscode/settings.json

@@ -5,6 +5,7 @@
         "buzzer.h": "c",
         "calc_lib.h": "c",
         "usertask.h": "c",
-        "detect.h": "c"
+        "detect.h": "c",
+        "nuc.h": "c"
     }
 }

MDK-ARM/.vscode/uv4.log

@@ -1,10 +1,9 @@
 *** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
 Build target 'R1'
+compiling ballTask.cpp...
 compiling shootTask.cpp...
-compiling shoot.cpp...
-compiling ball.cpp...
 linking...
-Program Size: Code=31644 RO-data=1832 RW-data=268 ZI-data=32252  
+Program Size: Code=31984 RO-data=1832 RW-data=272 ZI-data=32264  
 FromELF: creating hex file...
 "R1\R1.axf" - 0 Error(s), 0 Warning(s).
-Build Time Elapsed:  00:00:06
+Build Time Elapsed:  00:00:05

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

@@ -1 +1 @@
-2025/6/26 3:22:31
+2025/7/16 22:30:59

MDK-ARM/R1.uvoptx

@@ -154,11 +154,15 @@
         </SetRegEntry>
       </TargetDriverDllRegistry>
       <Breakpoint/>
+<<<<<<< HEAD
+<<<<<<< HEAD
+=======
+>>>>>>> 上层测试
       <WatchWindow1>
         <Ww>
           <count>0</count>
           <WinNumber>1</WinNumber>
-          <ItemText>rc_ctrl</ItemText>
+          <ItemText>rc_ctrl,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>1</count>
@@ -168,24 +172,58 @@
         <Ww>
           <count>2</count>
           <WinNumber>1</WinNumber>
+<<<<<<< HEAD
           <ItemText>ball,0x0A</ItemText>
+=======
+          <ItemText>ball</ItemText>
+>>>>>>> 上层测试
         </Ww>
         <Ww>
           <count>3</count>
           <WinNumber>1</WinNumber>
-          <ItemText>ball_exit,0x0A</ItemText>
+<<<<<<< HEAD
+          <ItemText>nucbuf</ItemText>
+=======
+          <ItemText>and1</ItemText>
+>>>>>>> 上层测试
         </Ww>
         <Ww>
           <count>4</count>
           <WinNumber>1</WinNumber>
-          <ItemText>nucbuf</ItemText>
+<<<<<<< HEAD
+          <ItemText>nuc_v</ItemText>
+=======
+          <ItemText>and1</ItemText>
+>>>>>>> 上层测试
         </Ww>
         <Ww>
           <count>5</count>
           <WinNumber>1</WinNumber>
-          <ItemText>nuc_v</ItemText>
+<<<<<<< HEAD
+          <ItemText>abc,0x0A</ItemText>
+=======
+          <ItemText>nucbuf</ItemText>
+>>>>>>> 上层测试
+        </Ww>
+        <Ww>
+          <count>6</count>
+          <WinNumber>1</WinNumber>
+<<<<<<< HEAD
+          <ItemText>shoot_wait,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>7</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>error_code</ItemText>
         </Ww>
       </WatchWindow1>
+=======
+>>>>>>> 上层测试
+=======
+          <ItemText>drop_message,0x0A</ItemText>
+        </Ww>
+      </WatchWindow1>
+>>>>>>> 上层测试
       <MemoryWindow4>
         <Mm>
           <WinNumber>4</WinNumber>

MDK-ARM/README.md

@@ -36,8 +36,29 @@ r1上层
   + 拨置👇发射清空掉落信号
 
 + 用一个攻守方档
+  + 初始移动到最上面 更待蓄力(不管攻方守方都在最上面等待)
   + 攻方时拨下立马蓄力并伸出(可小角度)
   + 守方时不动并保持缩回
   + 👇  运球档正常运球
   + 中  初始档直接缩回
   + 👆 配合档 完成配合并伸出才能发射
+
++ 传球模式
+  + 自动
+    + 底盘的传球对准档拨下
+    + 我的蓄力进入传球拟合
+    + 继续拨下发射
+  + 手动
+    + 目前只能打固定距离
+      + 切相同传球档 自动蓄力到传球力度
+      + 发射档发射
+    + 图传多距离
+      + 传球档
+      + 旋钮+看图传点位调整
+
++ 修复
+  + 6.29 发射误操作导致没有拟合作用就射了(已修复)
+  + 6.29 串口不稳定 重新拔插一下
+  + 6.29 nuc位置更新慢
+  + 6.29 添加光电上电保护防止跳尺(已添加)
+  

README.md

@@ -5,19 +5,60 @@ r1上层
 ## 外设
 
 + CAN1
-  - 扳机2006 id:0x205
-  - 三摩擦 id:123 
+  + 扳机2006 id:0x201
++ CAN2
+  + 小米电机 id:1  
 + UART
-  - uart1 波特率4000000 id：2
-  - uart6 nuc
-  - uart3 遥控器接收
+  + uart1 波特率4000000 id：2
+  + uart6 nuc
+  + uart3 遥控器接收
 + GPIO
-  - PI6运球光电 
-  - PE11 运球气缸
-
-
+  + PI6运球光电
+  + PE13 爪气缸
+  + PE14 砸气缸
 
 ## 遥控器
 
+## 待解决
 
++ 用了将运球和伸缩绑定到一起  √
++ 串口收数加个滤波  √
 
+## 思路
+
++ 👆 传球档           👆  配合档  
++ 中  初始档           中  初始档
++ 👇  发射档          👇  运球档
++ 起步遥控档我直接蓄力准备接球  + 可多次的运球
+  + 缩回转移球
+  + 蓄力到位,收到掉落信号和已伸出信号
+  + 根据视觉拟合信息的动态调整
+  + 拨置👇发射清空掉落信号
+
++ 用一个攻守方档
+  + 初始移动到最上面 更待蓄力(不管攻方守方都在最上面等待)
+  + 攻方时拨下立马蓄力并伸出(可小角度)
+  + 守方时不动并保持缩回
+  + 👇  运球档正常运球
+  + 中  初始档直接缩回
+  + 👆 配合档 完成配合并伸出才能发射
+
++ 传球模式
+  + 自动
+    + 底盘的传球对准档拨下
+    + 我的蓄力进入传球拟合
+    + 继续拨下发射
+  + 手动
+    + 目前只能打固定距离
+      + 切相同传球档 自动蓄力到传球力度
+      + 发射档发射
+    + 图传多距离
+      + 传球档
+      + 旋钮+看图传点位调整
+
++ 修复
+  + 6.29 发射误操作导致没有拟合作用就射了(已修复)
+  + 6.29 串口不稳定 重新拔插一下
+  + 6.29 nuc位置更新慢
+  + 6.29 添加光电上电保护防止跳尺(已添加)
+  

User/bsp/TopDefine.h

@@ -18,9 +18,6 @@
 
 #define ONE_CONTROL 1
 
-
-
-
 //是否使用大疆DT7遥控器
 #ifndef DT7
 #define DT7 0
@@ -31,7 +28,8 @@
 //================任务通知,时间组================//
 //事件组
 #define EVENT_RC    (1<<1)
-#define EVENT_CAN   (1<<2)
+#define EVENT_CAN1  (1<<2)
+#define EVENT_CAN2  (1<<3)
 
 //================任务通知================//
 //运球
@@ -43,6 +41,10 @@
 #define HANDING_FINISH (1<<2)
 //伸缩结束
 #define EXTEND_OK	(1<<3)
+//等待ok
+#define WAIT_OK		(1<<4)
+//可以防守收回
+#define DEF_READY	(1<<5)
 
 //要发送ok了
 #define BALL_SEND (1<<6)

User/bsp/gpio_it.c

@@ -20,53 +20,53 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
 }
 
 /* Exported functions ------------------------------------------------------- */
-int8_t BSP_GPIO_RegisterCallback(uint16_t pin, void (*callback)(void)) {
-  if (callback == NULL) return BSP_ERR_NULL;
+// int8_t BSP_GPIO_RegisterCallback(uint16_t pin, void (*callback)(void)) {
+//   if (callback == NULL) return BSP_ERR_NULL;
 
-  for (uint8_t i = 0; i < 16; i++) {
-    if (pin & (1 << i)) {
-      GPIO_Callback[i] = callback;
-      break;
-    }
-  }
-  return BSP_OK;
-}
+//   for (uint8_t i = 0; i < 16; i++) {
+//     if (pin & (1 << i)) {
+//       GPIO_Callback[i] = callback;
+//       break;
+//     }
+//   }
+//   return BSP_OK;
+// }
 
-int8_t BSP_GPIO_EnableIRQ(uint16_t pin) {
-  switch (pin) {
+// int8_t BSP_GPIO_EnableIRQ(uint16_t pin) {
+//   switch (pin) {
 
-    // case USER_KEY_Pin:
-    //   HAL_NVIC_EnableIRQ(USER_KEY_EXTI_IRQn);
-    //   break;
+//     case KEY_Pin:
+//       HAL_NVIC_EnableIRQ(KEY_Pin);
+//       break;
 
-      /*
-      case XXX_Pin:
-              HAL_NVIC_EnableIRQ(XXX_IRQn);
-              break;
-      */
+//       /*
+//       case XXX_Pin:
+//               HAL_NVIC_EnableIRQ(XXX_IRQn);
+//               break;
+//       */
 
-    default:
-      return BSP_ERR;
-  }
-  return BSP_OK;
-}
+//     default:
+//       return BSP_ERR;
+//   }
+//   return BSP_OK;
+// }
 
-int8_t BSP_GPIO_DisableIRQ(uint16_t pin) {
-  switch (pin) {
+// int8_t BSP_GPIO_DisableIRQ(uint16_t pin) {
+//   switch (pin) {
 
-    // case USER_KEY_Pin:
-    //   HAL_NVIC_DisableIRQ(USER_KEY_EXTI_IRQn);
-    //   break;
+//     case KEY_Pin:
+//       HAL_NVIC_DisableIRQ(KEY_IRQn);
+//       break;
 
-      /*
-      case XXX_Pin:
-              HAL_NVIC_EnableIRQ(XXX_IRQn);
-              break;
-      */
+//       /*
+//       case XXX_Pin:
+//               HAL_NVIC_EnableIRQ(XXX_IRQn);
+//               break;
+//       */
 
-    default:
-      return BSP_ERR;
-  }
-  return BSP_OK;
-}
+//     default:
+//       return BSP_ERR;
+//   }
+//   return BSP_OK;
+// }
 

User/device/detect.c

@@ -24,15 +24,8 @@ void detect_ball(void)
 void detect_init(void)
 {
 
-    BSP_GPIO_RegisterCallback(KEY_Pin, detect_exit);
-    BSP_GPIO_RegisterCallback(BALL_Pin, detect_ball);
+    // BSP_GPIO_RegisterCallback(KEY_Pin, detect_exit);
+    // BSP_GPIO_RegisterCallback(BALL_Pin, detect_ball);
 
-    // 启用按键中断
-    if (BSP_GPIO_EnableIRQ(KEY_Pin) != BSP_OK) {
-        // 错误处理
-    }  
-    if (BSP_GPIO_EnableIRQ(BALL_Pin) != BSP_OK) {
-        // 错误处理
-    }  
     
 }

User/device/djiMotor.c

@@ -215,15 +215,13 @@ static osThreadId_t thread_alert;
 void Dji_Motor_CB()
 {
   HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
+  osThreadFlagsSet(thread_alert, EVENT_CAN1);
 
-  osThreadFlagsSet(thread_alert, EVENT_CAN);
- // osEventFlagsSet(eventReceive, EVENT_CAN);
 }
 void can2_Motor_CB(void)
 {
   HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO1, &rx_header, rx_data);
-
-  //osThreadFlagsSet(thread_alert, EVENT_CAN);
+  osThreadFlagsSet(thread_alert, EVENT_CAN2);
 }
 
 /**
@@ -234,9 +232,6 @@ void can2_Motor_CB(void)
 void djiInit(void)
 {
   thread_alert = osThreadGetId();
-
-  
-
   BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
                           Dji_Motor_CB);
   BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB,
@@ -250,11 +245,9 @@ void djiInit(void)
 */
 uint32_t waitNewDji() 
 {
-  // return osEventFlagsWait(
-	// 			eventReceive, EVENT_CAN,osFlagsWaitAny, osWaitForever);
+  // 等待CAN1或CAN2任意一个事件
   return osThreadFlagsWait(
-    EVENT_CAN, osFlagsWaitAll, osWaitForever);
-
+    EVENT_CAN1 | EVENT_CAN2, osFlagsWaitAny, osWaitForever);
 }
 #endif
 

User/device/nuc.c

@@ -4,25 +4,38 @@
 
 static osThreadId_t thread_alert;
 
-uint8_t nucbuf[32];
+volatile uint32_t drop_message = 0;
+
+uint8_t nucbuf[18];
 uint8_t packet[32]; // 假设最大数据包长度为 32 字节
 
-static void NUC_IdleCallback(void) {	
-	osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
-
+static void NUC_CBLTCallback(void)
+{
+  osThreadFlagsSet(thread_alert, SIGNAL_NUC_RAW_REDY);
+}
+uint32_t error_code;
+static void NUC_ERRORCALLBACK(void)
+{
+   NUC_Restart();
+   error_code = HAL_UART_GetError(BSP_UART_GetHandle(BSP_UART_AI));
+ 
 }
 
-int8_t NUC_Init(NUC_t *nuc){
-    if(nuc == NULL) return DEVICE_ERR_NULL;
-    if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
-   BSP_UART_RegisterCallback(BSP_UART_AI,BSP_UART_IDLE_LINE_CB,
-                            NUC_IdleCallback);
-	
-   return DEVICE_OK;
+int8_t NUC_Init(NUC_t *nuc)
+{
+  if (nuc == NULL) 
+    return DEVICE_ERR_NULL;
+  if ((thread_alert = osThreadGetId()) == NULL)
+    return DEVICE_ERR_NULL;
+  BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
+                            NUC_CBLTCallback);
+  BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_ERROR_CB,
+                            NUC_ERRORCALLBACK);
+  return DEVICE_OK;
 }
 
 int8_t NUC_StartReceiving(void) {
-  if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_AI),
+  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI),
                            (uint8_t *)nucbuf,
                            sizeof(nucbuf)) == HAL_OK)
     return DEVICE_OK;
@@ -37,7 +50,7 @@ int8_t NUC_Restart(void) {
 
 
 bool_t NUC_WaitDmaCplt(void) {
-  return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) ==
+  return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,20) ==
           SIGNAL_NUC_RAW_REDY);
 
 }
@@ -68,7 +81,63 @@ int8_t NUC_SendPacket(void *data, uint16_t length) {
 
   return DEVICE_OK; // 发送成功
 }
-int wzcsb=0;
+
+// 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) goto error;  //发送ID不是底盘
+//   else
+//   {
+//     n->status_fromnuc = nucbuf[1];
+//     n->ctrl_status = nucbuf[2];
+//  switch (n->status_fromnuc) {
+//       case VISION:
+//           /* 协议格式 
+//              0xFF  HEAD
+//              0x07
+//                控制帧
+//              0x01  相机帧
+//              x    fp32
+//              0xFE  TAIL
+//           */
+//       if (nucbuf[7] != TAIL) goto error;
+
+//       instance.data[3] = nucbuf[6];
+//       instance.data[2] = nucbuf[5];
+//       instance.data[1] = nucbuf[4];
+//       instance.data[0] = nucbuf[3];
+//       n->vision.x = instance.x[0];
+
+//       instance.data[7] = nucbuf[10];
+//       instance.data[6] = nucbuf[9];
+//       instance.data[5] = nucbuf[8];
+//       instance.data[4] = nucbuf[7];
+//       n->vision.y = instance.x[1];
+
+
+//           break;
+//   }
+//    return DEVICE_OK;
+//   }
+
+
+// error:
+//   drop_message++;
+//   return DEVICE_ERR;
+// }
+
+
+  /* 协议格式 
+      0xFF  HEAD
+      x    fp32
+      y    fp32
+      0xFE  TAIL
+  */
 int8_t NUC_RawParse(NUC_t *n) {
   if (n == NULL) return DEVICE_ERR_NULL;
   union {
@@ -80,47 +149,26 @@ int8_t NUC_RawParse(NUC_t *n) {
   if(nucbuf[0]!=HEAD) goto error;  //发送ID不是底盘
   else
   {
-    n->status_fromnuc = nucbuf[1];
-    n->ctrl_status = nucbuf[2];
- switch (n->status_fromnuc) {
-      case VISION:
-          /* 协议格式 
-             0xFF  HEAD
-             0x09
-               控制帧
-             0x01  相机帧
-             x    fp32
-             y    fp32
-             z    fp32
-             0xFE  TAIL
-          */
-      if (nucbuf[7] != TAIL) goto error;
 
-      instance.data[3] = nucbuf[6];
-      instance.data[2] = nucbuf[5];
-      instance.data[1] = nucbuf[4];
-      instance.data[0] = nucbuf[3];
+      if (nucbuf[17] != TAIL) goto error;
+
+      instance.data[3] = nucbuf[8];
+      instance.data[2] = nucbuf[7];
+      instance.data[1] = nucbuf[6];
+      instance.data[0] = nucbuf[5];
       n->vision.x = instance.x[0];
 
-      instance.data[7] = nucbuf[10];
-      instance.data[6] = nucbuf[9];
-      instance.data[5] = nucbuf[8];
-      instance.data[4] = nucbuf[7];
+      instance.data[7] = nucbuf[16];
+      instance.data[6] = nucbuf[15];
+      instance.data[5] = nucbuf[14];
+      instance.data[4] = nucbuf[13];
       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;
-  }
-
-
+  
 error:
-  wzcsb++;
+  drop_message++;
   return DEVICE_ERR;
 }
 
@@ -198,3 +246,7 @@ int8_t NUC_HandleOffline(NUC_t *cmd)
 	 memset(cmd, 0, sizeof(*cmd));
 	return 0;
 }
+
+
+
+

User/device/nuc.h

@@ -8,6 +8,7 @@ extern "C" {
 #include "struct_typedef.h"
 #include "device.h"
 #include "uart_it.h"
+#include "main.h"
 
 // 帧头针尾
 #define HEAD (0xFF)
@@ -15,7 +16,7 @@ extern "C" {
 
 #define ODOM (0x04)
 #define PICK (0x06)
-#define VISION (0x09)
+#define VISION (0x07)
 #define SEND (0X07)
 
 // 写结构体存入视觉信息

User/lib/calc_lib.c

@@ -164,6 +164,8 @@ fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max)
         *num = Limit_min;
         return Limit_min;
     }
-  
-}
+
+     return *num; // 如果在范围内，直接返回原值
+}
+  
 

User/module/ball.cpp

@@ -9,107 +9,105 @@
 extern RC_ctrl_t rc_ctrl;
 extern int ball_exit;
 
-//伸缩
-#define I_ANGLE 147
-#define O_ANGLE 187
-#define WAIT_POS 165
+// 外死点168  外163 中150  内127  限位124.8
+// 伸缩
+//外死点89  外85 中75  内49  限位46 
 
-//PE11 气缸
+#define I_ANGLE 49
+#define O_ANGLE 85
+#define WAIT_POS 75
+#define HANGDING_POS 89
+
+// PE11 气缸git stash apply
 
 Ball ::Ball()
-{  
-   detect_init();
+{
+    // 小米电机
+    feedback = get_CyberGear_point();
 
-    //小米电机
-    feedback=get_CyberGear_point();
-
-    //小米电机初始化
-    xiaomi.position = I_ANGLE;    // 
-    xiaomi.speed    = 25;   // 
-    xiaomi.K_P      = 80;  // 位置增益
-    xiaomi.K_D      =20;    // 位置阻尼
-    xiaomi.K_C      = 12 ;  // 力矩
-    xiaomi.Pmax     =1; //好像没啥用
+    // 小米电机初始化
+    xiaomi.position = I_ANGLE; //
+    xiaomi.speed = 35;         //
+    xiaomi.K_P = 40;           // 位置增益
+    xiaomi.K_D = 100;           // 位置阻尼
+    xiaomi.K_C = 12;           // 力矩
+    xiaomi.Pmax = 1;           // 好像没啥用
 
     // //小米电机初始化
-    // xiaomi.position = 0;    // 
-    // xiaomi.speed    = 0;   // 
-    // xiaomi.K_P      = 0;  // 
-    // xiaomi.K_D      =0;    // 
-    // xiaomi.K_C      = 0 ;  
+    // xiaomi.position = 0;    //
+    // xiaomi.speed    = 0;   //
+    // xiaomi.K_P      = 0;  //
+    // xiaomi.K_D      =0;    //
+    // xiaomi.K_C      = 0 ;
     // xiaomi.Pmax     =0;
 
-    //状态机状态初始化
-    currentState1= BALL_IDLE;
-
+    // 状态机状态初始化
+    currentState1 = BALL_IDLE;
 }
 
-
-// E键 sw[1]  👆 200 shoot  中 1000 stop sw[2]👇1800 
+// E键 sw[1]  👆 200 shoot  中 1000 stop sw[2]👇1800
 // G键 sw[6]👆 200  中 1000 👇1800
 //     sw[5] 👆 200 👇1800
-//左旋 sw[7] 200 --1800
+// 左旋 sw[7] 200 --1800
 void Ball::rc_mode()
 {
-    if(rc_ctrl.sw[6]==200)
+    if (rc_ctrl.sw[6] == 200)
     {
-        rc_key=UP2;
+        rc_key = UP2;
     }
-     if(rc_ctrl.sw[6]==1000)
+    if (rc_ctrl.sw[6] == 1000)
     {
-        rc_key=MIDDLE2;
+        rc_key = MIDDLE2;
     }
-     if(rc_ctrl.sw[6]==1800)
+    if (rc_ctrl.sw[6] == 1800)
     {
-        rc_key=DOWN2;
+        rc_key = DOWN2;
     }
 
-    if(rc_ctrl.sw[5]==200)
+    if (rc_ctrl.sw[5] == 200)
     {
-      extern_key=IN;  
+        extern_key = IN;
     }
-    if(rc_ctrl.sw[5]==1800)
+    if (rc_ctrl.sw[5] == 1800)
     {
-      extern_key=OUT;  
+        extern_key = OUT;
     }
 
-    if(rc_ctrl.sw[5]==1800)
+    if (rc_ctrl.sw[5] == 1800)
     {
-       ready_key=SIDE; 
+        ready_key = SIDE;
     }
-    if(rc_ctrl.sw[5]==200)
+    if (rc_ctrl.sw[5] == 200)
     {
-         ready_key=0; //默认不准备
+        ready_key = DEF; // 默认不准备
     }
 }
 
 void Ball::Send_control()
 {
-  
-    CAN_XiaoMi(1,&xiaomi,&hcan2);
-    osDelay(1);
 
+    CAN_XiaoMi(1, &xiaomi, &hcan2);
+    osDelay(1);
 }
 
-#if ONE_CONTROL==0
+#if ONE_CONTROL == 0
 
 void Ball::ballDown(void)
 {
 
     HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
     HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
-
 }
 
 void Ball::Move_Extend()
 {
-    if(extern_key==IN)
+    if (extern_key == IN)
     {
         xiaomi.position = I_ANGLE;
     }
-    if(extern_key==OUT)
+    if (extern_key == OUT)
     {
-        xiaomi.position = O_ANGLE;
+        xiaomi.position = HANGDING_POS;
     }
 }
 
@@ -119,64 +117,60 @@ void Ball::ball_control()
 
     Move_Extend();
 
-    switch (rc_key){
-        case MIDDLE2:
-       HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); //确保爪气缸关闭 
-       HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭   
+    switch (rc_key)
+    {
+    case MIDDLE2:
+        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
+        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
         if (currentState1 == BALL_FINISH)
         {
             currentState1 = BALL_IDLE;
         }
-        else {
-             currentState1 = BALL_IDLE; // 默认回到空闲状态
+        else
+        {
+            currentState1 = BALL_IDLE; // 默认回到空闲状态
         }
         break;
 
-       case UP2: 
+    case UP2:
         ballDown();
         break;
 
-        case DOWN2: 
+    case DOWN2:
         ballHadling();
-        //test_Handling();
 
         break;
-
     }
 
-    Send_control(); 
-
+    Send_control();
 }
 
-
- int ball_state = 0;
- int last_ball_state = 0; // 上一次的光电状态
+int ball_state = 0;
+int last_ball_state = 0; // 上一次的光电状态
 
 void Ball::ballHadling(void)
 {
-  
+
     switch (currentState1)
     {
     case BALL_IDLE:
-       HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); //确保爪气缸关闭 
-       HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭 
-        if (rc_key == DOWN2) // 检测按键是否被按下
+        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
+        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
+        if (rc_key == DOWN2)                                           // 检测按键是否被按下
         {
-          
-         
-            currentState1 = BALL_FORWARD; 
+            currentState1 = BALL_FORWARD;
         }
         break;
 
     case BALL_FORWARD:
         HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
-        osDelay(5); 
+        osDelay(5);
         HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
-        currentState1 = BALL_DROP; // 切换到球下落状态
+        currentState1 = BALL_DROP;                                 // 切换到球下落状态
         break;
 
-        case BALL_DROP:
-        osDelay(100); // 延时 100ms
+    case BALL_DROP:
+        osDelay(100);                                                // 延时 100ms
         HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
         // 一直检测有球（ball_state == 0），等球离开
         if (ball_state == 1 && last_ball_state == 0) // 球离开
@@ -186,7 +180,7 @@ void Ball::ballHadling(void)
         last_ball_state = ball_state;
         break;
 
-        case BALL_FLAG:
+    case BALL_FLAG:
         osDelay(10); // 延时 50ms
         // 等待球弹回再次检测到球
         if (ball_state == 0 && last_ball_state == 1) // 球弹回
@@ -197,18 +191,17 @@ void Ball::ballHadling(void)
         break;
 
     case BALL_CLOSE:
-
-            osDelay(25); 
-            HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
-            currentState1 = BALL_FINISH; // 切换到反转状态
+        osDelay(100);
+        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
+        currentState1 = BALL_FINISH;                                   // 切换到反转状态
         break;
 
     case BALL_FINISH:
-        osDelay(50); // 延时 50ms
+        osDelay(50);                                                   // 延时 50ms
         HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
-        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
+        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
 
-        //currentState1 = BALL_IDLE; // 回到空闲状态
+        // currentState1 = BALL_IDLE; // 回到空闲状态
 
         break;
 
@@ -216,11 +209,7 @@ void Ball::ballHadling(void)
         currentState1 = BALL_IDLE; // 默认回到空闲状态
         break;
     }
-
-    
- }   
-
-
+}
 
 #endif
 
@@ -228,156 +217,201 @@ void Ball::ballHadling(void)
 
 void Ball::ball_control()
 {
-    hand_thread = osThreadFlagsGet(); // 获取任务通知标志位
-    ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 1，无球 0)
-
-    //进攻
-    if(ready_key==SIDE)
-    {
-            switch (rc_key){
-        case MIDDLE2:
-         Idle_control();
-        break;
-
-       case UP2: 
-        ballDown();
-        break;
-
-       case DOWN2: 
-         ballHadling();
-        break;
-
-    }
-    }
-    //防守
-    else
-    {
-      xiaomi.position = I_ANGLE; // 保持收回
-      HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭 
-      HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭 
-
-    }
+    hand_thread = osThreadFlagsGet();                              // 获取任务通知标志位
+    ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 0，无球 1)
     
 
+    // 进攻
+    if (ready_key == SIDE)
+    {
+        osThreadFlagsClear(DEF_READY);
+        switch (rc_key)
+        {
+        case MIDDLE2:
+            Idle_control();
+            break;
 
-    Send_control(); 
+        case UP2:
+            ballDown();
+            break;
+
+        case DOWN2:
+            ballHadling();
+            break;
+        }
+
+        Send_control();
+    }
+    // 防守
+    else if(ready_key == DEF)
+    {
+        if(hand_thread & DEF_READY)
+        {
+         xiaomi.position = I_ANGLE; 
+
+        }
+                                    // 保持收回
+        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
+        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
+
+        osThreadFlagsClear(EXTEND_OK);
+        osThreadFlagsClear(READY_TELL); // 蓄力标志位
+        osThreadFlagsClear(HANDING_FINISH);
+
+        haveball=0;//变为空球状态
+
+        currentState1 = BALL_IDLE;
+
+        Send_control();
+    }
 
 }
 
-
-//任务通知来作全过程
+// 任务通知来作全过程
 
 void Ball::ballDown(void)
 {
-    switch(currentState1)
+     osThreadFlagsClear(HANDING_FINISH);
+    switch (currentState1)
     {
-        case BALL_IDLE:
-            xiaomi.position = I_ANGLE; // 保持收回
-            if(feedback->position_deg >= I_ANGLE-0.8 && feedback->position_deg <= I_ANGLE+0.8)
-            {
-                currentState1 = EXTEND_DOWN;
-            }
-            break;
+    case BALL_IDLE:
+        xiaomi.position = I_ANGLE; // 保持收回
+        if (feedback->position_deg >= I_ANGLE - 1 && feedback->position_deg <= I_ANGLE + 1)
+        {
+            currentState1 = EXTEND_DOWN;
+        }
+        break;
 
-        case EXTEND_DOWN:
-            HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);
-            HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
-            // 检测到球自由下落一次就切换状态
-            if(ball_state == 1)
-            {
-                currentState1 = EXTEND_OUT;
-            }
-            break;
+    case EXTEND_DOWN:
+        osDelay(500); // 不放太快
+        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);
+        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
+        // 检测到球自由下落一次就切换状态
+        if (ball_state == 1)
+        {
+            currentState1 = EXTEND_OUT;
+        }
+        break;
 
-        case EXTEND_OUT:
-            xiaomi.position = O_ANGLE; // 保持伸出
-            if(feedback->position_deg >= O_ANGLE-0.2 && feedback->position_deg <= O_ANGLE+0.2)
-            {
-                osThreadFlagsSet(task_struct.thread.shoot, EXTEND_OK);
-                currentState1 = EXTEND_FINISH; // 保持伸出，直到拨杆复位
-            }
-            break;
+    case EXTEND_OUT:
+        xiaomi.position = O_ANGLE; // 保持伸出
+        if (feedback->position_deg >= O_ANGLE - 1 && feedback->position_deg <= O_ANGLE + 1)
+        {
 
-        case EXTEND_FINISH:
-            xiaomi.position = O_ANGLE; // 一直保持伸出
-            // 等待拨杆复位（如切到MIDDLE2），Idle_control会负责回位
-            break;
+            osThreadFlagsSet(task_struct.thread.shoot, EXTEND_OK);
+            currentState1 = EXTEND_FINISH; // 保持伸出，直到拨杆复位
+        }
+        break;
 
-        default:
-            currentState1 = BALL_IDLE;
-            break;
+    case EXTEND_FINISH:
+        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
+        xiaomi.position = O_ANGLE; // 一直保持伸出
+        // 等待拨杆复位（如切到MIDDLE2），Idle_control会负责回位
+        break;
+
+    default:
+        currentState1 = BALL_IDLE;
+        break;
     }
 }
 
+
 void Ball::Idle_control()
 {
-    HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭 
-    HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭 
+    if(ball_state==1 && haveball==0)// 读取光电状态(有球 0，无球 1)
+    {
+      HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 确保爪气缸张开  
+
+    }
+    if(ball_state==0)
+    {
+        haveball=1;//变为持球状态
+        osDelay(500);
+        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸闭合
+    }
+
+
+    // HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保爪气缸关闭
+    HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
 
-    
     osThreadFlagsClear(EXTEND_OK);
 
-    if(ready_key==SIDE) // 检测是否准备好
+    if (ready_key == SIDE) // 检测是否准备好
     {
-        xiaomi.position = WAIT_POS; 
-        if(feedback->position_deg >= WAIT_POS-3)
+        if(hand_thread & HANDING_FINISH)
         {
-          osThreadFlagsSet(task_struct.thread.shoot, READY_TELL);
+            xiaomi.position=HANGDING_POS;//继续保持外伸
+
         }
+        else
+        {
+          xiaomi.position = WAIT_POS;
+          if (feedback->position_deg >= WAIT_POS - 3)
+         {
+            // 只在READY_TELL未置位时发送，防止重复
+            if ((osThreadFlagsGet() & READY_TELL) == 0)
+            {
+                osThreadFlagsSet(task_struct.thread.shoot, READY_TELL);
+            }
+         }
+        }
+
+
     }
     else
     {
         xiaomi.position = I_ANGLE; // 默认回到收回位置
     }
-    
+
     // 拨杆回到中间挡位时，回位并重置状态机
-    if(currentState1==EXTEND_FINISH)//转移后
+    if (currentState1 == EXTEND_FINISH) // 转移后
     {
-         xiaomi.position = I_ANGLE; 
-         currentState1 = BALL_IDLE;
+        xiaomi.position = I_ANGLE;
+        currentState1 = BALL_IDLE;
     }
-    if(currentState1==BALL_FINISH)//运球完成
+    if (currentState1 == BALL_FINISH) // 运球完成
     {
-        xiaomi.position = O_ANGLE; 
-         currentState1 = BALL_IDLE;
+        xiaomi.position = O_ANGLE;
+        currentState1 = BALL_IDLE;
     }
     else
     {
-       currentState1 = BALL_IDLE;  
+        currentState1 = BALL_IDLE;
     }
-    //xiaomi.position = I_ANGLE;
-   
+
 }
 
- int ball_state = 0;
- int last_ball_state = 0; // 上一次的光电状态
+
+
+int ball_state = 0;
+int last_ball_state = 0; // 上一次的光电状态
 
 void Ball::ballHadling(void)
 {
-  
+
     switch (currentState1)
     {
     case BALL_IDLE:
         if (rc_key == DOWN2)
-        {   
-            xiaomi.position = O_ANGLE;//外伸       
-            if(feedback->position_deg>= O_ANGLE-1)// 确保伸缩电机到位
+        {
+            xiaomi.position = HANGDING_POS;                 // 外伸
+            if (feedback->position_deg >= HANGDING_POS - 0.5f) // 确保伸缩电机到位
             {
-                 currentState1 = BALL_FORWARD; 
+                currentState1 = BALL_FORWARD;
             }
-           
         }
         break;
 
     case BALL_FORWARD:
+        osDelay(300); // 不放太快
         HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸爪子
-        osDelay(5); 
+        osDelay(5);
         HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
-        currentState1 = BALL_DROP; // 切换到球下落状态
+        currentState1 = BALL_DROP;                                 // 切换到球下落状态
         break;
 
-        case BALL_DROP:
-        osDelay(100); // 延时 100ms
+    case BALL_DROP:
+        osDelay(100); //不要动这里                                        
         HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
         // 一直检测有球（ball_state == 0），等球离开
         if (ball_state == 1 && last_ball_state == 0) // 球离开
@@ -387,7 +421,7 @@ void Ball::ballHadling(void)
         last_ball_state = ball_state;
         break;
 
-        case BALL_FLAG:
+    case BALL_FLAG:
         osDelay(10); // 延时 50ms
         // 等待球弹回再次检测到球
         if (ball_state == 0 && last_ball_state == 1) // 球弹回
@@ -399,26 +433,22 @@ void Ball::ballHadling(void)
 
     case BALL_CLOSE:
 
-            osDelay(25); 
-            HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
-            currentState1 = BALL_FINISH; // 切换到反转状态
+         osDelay(100); 
+        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
+        currentState1 = BALL_FINISH;                                   // 切换到反转状态
         break;
 
     case BALL_FINISH:
         osDelay(50); // 延时 50ms
-        //osThreadFlagsSet(task_struct.thread.ball, HANDING_FINISH); 
+        osThreadFlagsSet(task_struct.thread.ball, HANDING_FINISH);
         HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
-        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
+        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);   // 确保下气缸关闭
         break;
 
     default:
         currentState1 = BALL_IDLE; // 默认回到空闲状态
         break;
     }
+}
 
-    
- }   
-
- 
 #endif
-

User/module/ball.hpp

@@ -39,7 +39,8 @@ typedef enum
     DOWN2,
     IN,
     OUT,
-    SIDE
+    SIDE,
+    DEF
 }ball_rc_mode;
 
 // 定义光电传感器检测宏
@@ -73,11 +74,10 @@ public:
     int16_t extern_key;
     int16_t ready_key; //准备按键
     //用于传接球,运球后通知
-    volatile BallState_t ballStatus;//是否有球
     volatile uint32_t hand_thread;//接收传回的线程通知
 
 private:  
-
+    bool haveball;
 
     
 };

User/module/gimbal.cpp

@@ -1,113 +0,0 @@
-#include "TopDefine.h"
-#include "gimbal.hpp"
-#include "remote_control.h"
-#include "calc_lib.h"
-#include "FreeRTOS.h"
-#include <cmsis_os2.h>
-
-#define KP 0.12
-#define KD 0.008
-//可活动角度
-#define ANGLE_ALLOW 1.0f
-extern RC_ctrl_t rc_ctrl;
-NUC_t nuc;
-
-const fp32 Gimbal:: Gimbal_speed_PID[3] = {50, 0.1,  0};
-const fp32 Gimbal:: Gimbal_angle_PID[3]=  { 5, 0.01, 0};
-
-#if GM6020ING ==1
-Gimbal::Gimbal()
-{
-  // GM6020_Motor = get_motor_point(6);
-  // GM6020_Motor->type = GM6020;
-  // PID_init(&speed_pid,PID_POSITION,Gimbal_speed_PID,16000, 6000);
-  // PID_init(&angle_pid,PID_POSITION,Gimbal_angle_PID,5000, 2000);
-
-  // result = 0;
-  // angleSet = 0;
-
-}
-
-void Gimbal::gimbalFlow()
-{
-  int16_t delta[1];
-  //angleSet = angle1;
-  delta[0] = PID_calc(&angle_pid,GM6020_Motor->total_angle,angleSet);
-  result = PID_calc(&speed_pid, GM6020_Motor->speed_rpm, delta[0]);
-
-  CAN_cmd_1FF(0,0,result,0,&hcan1);
-  osDelay(1);
-
-}
-
-void Gimbal::gimbalZero()
-{
-  angleSet=0;
-  //gimbalFlow();
-
-}
-
-void Gimbal::gimbalVision(const NUC_t &nuc)
-{
-    int16_t delta[1];
-    angleSet = nuc.vision.x;
-    delta[0] = PID_calc(&angle_pid,GM6020_Motor->total_angle,angleSet);
-    result = PID_calc(&speed_pid, GM6020_Motor->speed_rpm, delta[0]);
-
-    CAN_cmd_1FF(0,0,result,0,&hcan1);
-    osDelay(1);
-}
-
-
-
-#else
-Gimbal::Gimbal()
-{
-
-			Kp = KP;
-			Kd = KD;
-			allowRange = ANGLE_ALLOW;
-}   
-
-void Gimbal::gimbalInit(void)
-{
-		int i;
-    GO_M8010_init();
-    for(i = 0;i < GO_NUM;i ++)
-    {
-        goData[i] = getGoPoint(i);//获取电机数据指针
-
-        angleSet[i] = 0;
-        offestAngle[i] = 0;
-				GO_M8010_send_data(&huart6, i,0,0,0,0,0,0);
-				offestAngle[i] = goData[i]->Pos;
-				HAL_Delay(100);
-
-    }
-		
-}
-
-void Gimbal::gimbalFlow(void)
-{
-
-    //angleSet[0] = map_fp32((float)rc_ctrl.ch[3],-800.0f,800.0f,-allowRange,allowRange) + offestAngle[0];
-    GO_M8010_send_data(&huart6, 0,0,0,angleSet[0],1,KP,KD);
-		osDelay(1);
-
-
-}
-
-
-void Gimbal::gimbalZero(void)
-{
-  GO_M8010_send_data(&huart6, 0,0,0,0,0,0,0);
-}
-
-void Gimbal::gimbalVision(const NUC_t &nuc)
-{
-    angleSet[0] = nuc.vision.x;
-    GO_M8010_send_data(&huart6, 0,0,0,angleSet[0],1,KP,KD);
-    osDelay(1);
-}
-
-#endif

User/module/gimbal.hpp

@@ -1,57 +0,0 @@
-#ifndef GIMBAL_HPP
-#define GIMBAL_HPP
-
-#include "GO_M8010_6_Driver.h"
-#include "djiMotor.h"
-#include "pid.h"
-#include "nuc.h"
-
-class Gimbal
-{
-public:
-    Gimbal();
-    void gimbalFlow(void);//云台随遥控器转动
-    void gimbalZero(void);//云台零阻尼模式
-		void gimbalInit(void);//go初始化
-    void gimbalVision(const NUC_t &nuc); // 接收 NUC_t 数据
-
-    int16_t result;
-    //暂存要发送的扭矩
-    //float result[GO_NUM]; 
-//		float Kp;
-//		float Kd;
-private:
-
-#if GM6020ING == 1
-//GM6020电机数据
- motor_measure_t *GM6020_Motor;
-
- static const float Gimbal_speed_PID[3];
- static const float Gimbal_angle_PID[3];
-
-  //电机速度pid结构体
-  pid_type_def speed_pid;
-  //位置环pid
-  pid_type_def angle_pid;
-
-  float angleSet;
-
-#else
-    motor_measure_t *motorData[GO_NUM];
-  //视觉发送的要调的角度
-    float self_angleSet;
-    GO_Motorfield* goData[GO_NUM];
-    //暂存目标位置
-    float angleSet[GO_NUM];
-    float offestAngle[GO_NUM];//go数据
-		float Kp;
-		float Kd;
-		float allowRange;
-
-#endif    
-};
-
-
-
-
-#endif

User/module/shoot.hpp

@@ -18,7 +18,8 @@ typedef enum {
     SHOOT_RETURN,       // 自动返回状态
     //运射配合
     GO_TOP,
-    BAKC
+    BAKC,
+    WAIT_CHANGE
 } ShootState_t;
 
 // // 定义状态枚举
@@ -40,8 +41,10 @@ typedef enum
     WAIT,
     TEST,
     VSION,
+    PASS,
     OFFENSIVE,
     DEFENSE
+    
 }rc_mode;
 
 // 光电传感器读取宏
@@ -84,6 +87,7 @@ public:
 
     //滤波器
     LowPassFilter2p_t distance_filter; // 用于滤波视觉距离
+    LowPassFilter2p_t pass_filter;
 
     //==========================公共变量==========================
     int16_t rc_key; //遥控器按键
@@ -94,6 +98,7 @@ public:
 
     volatile uint32_t shoot_thread;//接收传回的线程通知
     fp32 distance;   //视觉距离
+    fp32 pass_distance;   //视觉距离
 
 private:
     //扳机2006
@@ -109,6 +114,8 @@ private:
     
     float limit_speed;//go电机限速
 
+    int shoot_wait;
+
 
 };
 

User/task/ballTask.cpp

@@ -8,11 +8,7 @@
 #include "vofa.h"
 extern RC_ctrl_t rc_ctrl;
 Ball ball;
-//float vofa[8];
 
-//检查光电
-int abc=0;
-int aaaa=146;
 
 extern int speedm;
 
@@ -22,7 +18,7 @@ void FunctionBall(void *argument)
 
     const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_BALL;
 	
-		osDelay(6000);//等待极致控制板启动
+	  osDelay(6000);//等待极致控制板启动
     XiaomiWait_init(1,&hcan2); //小米电机初始化
     
     uint32_t tick = osKernelGetTickCount();
@@ -32,18 +28,9 @@ void FunctionBall(void *argument)
 #ifdef DEBUG
      task_struct.stack_water_mark.ball = osThreadGetStackSpace(osThreadGetId());  
 #endif          
-
-    abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
-
-     ball.rc_mode(); // 遥控器模式  
+      ball.rc_mode(); // 遥控器模式  
     
-     ball.ball_control(); // 控制球的动作    
-
-//        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 确保气缸爪子闭合
-//        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET);   // 确保下气缸关闭
-
-    // ball.xiaomi.position = aaaa; 
-    // CAN_XiaoMi(1,&ball.xiaomi,&hcan2);
+      ball.ball_control(); // 控制球的动作    
 
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     osDelayUntil(tick);

User/task/encodeCan.cpp

@@ -26,7 +26,7 @@ void FunctionCan(void *argument)
      task_struct.stack_water_mark.can = osThreadGetStackSpace(osThreadGetId()); 
     
   #endif 
-        waitNewDji();
+        //waitNewDji();
         djiMotorEncode();
         can2MotorEncode();
 

User/task/gimbalTask.cpp

@@ -1,47 +0,0 @@
-#include "TopDefine.h"
-#include "FreeRTOS.h"
-#include "userTask.h"
-#include <cmsis_os2.h>
-#include "gimbalTask.hpp"
-#include "gimbal.hpp"
-#include "main.h"
-#include "remote_control.h"
-#include "nuc.h"
-Gimbal gimbal;
-// NUC_t nucData; // 用于存储从队列接收的数据
-extern RC_ctrl_t rc_ctrl;
-int cnt1=0;
-
-void FunctionGimbal(void *argument)
-{
-	(void)argument; /* 未使用argument，消除警告 */
-
-    const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_GIMBAL;
-	
-	HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET);
-
-	uint32_t tick = osKernelGetTickCount();
-
-	while(1)
-	{
-   #ifdef DEBUG
-		task_struct.stack_water_mark.gimbal = osThreadGetStackSpace(osThreadGetId());  
-   #endif 
-
-		//cnt1++;
-
-    //  gimbal.gimbalFlow();
-		// 从消息队列接收视觉数据
-		// if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
-		// {
-		// 	// 使用接收到的视觉数据调整云台
-		// 	//gimbal.gimbalVision(nucData);
-		// }
-			
-		osDelay(1);
-
-		tick += delay_tick; /* 计算下一个唤醒时刻 */
-        osDelayUntil(tick);
-	}
-}
-

User/task/gimbalTask.hpp

@@ -1,5 +0,0 @@
-#ifndef GIMBALTASK_HPP
-#define GIMBALTASK_HPP
-
-
-#endif

User/task/nucTask.cpp

@@ -1,58 +1,67 @@
 #include "nucTask.hpp"
 #include "nuc.h"
 #include "userTask.h"
-#include "TopDefine.h"//事件组的一些东西
+#include "TopDefine.h" //事件组的一些东西
 #include "FreeRTOS.h"
 #include <cmsis_os2.h>
 #include <stdio.h>
-
+#include "bsp_buzzer.h"
 
 #ifdef DEBUG
 
 NUC_t cmd_fromnuc;
-//int send[3]={1,2,3};
-float send[3]={1,2,3};
 
 #endif
 
 void Function_nuc(void *argument)
 {
-    (void)argument; /* 未使用argument，消除警告 */
-    
-   const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_AI;
+        (void)argument; /* 未使用argument，消除警告 */
 
-    NUC_Init(&cmd_fromnuc);
-  
-    HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET);
+        const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_AI;
 
-   uint32_t tick = osKernelGetTickCount();
+        NUC_Init(&cmd_fromnuc);
 
-    while(1)
-    {
+        uint32_t tick = osKernelGetTickCount();
+
+        while (1)
+        {
 #ifdef DEBUG
-        task_struct.stack_water_mark.nuc = osThreadGetStackSpace(osThreadGetId());  
-#endif         
+        task_struct.stack_water_mark.nuc = osThreadGetStackSpace(osThreadGetId());
+#endif
         NUC_StartReceiving();
 
-        NUC_RawParse(&cmd_fromnuc);
-        //NUC_SendPacket(&send, sizeof(send)); // 发送数据包
-//掉线处理有空写 
-        // if(NUC_WaitDmaCplt())
-        // {
-        //     NUC_RawParse(&cmd_fromnuc);
-            
+        // NUC_RawParse(&cmd_fromnuc);
+        // NUC_SendPacket(&send, sizeof(send)); // 发送数据包
+        // 掉线处理有空写
+        //  if(NUC_WaitDmaCplt())
+        //  {
+        //      NUC_RawParse(&cmd_fromnuc);
+
         // }
         // else
         // {
         //     NUC_HandleOffline(&cmd_fromnuc);
         // }
 
-        osMessageQueueReset(task_struct.msgq.nuc); // 清空消息队列
-        osMessageQueuePut(task_struct.msgq.nuc,&(cmd_fromnuc),0,0);
-        osDelay(2);
+        if (NUC_RawParse(&cmd_fromnuc) == DEVICE_OK)
+        {
+                //BSP_Buzzer_Stop();
+                HAL_GPIO_WritePin(LED_G_GPIO_Port, LED_G_Pin, GPIO_PIN_SET);   
+                HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET); 
+                osMessageQueueReset(task_struct.msgq.nuc);                     // 清空消息队列
+                osMessageQueuePut(task_struct.msgq.nuc, &(cmd_fromnuc), 0, 0);
+        }
+        else
+        {
 
-       tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/
-       osDelayUntil(tick);
-        
-    }
+                // BSP_Buzzer_Start();
+                // BSP_Buzzer_Set(1, 5000);
+                HAL_GPIO_WritePin(LED_G_GPIO_Port, LED_G_Pin, GPIO_PIN_RESET); 
+                HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_SET);   
+                NUC_HandleOffline(&cmd_fromnuc);
+        }
+
+        tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/
+        osDelayUntil(tick);
+        }
 }

User/task/shootTask.cpp

@@ -10,14 +10,14 @@ extern RC_ctrl_t rc_ctrl;
 Shoot shoot;
 NUC_t nucData; // 自瞄
 
-int aaaxxx=0;
+
 
 void FunctionShoot(void *argument)
 {
 	(void)argument; /* 未使用argument，消除警告 */
 
-    const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_SHOOT;
-
+    const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_SHOOT;
+	osDelay(3000);//等待M2006电机启动
 	uint32_t tick = osKernelGetTickCount();
 
 while(1)
@@ -35,13 +35,6 @@ while(1)
 
 	shoot.shoot_control();
 	
-//	shoot.t_posSet=aaaxxx;
-//	shoot.trigger_control();
-//  shoot.GO_SendData(goangle,5);
-//  shoot.shoot_control();
-//	shoot.t_posSet=goangle;
-//	shoot.trigger_control();
-
 	tick += delay_tick; /* 计算下一个唤醒时刻 */
 	osDelayUntil(tick);		
 

User/task/userTask.h

@@ -13,11 +13,8 @@ extern "C" {
 /* Exported constants ------------------------------------------------------- */
 
 /* 所有任务都要define一个“任务运行频率”和“初始化延时” */
-#define TASK_FREQ_CTRL_GIMBAL (250u)
-#define TASK_FREQ_CTRL_SHOOT (500u)
-#define TASK_FREQ_CTRL_COMMAND (500u)
-#define TASK_FREQ_MONITOR (2u)
-#define TASK_FREQ_CAN (1500u)
+#define TASK_FREQ_SHOOT (500u)
+#define TASK_FREQ_CAN (500u)
 #define TASK_FREQ_AI (500u)
 #define TASK_FREQ_BALL (500u)
 
@@ -42,8 +39,6 @@ typedef struct
         //osThreadId_t rc;
         osThreadId_t nuc;
         osThreadId_t shoot;
-        osThreadId_t gimbal;
-        //osThreadId_t handling;
         osThreadId_t ball;
 
     }thread;
@@ -64,32 +59,23 @@ typedef struct
     #ifdef DEBUG
     struct {
         UBaseType_t can;
-        //UBaseType_t rc;
         UBaseType_t nuc;
         UBaseType_t shoot;
-        UBaseType_t gimbal;
-        //UBaseType_t handling;
         UBaseType_t ball;
 
     } stack_water_mark; /* stack使用 */
 
     struct {
         float can;
-        //float rc;
         float nuc;
         float shoot;
-        float gimbal;
-        //float handling;
         float ball;
     } freq; /* 任务运行频率 */
 
     struct {
         float can;
-        //float rc;
         float nuc;
         float shoot;
-        float gimbal;
-        //float handling;
         float ball;
     } last_up_time; /* 任务最近运行时 */
 #endif
@@ -106,7 +92,6 @@ extern const osEventFlagsAttr_t attr_event;
 
 
 /* Exported functions prototypes -------------------------------------------- */
-void FunctionTake(void *argument);
 void FunctionShoot(void *argument);
 void FunctionCan(void *argument);
 void FunctionRc(void *argument);