开始做拟合自瞄

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

@@ -90,3 +90,13 @@
 
 [info] Log at : 2025/5/20|19:15:21|GMT+0800
 
+[info] Log at : 2025/5/21|17:11:48|GMT+0800
+
+[info] Log at : 2025/5/21|19:21:48|GMT+0800
+
+[info] Log at : 2025/5/22|18:05:58|GMT+0800
+
+[info] Log at : 2025/5/23|19:18:37|GMT+0800
+
+[info] Log at : 2025/5/23|19:32:28|GMT+0800
+

MDK-ARM/.vscode/settings.json

@@ -13,6 +13,22 @@
         "functional": "cpp",
         "vofa.h": "c",
         "user_math.h": "c",
-        "queue": "cpp"
+        "queue": "cpp",
+        "cctype": "cpp",
+        "cerrno": "cpp",
+        "cfloat": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "exception": "cpp",
+        "new": "cpp",
+        "typeinfo": "cpp",
+        "ostream": "cpp"
     }
 }

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 ball.cpp...
+compiling gimbalTask.cpp...
 linking...
-Program Size: Code=28460 RO-data=1824 RW-data=256 ZI-data=23936  
+Program Size: Code=28656 RO-data=1824 RW-data=252 ZI-data=23940  
 FromELF: creating hex file...
 "R1-shooter\R1-shooter.axf" - 0 Error(s), 0 Warning(s).
-Build Time Elapsed:  00:00:02
+Build Time Elapsed:  00:00:03

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

@@ -1 +1 @@
-2025/5/20 23:55:49
+2025/5/23 20:55:43

MDK-ARM/R1-shooter.uvoptx

@@ -103,7 +103,7 @@
         <bEvRecOn>1</bEvRecOn>
         <bSchkAxf>0</bSchkAxf>
         <bTchkAxf>0</bTchkAxf>
-        <nTsel>3</nTsel>
+        <nTsel>6</nTsel>
         <sDll></sDll>
         <sDllPa></sDllPa>
         <sDlgDll></sDlgDll>
@@ -114,7 +114,7 @@
         <tDlgDll></tDlgDll>
         <tDlgPa></tDlgPa>
         <tIfile></tIfile>
-        <pMon>BIN\CMSIS_AGDI.dll</pMon>
+        <pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
       </DebugOpt>
       <TargetDriverDllRegistry>
         <SetRegEntry>
@@ -140,7 +140,7 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGUARM</Key>
-          <Name></Name>
+          <Name>(105=-1,-1,-1,-1,0)</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -153,7 +153,40 @@
           <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>
@@ -255,6 +288,21 @@
           <WinNumber>1</WinNumber>
           <ItemText>angle1,0x0A</ItemText>
         </Ww>
+        <Ww>
+          <count>20</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>rx_header</ItemText>
+        </Ww>
+        <Ww>
+          <count>21</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>rx_data</ItemText>
+        </Ww>
+        <Ww>
+          <count>22</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>motor_5065</ItemText>
+        </Ww>
       </WatchWindow1>
       <WatchWindow2>
         <Ww>
@@ -985,7 +1033,7 @@
 
   <Group>
     <GroupName>User/device</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -1065,7 +1113,7 @@
 
   <Group>
     <GroupName>User/module</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>

User/bsp/uart_it.c

@@ -13,7 +13,7 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
     return BSP_UART_DR16;
   else if (huart->Instance == USART1)
     return BSP_UART_REF;
