视觉测试可以

User/bsp/can_init.c

@@ -18,18 +18,18 @@ void can_filter_init(void)
     can_filter_st.FilterMaskIdHigh = 0x0000;
     can_filter_st.FilterMaskIdLow = 0x0000;
     can_filter_st.FilterBank = 0;
+    can_filter_st.SlaveStartFilterBank = 14;
     can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
     HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);
     HAL_CAN_Start(&hcan1);
     HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
 
 
-    can_filter_st.SlaveStartFilterBank = 14;
+    
     can_filter_st.FilterBank = 14;
+    can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO1;
     HAL_CAN_ConfigFilter(&hcan2, &can_filter_st);
     HAL_CAN_Start(&hcan2);
     HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);
    
-
-
 }

User/device/GO_M8010_6_Driver.c

@@ -93,6 +93,8 @@ void GO_M8010_send_data(UART_HandleTypeDef *huart, int id,int rev,float T,float
         return;
     }
 
+    Pos = Pos * (3.14159f / 180.0f); // 角度转弧度
+
     // assign motor target goal to the buffer
     motor->motor_send_data.head[0]=0xFE;
     motor->motor_send_data.head[1]=0xEE;
@@ -160,6 +162,7 @@ void GO_M8010_recv_data(uint8_t* Temp_buffer)
     int32_t Pos = Temp_buffer[10]<<24 | Temp_buffer[9]<<16 | Temp_buffer[8]<<8 | Temp_buffer[7];
     motor->Pos = Pos;
     motor->Pos = (motor->Pos * 6.28319f )/(32768*6.33f);
+    motor->Pos = motor->Pos * (180.0f / 3.14159f); // 弧度转角度
     int8_t Temp = Temp_buffer[11] & 0xFF;
     motor->Temp = Temp;
     motor->MError = Temp_buffer[12] & 0x7;

User/device/djiMotor.c

@@ -22,6 +22,7 @@
 #include "TopDefine.h"
 #include<cmsis_os2.h>
 #include "FreeRTOS.h"
+#include <math.h>
 
 #include "odrive_can.h"
 
@@ -78,7 +79,12 @@
 
 	
 #if DEBUG == 1
+
+//电机回传数据结构体
 motor_measure_t motor_chassis[5];
+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];
 #endif
@@ -92,6 +98,41 @@ static uint8_t              can_send_data_200[8];
 CAN_RxHeaderTypeDef dji_rx_header;
 uint8_t dji_rx_data[8];
 
