初测自瞄

README.md

@@ -1,3 +1,17 @@
 # r1upper
 
 r1上层
+
+## 外设
+
+//PE9  刹车光电
+
+//PE11 接球气缸
+
+//PE13 爪子气缸
+
+//PE14 下球气缸
+
+//PC6  接球光电
+
+//PI6  运球光电

User/device/djiMotor.c

@@ -78,9 +78,7 @@
 #if DEBUG == 1
 
 //电机回传数据结构体
-motor_measure_t motor_chassis[5];
+motor_measure_t motor_chassis[10];
-motor_measure_t GM6020_motor[2];
-motor_measure_t motor_2006[2];
 CAN_MotorFeedback_t motor_5065[2];
 #else
 static motor_measure_t motor_chassis[5];
@@ -299,24 +297,6 @@ void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeD
  {
      return &motor_chassis[i];
  }
- /**
-   * @brief          返回GM6020电机数据指针
-   * @param[in]      i: 电机编号
-   * @retval         电机数据指针
-   */
- motor_measure_t *get_6020_motor_point(uint8_t i)
- {
-     return &GM6020_motor[i];
- }
- /**
-   * @brief          返回M2006电机数据指针
-   * @param[in]      i: 电机编号
-   * @retval         电机数据指针
-   */
- motor_measure_t *get_2006_motor_point(uint8_t i)
- {
-     return &motor_2006[i];
- }
 
  /**
     * @brief          限制vesc电机转速

User/device/djiMotor.h

@@ -133,22 +133,6 @@ extern void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_Hand
   */
  extern motor_measure_t *get_motor_point(uint8_t i);
 