-  else if (huart->Instance == USART1)
+  else if (huart->Instance == USART6)
     return BSP_UART_AI;
   /*
   else if (huart->Instance == USARTX)
@@ -112,7 +112,7 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
     case BSP_UART_REF:
       return &huart1;
     case BSP_UART_AI:
-      return &huart1;
+      return &huart6;
     /*
     case BSP_UART_XXX:
             return &huartX;

User/module/ball.cpp

@@ -10,8 +10,7 @@ extern int key;
 // C键 sw[4]👆 200   中1000  👇1800
 // D键 sw[5]👆 1800   👇200
 
-#define START 0
+#define M_SPEED 4000
-#define OUT 100
 
 #if HANDLING3 == 1
 //三摩擦轮运球！！！
@@ -21,8 +20,9 @@ const fp32 Ball:: M3508_speed_PID[3] = {15, 0.03, 0};
 const fp32 Ball:: Extend_speed_PID[3] = { 25, 0, 0.};
 const fp32 Ball:: Extend_angle_PID[3]=  { 60, 0, 0.1};
 
+//摩擦轮转速
 int speedm=0;
-int speedm1=0;
+
 //PE11 气缸
 
 Ball ::Ball()
@@ -64,27 +64,6 @@ Ball ::Ball()
   currentState1= BALL_IDLE;
 
 
-    //   for(int i = 0;i < MOTOR_NUM;i ++)
-    // {
-    //  //摩擦轮滤波器初始化
-    //  LowPassFilter2p_Init(filter.in + i , 500,
-    //     -1.0f);
-    //  LowPassFilter2p_Init(filter.out + i, 500,
-    //     -1.0f);   
-    // }
-
-}
-
-void  Ball :: Filter_init(float target_freq)
-{
-    for(int i = 0;i < MOTOR_NUM;i ++)
-    {
-     //摩擦轮滤波器初始化
-     LowPassFilter2p_Init(filter.in + i , target_freq,
-        -1.0f);
-     LowPassFilter2p_Init(filter.out + i, target_freq,
-        -1.0f);   
-    }
 }
 
 void Ball ::Extend_control(int angle)
@@ -169,10 +148,6 @@ void Ball::ballTake(void)
 }
 
  
-
-	int flag =0;
-	int ball_state = 0;
-   
 void Ball::ballHadling(void)
 {
 
@@ -181,7 +156,6 @@ void Ball::ballHadling(void)
     {
     case BALL_IDLE:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
-     //   if (key > 0) // 检测按键是否被按下
         if (rc_ctrl.sw[4] > 1000||key > 0) // 检测按键是否被按下,自动回弹的
         {
             speedm=-4000;
@@ -223,7 +197,7 @@ void Ball::ballHadling(void)
      break;
 
     case BALL_CLOSE:
-       // osDelay(200); // 延时50ms
+
        if(ball_state == 1)
        {
            HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
@@ -235,7 +209,6 @@ void Ball::ballHadling(void)
     case BALL_FINISH:
         osDelay(200); // 延时50ms
         key=0; // 重置按键状态
-        flag=0;
         speedm=0;
         osThreadFlagsSet(task_struct.thread.shoot, BALL_OK);
         currentState1 = BALL_IDLE; // 回到空闲状态

User/module/ball.hpp

@@ -48,7 +48,6 @@ class Ball
 {
 public:
     Ball();
-    void Filter_init(float target_freq);
     void Spin(float speed);
     void ballHadling(void);
     void ballDown(void);
@@ -58,6 +57,8 @@ public:
     void Extend_control(int angle);
 
     BallState_t currentState1; // 当前状态
+    int flag;//暂时还没用到
+	  int ball_state ;//光电检测
     
     //伸缩6020
     int16_t e_result[2];
@@ -70,7 +71,6 @@ public:
     volatile BallState_t ballStatus;//是否有球
     volatile uint32_t flag_thread;//接收传回的线程通知
 
-      Filter filter;
 private:  
     //滤波一下
   

User/module/shoot.cpp

@@ -13,6 +13,8 @@ float vofa[8];
 #define SHOOT_SPEED_BACK -2500
 #define Error 50
 
+int test_speed =30500;
+
 #define STOP 0
 
 #define Trigger_Torque -5000
@@ -95,6 +97,8 @@ void Shoot::shootStop()
 }
 
 void Shoot::shootStateMachine() {
+
+     distance =3.5;
     switch (currentState) {
         case SHOOT_IDLE: {
             speed_5065=STOP;
@@ -108,9 +112,10 @@ void Shoot::shootStateMachine() {
 
         case SHOOT_FIRE: {
             // 发射状态，控制电机发射
-            speed_5065 = SHOOT_SPEED;
+          // speed_5065 = test_speed;
-        if(motor_5065[0]->rotor_speed>=SHOOT_SPEED-Error && motor_5065[0]->rotor_speed<=SHOOT_SPEED+Error &&
+            speed_5065 =distanceToSpeed(distance);
-		   motor_5065[1]->rotor_speed>=SHOOT_SPEED-Error && motor_5065[1]->rotor_speed<=SHOOT_SPEED+Error)
+        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;//恒力扳机
 		}
@@ -161,3 +166,12 @@ void Shoot::shootStateMachine() {
         vofa_tx_main(vofa); // 发送数据到虚拟串口
      
 }
+
+//拟合函数
+float distanceToSpeed(float x) {
+    return -2.3958f * x * x * x * x
+           + 59.615f * x * x * x
+           - 525.63f * x * x
+           + 2136.4f * x
+           + 28001.0f;
+}

User/module/shoot.hpp

@@ -44,6 +44,9 @@ public:
     volatile BallState_t ballStatus;//是否有球
     volatile uint32_t flag_thread;//接收传回的线程通知
 
+    //距离
+    float distance;
+
 private:
 
     //扳机2006
@@ -57,4 +60,6 @@ private:
 };
 
 
+float distanceToSpeed(float x);
+
 #endif

User/task/gimbalTask.cpp

@@ -8,7 +8,7 @@
 #include "remote_control.h"
 #include "nuc.h"
 Gimbal gimbal;
-NUC_t nucData; // 用于存储从队列接收的数据
+// NUC_t nucData; // 用于存储从队列接收的数据
 extern RC_ctrl_t rc_ctrl;
 int cnt1=0;
 
@@ -32,11 +32,11 @@ void FunctionGimbal(void *argument)
 
     //  gimbal.gimbalFlow();
 		// 从消息队列接收视觉数据
-		if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
+		// if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
-		{
+		// {
-			// 使用接收到的视觉数据调整云台
+		// 	// 使用接收到的视觉数据调整云台
-			//gimbal.gimbalVision(nucData);
+		// 	//gimbal.gimbalVision(nucData);
-		}
+		// }
 			
 		osDelay(1);
 

User/task/shootTask.cpp

@@ -6,8 +6,10 @@
 #include "shoot.hpp"
 #include "motor.hpp"
 #include "remote_control.h"
+#include "nuc.h"
 extern RC_ctrl_t rc_ctrl;
 Shoot shoot;
+NUC_t nucData; // 自瞄
 
 int shoot_flag = 0;
 
@@ -43,6 +45,11 @@ while(1)
 
 	shoot_flag=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
 
+	if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
+	{
+		// 使用接收到的视觉数据调整云台
+		//gimbal.gimbalVision(nucData);
+	}
 	if(rc_ctrl.sw[1]>1000)
 	{
 		shoot.shootStateMachine();