+static CAN_TxHeaderTypeDef vesc_tx_message;
+static uint8_t  vesc_send_data[4];
+
+
+/**
+ * @brief         解析vesc电机反馈数据
+ * @param[in,out] feedback  :  Vesc电机反馈数据结构体指针
+ * @param[in]     raw       :  Vesc电机反馈数据的原始数据
+ * @retval        none
+ */
+static void CAN_VescMotor_Decode_1(CAN_MotorFeedback_t *feedback,
+  const uint8_t *raw)
+{
+  if (feedback == NULL || raw == NULL) return;												 
+  union 
+  {
+  int x;
+  uint8_t data[4];
+  }speed;
+
+  speed.data[0]= raw[3];
+  speed.data[1]= raw[2];
+  speed.data[2]= raw[1];
+  speed.data[3]= raw[0];
+  feedback->rotor_speed = speed.x;
+  union
+  {
+  int16_t y;
+  uint8_t dat[2];
+  }current;
+  current.dat[0]= raw[5];
+  current.dat[1]= raw[4];
+  feedback->torque_current =(fp32)current.y/10;
+}
+
 /**
  * @brief           数据处理函数
  * @param[in]       none
@@ -101,13 +142,10 @@ void djiMotorEncode()
 {
     switch (dji_rx_header.StdId)
     {
-    case 0x201:
+    case M3508_1:
-    case 0x202:
+    case M3508_2:
-    case 0x203:
+    case M3508_3:
-    case 0x204:
+    case M3508_4:
-    case 0x205:
-    case 0x206:
-    case 0x207:
     {
         static uint8_t i = 0;
         //get motor id
@@ -121,6 +159,36 @@ void djiMotorEncode()
         }
         break;
     }
+    case M2006_1:
+    case M2006_2:
+    {
+        static uint8_t i = 0;
+        //get motor id
+        i = dji_rx_header.StdId - 0x205;
+        if(motor_chassis[i].msg_cnt<=50)
+        {
+            motor_chassis[i].msg_cnt++;
+            get_motor_offset(&motor_chassis[i], dji_rx_data);
+        }else{
+          get_2006_motor_measure(&motor_chassis[i], dji_rx_data);
+        }
+        break;
+    }
+    case GM6020_1:
+    case GM6020_2:
+    {
+        static uint8_t i = 0;
+        //get motor id
+        i = dji_rx_header.StdId - 0x207;
+        if(GM6020_motor[i].msg_cnt<=50)
+        {
+          GM6020_motor[i].msg_cnt++;
+            get_motor_offset(&GM6020_motor[i], dji_rx_data);
+        }else{
+          get_6020_motor_measure(&GM6020_motor[i], dji_rx_data);
+        }
+        break;
+    }
 
             
     default:
@@ -129,6 +197,26 @@ void djiMotorEncode()
     }
     }
 }
+
+void vescMotorEncode()
+{
+  switch (dji_rx_header.ExtId) {
+    
+    case CAN_VESC5065_M1_MSG1:
+      // 存储消息到对应的电机结构体中
+      CAN_VescMotor_Decode_1(&motor_5065[0], dji_rx_data);
+      break;
+
+    case CAN_VESC5065_M2_MSG1:
+      // 存储消息到对应的电机结构体中
+      CAN_VescMotor_Decode_1(&motor_5065[1], dji_rx_data);
+      break;
+      
+    default:
+      break;
+  }
+
+}
 #if FREERTOS_DJI == 0
 /**
   * @brief          hal库CAN回调函数,接收电机数据
@@ -232,7 +320,7 @@ void CAN_cmd_200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,
 }
 
 /**
-  * @brief          发送电机控制电流(0x205,0x206,0x207,0x208)
+  * @brief          发送电机控制电流(0x209,0x20A,0x20B)
   * @param[in]      motor1: (0x209) 电机控制电流
   * @param[in]      motor2: (0x20A) 电机控制电流
   * @param[in]      motor3: (0x20B) 电机控制电流
@@ -241,7 +329,7 @@ void CAN_cmd_200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,
 void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeDef*hcan)
 {
     uint32_t send_mail_box;
-    tx_meessage_2ff.StdId = 0x1FF;
+    tx_meessage_2ff.StdId = 0x2FF;
     tx_meessage_2ff.IDE = CAN_ID_STD;
     tx_meessage_2ff.RTR = CAN_RTR_DATA;
     tx_meessage_2ff.DLC = 0x08;
@@ -265,4 +353,83 @@ motor_measure_t *get_motor_point(uint8_t i)
 {
     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电机转速
+   * @param[in/out]  rpm:  vesce电机转速
+   * @retval         none
+   *
+   * 如果rpm的绝对值大于wtrcfg_VESC_COMMAND_ERPM_MAX,
+   * 则将rpm的值设置为wtrcfg_VESC_COMMAND_ERPM_MAX或-wtrcfg_VESC_COMMAND_ERPM_MAX
+   */
+  void assert_param_rpm(float *rpm){
+    if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX )
+      *rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ;
+  }
+  
+  /**
+    * @brief          使vesc电机进入转速模式
+    * @param[in]      controller_id: vesc电机控制器id
+    * @param[in]      RPM: 电机转速
+    * @retval         RPM（1000-50000之间的数）
+    */
+  void  CAN_VESC_RPM(uint8_t controller_id, float RPM)
+  {
+        uint32_t id;
+        int32_t data;
+        uint32_t send_mail_box;
+    
+        id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8);
+        assert_param_rpm(&RPM);
+        data = (int32_t)(RPM);
+    
+        vesc_tx_message.ExtId = id;
+        vesc_tx_message.IDE = CAN_ID_EXT;
+        vesc_tx_message.RTR = CAN_RTR_DATA;
+        vesc_tx_message.DLC = 0x04;
+        vesc_send_data[0] = data >> 24 ;
+        vesc_send_data[1] = data >> 16 ;
+        vesc_send_data[2] = data >> 8 ;
+        vesc_send_data[3] = data ;
+    
+        HAL_CAN_AddTxMessage(&hcan1, &vesc_tx_message, vesc_send_data, &send_mail_box);
+  }
+  
+  
+  /**
+    * @brief          使vesc电机进入制动模式
+    * @param[in]      controller_id: vesc电机控制器id
+    * @retval         none
+    */
+  void  CAN_VESC_HEAD(uint8_t controller_id)
+  {
+        uint32_t id;
+        uint32_t send_mail_box;
+    
+        id = controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8);
+    
+        vesc_tx_message.ExtId = id;
+        vesc_tx_message.IDE = CAN_ID_EXT;
+        vesc_tx_message.RTR = CAN_RTR_DATA;
+        vesc_tx_message.DLC = 0x04;
+    
+        HAL_CAN_AddTxMessage(&hcan1, &vesc_tx_message, vesc_send_data, &send_mail_box);
+}