- /**
-   * @brief          返回GM6020电机数据指针
-   * @param[in]      i: 电机编号
-   * @retval         电机数据指针
-   */
-  motor_measure_t *get_6020_motor_point(uint8_t i);
- 
-  /**
-   * @brief          返回M2006电机数据指针
-   * @param[in]      i: 电机编号
-   * @retval         电机数据指针
-   */
- motor_measure_t *get_2006_motor_point(uint8_t i);
- 
-
-
 /**
  * @brief           数据处理函数
  * @param[in]       none

User/module/ball.cpp

@@ -4,137 +4,66 @@
 #include "detect.h"
 
 extern RC_ctrl_t rc_ctrl;
-#define KP 0.8
+extern int key;
-#define KD 0.008
-//PE9  刹车光电
-//PE11 接球气缸
-//PE13 爪子气缸
-//PE14 下球气缸
-//PC6  接球光电
-//PI6  运球光电
 
-int aaa=0;
 // 定义状态机变量
 BallState_t currentBallState = BALL_IDLE;
 
+#define MOTOR_SPEED 5000
+
+const fp32 Ball:: M3508_speed_PID[3] = {5, 0, 0};
+
+//三摩擦轮运球！！！
+
 Ball ::Ball()
 {  
-    aaa=10;
+   detect_init();
-    //ballState =HAL_GPIO_ReadPin(in_ball_GPIO_Port, in_ball_Pin);
+   for(int i = 0;i < MOTOR_NUM;i ++)
-detect_init();
-   
-}
-
-void Ball ::ballCatch(void)
-{
-
-    if(rc_ctrl.ch[1]>=300)
  {
-        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//收网    
+    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, 6000);//pid初始化
     }  
-    else if(rc_ctrl.ch[1]<300)
+    result[i] = 0;
-    {
+    speedSet[i] = 0;    
-        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//放网
  }
      
 }
 
-void Ball ::ballFlow(void)
+void Ball ::Spin(float speed)
 {
-    GO_M8010_send_data(&huart6, 1,0,0,angleSet[1],0.5,KP,KD);
+    for(int i = 0;i < MOTOR_NUM;i ++)
-}
-
-void Ball ::ballZero(void)
-{
-    GO_M8010_send_data(&huart6, 1,0,0,0,0,0,0);
-}
-
-void Ball ::ballHadling(void)
-{
-    if(rc_ctrl.sw[0]==306)
     {
-        HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET);//爪子气缸 开爪
+        speedSet[i] = speed;
-        osDelay(200);
+        result[i] = PID_calc(&speed_pid[i],hand_Motor[i]->speed_rpm,speedSet[i]);
-        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);//下球气缸 打出
+
-        osDelay(500);
+    }
-        angleSet[1] = 2;
+    CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1);
-        ballFlow();
+    osDelay(1);
+
+}
+
+void Ball::ballHadling(void)
+{
+    static bool isReversed = false; // 静态变量，记录当前摩擦轮状态，初始为正转
+
+    if (key > 0) // 检测按键是否被按下
+    {
+        key = 0; // 重置按键状态，防止重复触发
+
+        if (isReversed)
+        {
+            // 当前为反转，切换为正转
+            Spin(MOTOR_SPEED); // 正转
+            isReversed = false;
         }
         else
         {
-        HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET);//爪子气缸 关爪
+            // 当前为正转，切换为反转
-        HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET);//下球气缸 打入
+            Spin(-MOTOR_SPEED); // 反转
-        angleSet[1] = 0;
+            isReversed = true;
-        ballFlow();
-    }
-
-}
-
-void Ball::ballStateMachine() {
-    switch (currentBallState) {
-        case BALL_IDLE: {
-            // 空闲状态，等待遥控器输入
-            if (rc_ctrl.sw[0] == 306) { // 假设遥控器开关控制开始运球
-              //  HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 收网
-              //  osDelay(200); // 等待收网完成
-                currentBallState = BALL_RELEASE; // 切换到下球气缸弹出状态
-               
-            }
-            break;
-        }
-
-        case BALL_RELEASE: {
-            // 下球气缸弹出，爪子张开
-            HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 下球气缸弹出
-            HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET);    // 爪子张开
-            osDelay(200); // 等待动作完成
-            currentBallState = BALL_JOINT_REVERSE; // 切换到关节电机反转状态
-            break;
-        }
-
-        case BALL_JOINT_REVERSE: {
-            // 关节电机反转置水平位置
-            angleSet[1] = 2; // 设置关节电机目标角度为水平位置
-            ballFlow();      // 控制关节电机动作
-            osDelay(500);    // 等待电机动作完成
-            currentBallState = BALL_FALLING; // 切换到球加速落下状态
-            break;
-        }
-
-        case BALL_FALLING: {
-            // 球加速落下，等待光电检测
-            if (IS_PHOTOELECTRIC_BALL()) {
-                currentBallState = BALL_WAIT_BOUNCE; // 切换到等待反弹状态
-            }
-            break;
-        }
-
-        case BALL_WAIT_BOUNCE: {
-            // 等待光电检测反弹
-            osDelay(50); // 简单延时，避免误触发
-            if (IS_PHOTOELECTRIC_BALL()) {
-                currentBallState = BALL_NET_CLOSE; // 切换到收网状态
-            }
-            break;
-        }
-
-        case BALL_NET_CLOSE: {
-            // 收网状态
-            // HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 收网
-            // osDelay(200); // 等待收网完成
-            HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET);  // 关闭爪子
-            HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);  // 下球气缸复位
-            angleSet[1] = 1; // 关节电机复位
-            ballFlow();
-            currentBallState = BALL_IDLE; // 切换回空闲状态
-            break;
-        }
-
-        default: {
-
-            currentBallState = BALL_IDLE; // 默认回到空闲状态
-            
-            break;
         }
     }
 }

User/module/ball.hpp

@@ -4,7 +4,8 @@
 #include "cmsis_os.h"
 #include "FreeRTOS.h"
 #include "bsp_delay.h"
-#include "GO_M8010_6_Driver.h"
+#include "djiMotor.h"
+#include "pid.h"
 
 // 定义状态枚举
 typedef enum {
@@ -16,6 +17,15 @@ typedef enum {
     BALL_NET_CLOSE      // 收网状态
 } BallState_t;
 
+typedef enum
+{
+    MOTOR_1 = 0,
+    MOTOR_2 = 1,
+    MOTOR_3 = 2,
+    MOTOR_DOWN = 3,
+    MOTOR_NUM
+}motorhangd_e;
+
 // 定义光电传感器检测宏
 #define IS_PHOTOELECTRIC_BALL() (HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin) == GPIO_PIN_RESET)
 
@@ -23,23 +33,27 @@ class Ball
 {
 public:
     Ball();
-    
+    void Spin(float speed);
     void ballHadling(void);
-    void ballCatch(void);
-    void ballFlow(void);
-    void ballStop(void);
-    void ballZero(void);
     void ballStateMachine(void);
-    int ballState; //球状态 0:无球 1:有球
+
+    int16_t result[MOTOR_NUM];
+   
 private:
-    //int ballState; //球状态 0:无球 1:有球
+
     int up_ball_state; //上球状态 0:无球 1:有球
-    GO_Motorfield *goData[GO_NUM]; //电机数据指针
+    motor_measure_t *hand_Motor[MOTOR_NUM];
-    float angleSet[GO_NUM]; //电机目标角度
+    //三个摩擦轮
-    float offestAngle[GO_NUM]; //电机初始角度
+    static const float M3508_speed_PID[3];
-    float Kp; //比例系数
+    static const float M3508_angle_PID[3];
-    float Kd; //微分系数
+
-    float allowRange; //可活动角度    
+    //电机速度pid结构体
+    pid_type_def speed_pid[MOTOR_NUM];
+    //位置环pid
+    pid_type_def angle_pid[MOTOR_NUM];
+
+    float angleSet;
+    float speedSet[MOTOR_NUM];
 };
 
 #endif

User/module/gimbal.cpp

@@ -10,8 +10,58 @@
 //可活动角度
 #define ANGLE_ALLOW 1.0f
 extern RC_ctrl_t rc_ctrl;
-
+int angle1 = 0;
 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()
 {
 
@@ -60,3 +110,5 @@ void Gimbal::gimbalVision(const NUC_t &nuc)
     GO_M8010_send_data(&huart6, 0,0,0,angleSet[0],1,KP,KD);
     osDelay(1);
 }
+
+#endif

User/module/gimbal.hpp

@@ -2,8 +2,11 @@
 #define GIMBAL_HPP
 
 #include "GO_M8010_6_Driver.h"
+#include "djiMotor.h"
 #include "pid.h"
 #include "nuc.h"
+
+#define GM6020ING 1
 class Gimbal
 {
 public:
@@ -11,17 +14,31 @@ public:
     void gimbalFlow(void);//云台随遥控器转动
     void gimbalZero(void);//云台零阻尼模式
 		void gimbalInit(void);//go初始化
-    //void gimbalVision(void);//云台视觉模式
     void gimbalVision(const NUC_t &nuc); // 接收 NUC_t 数据
+
+    int16_t result;
     //暂存要发送的扭矩
     //float result[GO_NUM]; 
 //		float Kp;
 //		float Kd;
 private:
-//    //电机速度pid结构体
+
-//    pid_type_def speed_pid[GO_NUM];
+#if GM6020ING == 1
-//    //位置环pid
+//GM6020电机数据
-//    pid_type_def angle_pid[GO_NUM];
+ 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];
@@ -31,6 +48,8 @@ private:
 		float Kp;
 		float Kd;
 		float allowRange;
+
+#endif    
 };
 
 

User/task/ballTask.cpp

@@ -15,10 +15,6 @@ void FunctionBall(void *argument)
     (void)argument; /* 未使用argument，消除警告 */
 
     const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_BALL;