User/device/djiMotor.h

@@ -16,6 +16,18 @@ typedef enum{
   M2006 = 2
 }motor_type_e;
 
+ enum{
+  M3508_1 = 0x201,
+  M3508_2 = 0x202,
+  M3508_3 = 0x203,
+  M3508_4 = 0x204,
+  M2006_1 = 0x205,
+  M2006_2 = 0x206,
+  GM6020_1 = 0x207,
+  GM6020_2 = 0x208,
+  CAN_VESC5065_M1_MSG1 =0x90a,           //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
+	CAN_VESC5065_M2_MSG1 =0x90b,
+};
 
 //rm motor data
 typedef struct
@@ -45,13 +57,40 @@ typedef struct
 } odrive_measure_t;
 
 
+typedef enum {
+	CAN_PACKET_SET_DUTY = 0,
+	CAN_PACKET_SET_CURRENT = 1,
+	CAN_PACKET_SET_CURRENT_BRAKE =2,
+	CAN_PACKET_SET_RPM = 3,
+	CAN_PACKET_SET_POS = 4,
+	CAN_PACKET_FILL_RX_BUFFER = 5,
+	CAN_PACKET_FILL_RX_BUFFER_LONG = 6,
+	CAN_PACKET_PROCESS_RX_BUFFER = 7,
+	CAN_PACKET_PROCESS_SHORT_BUFFER = 8,
+	CAN_PACKET_STATUS = 9,
+	CAN_PACKET_SET_CURRENT_REL = 10,
+	CAN_PACKET_SET_CURRENT_BRAKE_REL = 11,
+	CAN_PACKET_SET_CURRENT_HANDBRAKE = 12,
+	CAN_PACKET_SET_CURRENT_HANDBRAKE_REL = 13
+} CAN_PACKET_ID;
 //motor calc ecd to angle 
 
+/* 电机反馈信息 */
+typedef struct {
+  float rotor_ecd;
+  float rotor_speed;
+  float torque_current;
+  float temp;
+} CAN_MotorFeedback_t;
+
 #define MOTOR_ECD_TO_ANGLE_3508  (360.0 / 8191.0 / (3591.0/187.0))
 #define MOTOR_ECD_TO_ANGLE_2006 (360.0 / 8191.0 / 36)
 #define MOTOR_ECD_TO_RAD 0.000766990394f
 #define MOTOR_ECD_TO_ANGLE_6020  (360.0 / 8191.0 )
 
+#define wtrcfg_VESC_COMMAND_ERPM_MAX 35000
+#define CAN_VESC_CTRL_ID_BASE (0x300)
+
 #if FREERTOS_DJI == 1
 
 /**
@@ -102,6 +141,19 @@ 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           数据处理函数
@@ -110,6 +162,12 @@ extern motor_measure_t *get_motor_point(uint8_t i);
 */
 void djiMotorEncode(void);
 
+void vescMotorEncode(void);
+
+void  CAN_VESC_RPM(uint8_t controller_id, float RPM);
+
+void  CAN_VESC_HEAD(uint8_t controller_id);
+
 #ifdef __cplusplus
 }
 #endif

User/device/nuc.c

@@ -1,6 +1,6 @@
 #include "nuc.h"
 #include <string.h>
-//#include "crc16.h"
+#include "crc_ccitt.h"
 
 uint8_t nucbuf[32];
 
@@ -32,60 +32,77 @@ int8_t NUC_Restart(void) {
   return DEVICE_OK;
 }
 
+
 bool_t NUC_WaitDmaCplt(void) {
   // return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) ==
   //         SIGNAL_NUC_RAW_REDY);
+  return 1;
 }
 
-int8_t NUC_RawParse(NUC_t *n){
+int8_t NUC_RawParse(NUC_t *n) {
-	if(n ==NULL) return DEVICE_ERR_NULL;
+  if (n == NULL) return DEVICE_ERR_NULL;
- 	union 
+  union {
-	{
+      float x[3];
-		float x[3];
+      char data[12];
-		char data[12];
+  } instance; // 方便在浮点数和字符数组之间进行数据转换
-	}instance;//方便在浮点数和字符数组之间进行数据转换
 
-  if(nucbuf[0]!=HEAD) goto error;  //发送ID不是底盘
+  // 校验数据包头
-  else{
+  if (nucbuf[0] != HEAD) {
-    n->status_fromnuc =nucbuf[1];
+      return DEVICE_ERR;
-		n->ctrl_status =nucbuf[2];
-    switch (n->status_fromnuc)
-    {
-		case VISION:
-				/* 协议格式 
-		   0xFF  HEAD
-		   0x0X  控制帧
-		   0x01  相机帧
-		   x    fp32
-		   y    fp32
-		   z    fp32
-		   0xFE  TAIL
-		  */
-		if(nucbuf[15]!=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];//
-       instance.data[11] = nucbuf[14];
-       instance.data[10] = nucbuf[13];
-       instance.data[9] = nucbuf[12];
-       instance.data[8] = nucbuf[11];
-      n->vision.z = instance.x[2];//
-      break;
-    case PICK:
-		   break;
-    }
-     return DEVICE_OK;
   }
-    error:
+  // 提取数据包中的 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;
 }
 
 

User/device/nuc.h

@@ -13,7 +13,7 @@
 #define ODOM (0x04)
 #define PICK (0x06)
 
-#define VISION (0x01)
+#define VISION (0x09)
 
 
 //写结构体存入视觉信息
@@ -27,9 +27,9 @@ typedef struct{
 
     struct
     {
-        float x;
+        fp32 x;
-        float y;
+        fp32 y;
-        float z;
+        fp32 z;
     }vision;
 
 }NUC_t;

User/module/ball.c

@@ -3,10 +3,18 @@
 #include "cmsis_os.h"
 #include "FreeRTOS.h"
 #include "bsp_delay.h"
+#include "go.h"
 
+// PC6  ball_up_Pin读运球
+// PE13 down_Pin 下推
+// PE14 paw_Pin  爪子
+// PI6 ball_in1_Pin 读接球
+// PI7 ball_in2_Pin 读接球
+// PE11 key_Pin 收网
 #define ballcome 1
 #define balldown 0
 
+extern GO go;
 //光电识别
 int ball_in(void) 
 {
@@ -39,32 +47,32 @@ int pass_ball(void)
 
 
 //运球
-void handling_ball(int hand, int paw) 
+void handling_ball(int hand, int paw,int angle) 
 {
-    paw =ball_in();
+    GO_TURN_ball(TURN,go,angle);//停止运球
+    paw =ball_in();//获取爪子有无球状态
     if (hand == 1 && paw == 1)
     {
         //爪子
         delay_us(100);
-        HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET);
-        osDelay(2);
-        delay_us(100);
-        //上推
-        HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET);
-        hand=0;
-    }
-    else
-    {
-        
-        //爪子
         HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_SET);
         osDelay(2);
         delay_us(100);
-        //下推
+        //下推  推下立马收回
         HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET);
+        osDelay(2);
+        HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET);
+
+        hand=0;//清除手指状态
     }
+    else if(hand == 0 && paw == 1)
+    {
         
+        //闭合爪子
+        HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET);
+        osDelay(2);
 
+    }
     
 
 }
@@ -77,10 +85,13 @@ int catch_ball(int inball)
     inball =pass_ball();
     if(inball==1)
     {
+        //接球
+        HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_SET);
         return ballcome;
     }
     else
     {
+        HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_RESET);
         return balldown;
     }
 

User/module/ball.h

@@ -12,7 +12,7 @@ typedef struct ball
 
 int ball_in(void);
 int pass_ball(void);