-
-    ball.ballZero();
-	ball.ballState=abc;
-
     uint32_t tick = osKernelGetTickCount();
 
     while(1)
@@ -26,14 +22,8 @@ void FunctionBall(void *argument)
 #ifdef DEBUG
      task_struct.stack_water_mark.ball = osThreadGetStackSpace(osThreadGetId());  
 #endif          
-        abc++;	
-        c30=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);	
-
-
-        ball.ballHadling();
-        ball.ballStateMachine();
-        ball.ballCatch();
        	
+        //ball.ballHadling(); // 处理摩擦轮转动
         osDelay(3);
 
         tick += delay_tick; /* 计算下一个唤醒时刻 */

User/task/encodeCan.cpp

@@ -24,7 +24,7 @@ void FunctionCan(void *argument)
     {
   #ifdef DEBUG
      task_struct.stack_water_mark.can = osThreadGetStackSpace(osThreadGetId()); 
-     task_struct.freq.can = osKernelGetTickFreq() / (osKernelGetTickCount() - tick); 
+    
   #endif 
         waitNewDji();
         djiMotorEncode();

User/task/gimbalTask.cpp

@@ -10,6 +10,7 @@
 Gimbal gimbal;
 NUC_t nucData; // 用于存储从队列接收的数据
 extern RC_ctrl_t rc_ctrl;
+int cnt1=0;
 
 void FunctionGimbal(void *argument)
 {
@@ -17,9 +18,6 @@ void FunctionGimbal(void *argument)
 
     const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_GIMBAL;
 	
-	HAL_Delay(500);
-	gimbal.gimbalInit();
-	gimbal.gimbalZero();
 	HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET);
 
 	uint32_t tick = osKernelGetTickCount();
@@ -29,6 +27,10 @@ void FunctionGimbal(void *argument)
    #ifdef DEBUG
 		task_struct.stack_water_mark.gimbal = osThreadGetStackSpace(osThreadGetId());  
    #endif 
+
+		cnt1++;
+
+    //  gimbal.gimbalFlow();
 		// 从消息队列接收视觉数据
 		if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
 		{
@@ -36,8 +38,6 @@ void FunctionGimbal(void *argument)
 			gimbal.gimbalVision(nucData);
 		}
 			
-			gimbal.gimbalZero();
-			gimbal.gimbalFlow();
 		osDelay(1);
 
 		tick += delay_tick; /* 计算下一个唤醒时刻 */

User/task/nucTask.cpp

@@ -30,6 +30,7 @@ void Function_nuc(void *argument)
 
         NUC_RawParse(&cmd_fromnuc);
         
+//掉线处理有空写 
         // if(NUC_WaitDmaCplt())
         // {
         //     NUC_RawParse(&cmd_fromnuc);

User/task/shootTask.cpp

@@ -25,11 +25,11 @@ while(1)
 #ifdef DEBUG
 	task_struct.stack_water_mark.shoot = osThreadGetStackSpace(osThreadGetId());  
 #endif
-    k++;
 
+    k++;
 	shoot.shootThree();
 	shoot.shootBack();
-	//shoot.shootStateMachine();
+
 	osDelay(2);
 
 	tick += delay_tick; /* 计算下一个唤醒时刻 */