-void handling_ball(int hand, int paw);
+void handling_ball(int hand, int paw,int angle);
 int catch_ball(int inball);
 
 

User/module/dji.c

@@ -15,8 +15,6 @@ pid_type_def speed_pid;//3508速度环pid结构体
 pid_type_def angle_pid;//3508位置环pid结构体
 
 //pid
-//fp32 M3508_speed_PID[3]={10.0,0.3,0.0};	//P,I,D(速度环)
-//fp32 M3508_angle_PID[3]={10.0,0.0,1.5};	//P,I,D(角度环)
 
 fp32 M3508_speed_PID[3]={5.0,0.3,0.0};	//P,I,D(速度环)
 fp32 M3508_angle_PID[3]={25.0,0.0,1.5};	//P,I,D(角度环)
@@ -30,6 +28,7 @@ float angleSet[MOTOR_NUM]; //位置环
 //编码器数据
 const motor_measure_t *trigger_motor_data;//3508电机数据
 
+
 pid_type_def t_speed_pid;//2006速度环pid结构体
 pid_type_def t_angle_pid;//2006位置环pid结构体
 
@@ -41,14 +40,24 @@ fp32  M2006_PID[3]={4.9,0.01,0.0};
 int16_t t_result; //速度环 //can最后发送的数据
 float t_angleSet[1]; //位置环
 
+//GM6020电机数据
+const motor_measure_t *GM6020_motor_data;//3508电机数据
+
+//6020pid
+pid_type_def speed_6020_pid;
+pid_type_def angle_6020_pid;
+
+const fp32 PID_6020_angle[3] = {50, 0.1, 0};
+const fp32 PID_6020_speed[3]={5,0.01,0};
+
 
 
 void motor_init(void)
 {
 //   motor_3508_data=get_motor_point(0);
     trigger_motor_data=get_motor_point(0);
-//    PID_init(&speed_pid,PID_POSITION,M3508_speed_PID,3000,1000);
+    GM6020_motor_data=get_6020_motor_point(0);
-//    PID_init(&angle_pid,PID_POSITION,M3508_angle_PID,7000,2000);
+
 	
 	  // PID_init(&speed_pid,PID_POSITION,M3508_speed_PID,500000,500000);
     // PID_init(&angle_pid,PID_POSITION,M3508_angle_PID,500000,100000);
@@ -56,6 +65,11 @@ void motor_init(void)
     PID_init(&t_speed_pid,PID_POSITION,M2006_speed_PID,500000,500000);
     PID_init(&t_angle_pid,PID_POSITION,M2006_angle_PID,500000,100000);
 
+
+    PID_init(&speed_6020_pid,PID_POSITION,PID_6020_speed,15000,15000);//6020 pid初始化
+    PID_init(&angle_6020_pid,PID_POSITION,PID_6020_angle,600,500);
+
+
 }
 
 float trigger_angle = 0;
@@ -67,6 +81,9 @@ void trigger_pos(fp32 angle)
   delta[0]=PID_calc(&t_angle_pid,trigger_motor_data->total_angle,angle);
   t_result=PID_calc(&t_speed_pid,trigger_motor_data->speed_rpm,delta[0]);
 
+   CAN_cmd_200(t_result,0,0,0,&hcan1);
+   osDelay(2);
+
 }
 
    
@@ -86,3 +103,24 @@ void motor_pos(fp32 angle)
 
 }
 
+//6020控制
+void my_GM6020_control(fp32 angle)
+{
+
+    fp32 angle_get;
+	  fp32 my_angle;
+	  int32_t M6020_speed;
+
+    //6020串级
+    angle_get= angle;
+
+    //外环角度环
+    my_angle=PID_calc(&angle_6020_pid,GM6020_motor_data->total_angle,angle_get);//角度环
+    //内环速度环	
+    M6020_speed=PID_calc(&speed_6020_pid,GM6020_motor_data->speed_rpm, my_angle);//速度环
+
+    CAN_cmd_1FF(M6020_speed, M6020_speed, M6020_speed, M6020_speed,&hcan1);
+    osDelay(2);
+
+}
+

User/module/dji.h

@@ -20,5 +20,6 @@ typedef enum
     void motor_pos(fp32 angle);
 
     void trigger_pos(fp32 angle);
+    void my_GM6020_control(fp32 angle);
 
 #endif

User/module/go.c

@@ -4,7 +4,7 @@
 #include "calc_lib.h"
 #include "FreeRTOS.h"
 #include <cmsis_os2.h>
-#define KP 0.12
+#define KP 0.9
 #define KD 0.008
 //可活动角度
 #define ANGLE_ALLOW 0.3f
@@ -13,7 +13,7 @@ extern RC_mess_t RC_mess;
 GO go;
 
 //初始化            
-void gimbalInit(void)
+void GO_Init(void)
 {
 		int i;
     GO_M8010_init();
@@ -38,22 +38,30 @@ void gimbalInit(void)
 
 void gimbalFlow(void)
 {
-    //用485模块发送数据有问题
+	go.angleSetgo[0]=1;
-    GO_M8010_send_data(&huart1, 0,0,0,go.angleSetgo[0],1,KP,KD);
    
     //串口6发送数据没有问题
-    GO_M8010_send_data(&huart6, 0,0,0,go.angleSetgo[0],1,KP,KD);
+  //  GO_M8010_send_data(&huart6, 0,0,0,go.angleSetgo[0],1,KP,KD);
     GO_M8010_send_data(&huart6, 1,0,0,go.angleSetgo[0],1,KP,KD);
-    GO_M8010_send_data(&huart6, 2,0,0,go.angleSetgo[0],1,KP,KD);
+//    GO_M8010_send_data(&huart6, 2,0,0,go.angleSetgo[0],1,KP,KD);
 		osDelay(1);
   
   
 }
 
-
 void gimbalZero(void)
 {
-    GO_M8010_send_data(&huart1, 0,0,0,0,0,0,0);
+    GO_M8010_send_data(&huart6, 0,0,0,0,0,0,0);
 		osDelay(1);
-    GO_M8010_send_data(&huart1, 1,0,0,0,0,0,0);
+    GO_M8010_send_data(&huart6, 1,0,0,0,0,0,0);
+}
+
+void GO_TURN_ball(GO_ID_t go_id,GO go_set,int angle)
+{
+
+  go_set.angleSetgo[go_id] = angle;
+  go_set.Kp = KP;
+  go_set.Kd = KD;
+  GO_M8010_send_data(&huart6, go_id,0,0,go_set.angleSetgo[go_id],1,go_set.Kp,go_set.Kd);
+
 }

User/module/go.h

@@ -3,6 +3,12 @@
 
 #include "GO_M8010_6_Driver.h"
 
+typedef enum{
+    TURN = 0,
+    GIMBAL = 1,
+    OTHER = 2
+  }GO_ID_t;
+
 typedef struct 
 {
     /* data */
@@ -16,9 +22,10 @@ typedef struct
 	float allowRange;
 }GO;
 
-void gimbalInit(void);
+void GO_Init(void);
 void gimbalFlow(void);
 void gimbalZero(void);
+void GO_TURN_ball(GO_ID_t go_id,GO go_set,int angle);
 
 
 #endif

User/task/ball_task.c

@@ -9,6 +9,8 @@
 extern RC_mess_t RC_mess;
 
 BASKETBALL basketball;
+int bb=0;
+int aa=8;
 void Task_Ball(void *argument) 
 {
     (void)argument; /* 未使用argument，消除警告 */
@@ -20,7 +22,16 @@ void Task_Ball(void *argument)
     while(1)
     {
 
-        handling_ball(basketball.hand, basketball.paw);
+     //   handling_ball(basketball.hand, basketball.paw,aa);
+        if(bb==1)
+        {
+            HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_SET);
+        }
+        else
+        {
+            HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_RESET);
+        }
+     
 
         tick += delay_tick; /* 计算下一个唤醒时刻 */
         osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */

User/task/dji_task.c

@@ -20,6 +20,7 @@ int speed=0;
 float m=0;
 float trigger_angle_o=0;
 int mode=0;
+int angle_6020=0;
 /**
  * \brief 电机任务
  *
@@ -38,39 +39,40 @@ void Task_Motor(void *argument)
 
    while(1)
   {
-    //收到消息队列新数据
+  //   //收到消息队列新数据
   
-    // 更新编码器数据
+  //   // 更新编码器数据
-    Update_Encoder(&encoder);
+  //   Update_Encoder(&encoder);
-		m=trigger_angle_o*(8191/360);
+	// 	m=trigger_angle_o*(8191/360);
-    trigger_angle_o=-2.5;//
+  //   trigger_angle_o=-2.5;//
-    if( mode == THREE )
+  //   if( mode == THREE )
-    {
+  //   {
 
-      //当最高点时进入离合
+  //     //当最高点时进入离合
-      if(encoder.round_cnt>=6)
+  //     if(encoder.round_cnt>=6)
-      {
+  //     {
-        trigger_angle_o=-2.5;
+  //       trigger_angle_o=-2.5;
-        trigger_pos(m);
+  //       trigger_pos(m);
-      }
+  //     }
-      else
+  //     else
-      {
+  //     {
-        trigger_angle_o=0;
+  //       trigger_angle_o=0;
-        trigger_pos(m);
+  //       trigger_pos(m);
-      }
+  //     }
 
-    }
+  //   }
-  else if( mode == DZ )
+  // else if( mode == DZ )
-    {
+  //   {
-      //退出离合
+  //     //退出离合
-      trigger_angle_o=0;
+  //     trigger_angle_o=0;
-      trigger_pos(m);
+  //     trigger_pos(m);
 
-    }
+  //   }
  
+  //   my_GM6020_control(angle_6020);
 
-    CAN_cmd_200(t_result,0,0,0,&hcan1);
+  //   CAN_cmd_200(t_result,0,0,0,&hcan1);//在module写好
-    osDelay(2);
+  //   osDelay(2);
 
 //     vofa[0]=motor_3508_data->speed_rpm;
 //     vofa[1]=speed;

User/task/go_task.c

@@ -16,7 +16,7 @@ void Task_Go(void *argument)
 	#if GOUSE==1
 
 	HAL_Delay(2000);
-	gimbalInit();
+	GO_Init();
 	HAL_GPIO_WritePin(LED_G_GPIO_Port, LED_G_Pin,GPIO_PIN_RESET);
 
 	while(1)
@@ -24,7 +24,7 @@ void Task_Go(void *argument)
           
 		//LED_green();
 		//LED_bule();
-        gimbalFlow();
+    //    gimbalFlow();
         osDelay(1);
 	}
 	#else

User/task/shoot_task.c

@@ -7,15 +7,22 @@
 #include "shoot.h"
 #include "odrive_shoot.h"
 #include "read_spi.h"
-#include "dji_task.h"
+#include "dji.h"
+#include "go.h"
 
 extern RC_mess_t RC_mess;
 extern Encoder_t encoder;
 extern motor_measure_t *trigger_motor_data;//3508电机数据
 extern int mode;
+extern GO go;
+int shoot_flag = 0;
+int trigger_flag = 0;
+int pos=0;
+int gopos=0;
 
 #define GO1_SHOOT 0
-#define ODRIVE_SHOOT 1
+#define ODRIVE_SHOOT 0
+#define VESC_SHOOT 1
 
 //odrive发射
 #define SHOOT3 12
@@ -34,6 +41,9 @@ void Task_Shoot(void *argument)
 
  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
 
+	 go.angleSetgo[0] = 0; //id暂时未知
+	  GO_TURN_ball(TURN,go,gopos); //发射舵机位置
+	osDelay(1000);
     while(1)
     {
      #if GO1_SHOOT == 1
@@ -55,7 +65,32 @@ void Task_Shoot(void *argument)
      
     }
    
+
      #endif
+      #if VESC_SHOOT == 1
+     gopos=35;
+      GO_TURN_ball(TURN,go,gopos); //发射舵机位置
+      if(shoot_flag==1)
+      {
+        CAN_VESC_RPM(0, 10000); //发射电机转速
+        CAN_VESC_RPM(1, 10000); //发射电机转速
+        if(trigger_flag == 1)
+         {
+          trigger_pos(pos); //扳机电机转速
+          trigger_flag = 0;
+         }
+
+
+      }
+      else if(shoot_flag == 0)
+      {
+        CAN_VESC_RPM(0, 0); //发射电机转速
+        CAN_VESC_RPM(1, 0); //发射电机转速
+        trigger_pos(0); //扳机电机转速
+      }
+
+
+      #endif
      tick += delay_tick; /* 计算下一个唤醒时刻 */
      osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */
     }