1

User/device/GO_M8010_6_Driver.c

@@ -46,12 +46,11 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
     if ((Temp_buffer[14] != (crc&0xFF)) || (Temp_buffer[15] != ((crc>>8) & 0xFF)))
     { 
         HAL_GPIO_WritePin(LED_B_GPIO_Port, LED_B_Pin, GPIO_PIN_SET);    //蓝色灯亮      // indicate CRC incorrect
-        HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_G_Pin, GPIO_PIN_SET);      //红色灯灭
         return;
     }
     // CRC pass and start converting data to the motor 
-    HAL_GPIO_WritePin(GPIOH, GPIO_PIN_11, GPIO_PIN_SET);                 // indicate CRC correct
+    HAL_GPIO_WritePin(LED_B_GPIO_Port, LED_B_Pin, GPIO_PIN_RESET);    //蓝色灯亮      // indicate CRC incorrect
-    HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_G_Pin, GPIO_PIN_RESET);      //红色灯灭
+
     GO_M8010_recv_data(Temp_buffer);
 
 }

User/device/djiMotor.c

@@ -118,10 +118,10 @@ static void CAN_VescMotor_Decode_1(CAN_MotorFeedback_t *feedback,
   uint8_t data[4];
   }speed;
 
-  speed.data[0]= raw[3];
+  speed.data[0]= raw[0];
-  speed.data[1]= raw[2];
+  speed.data[1]= raw[1];
-  speed.data[2]= raw[1];
+  speed.data[2]= raw[2];
-  speed.data[3]= raw[0];
+  speed.data[3]= raw[3];
   feedback->rotor_speed = speed.x;
   union
   {
@@ -165,12 +165,12 @@ void djiMotorEncode()
         static uint8_t i = 0;
         //get motor id
         i = dji_rx_header.StdId - 0x205;
-        if(motor_chassis[i].msg_cnt<=50)
+        if(motor_2006[i].msg_cnt<=50)
         {
-            motor_chassis[i].msg_cnt++;
+          motor_2006[i].msg_cnt++;
-            get_motor_offset(&motor_chassis[i], dji_rx_data);
+            get_motor_offset(&motor_2006[i], dji_rx_data);
         }else{
-          get_2006_motor_measure(&motor_chassis[i], dji_rx_data);
+          get_2006_motor_measure(&motor_2006[i], dji_rx_data);
         }
         break;
     }

User/device/djiMotor.h

@@ -25,8 +25,8 @@ typedef enum{
   M2006_2 = 0x206,
   GM6020_1 = 0x207,
   GM6020_2 = 0x208,
-  CAN_VESC5065_M1_MSG1 =0x90a,           //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
+  CAN_VESC5065_M1_MSG1 =0x1001,           //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
-	CAN_VESC5065_M2_MSG1 =0x90b,
+	CAN_VESC5065_M2_MSG1 =0x1002,
 };
 
 //rm motor data

User/device/remote_control.c

@@ -202,6 +202,8 @@ void RC_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num)
 #else
 //DMA双缓冲区+串口空闲中断
 
+// 02 53 44 1x1 3x2 
+
 static osEventFlagsId_t eventReceive;
 
 RC_mess_t RC_mess;

User/module/ball.c

@@ -5,7 +5,7 @@
 #include "bsp_delay.h"
 #include "go.h"
 
-// PC6  ball_up_Pin读运球
+// PC6  ball_up_Pin读运球  检测到输出0,未检测到输出1
 // PE13 down_Pin 下推
 // PE14 paw_Pin  爪子
 // PI6 ball_in1_Pin 读接球
@@ -18,67 +18,46 @@ extern GO go;
 //光电识别
 int ball_in(void) 
 {
-    if (HAL_GPIO_ReadPin(ball_up_GPIO_Port, ball_up_Pin) == GPIO_PIN_SET)
+        return (HAL_GPIO_ReadPin(ball_up_GPIO_Port, ball_up_Pin));
-    {
-        return ballcome;
-    }
-    else
-    {
-        return balldown;
-    }
-
-
 }
+
 int pass_ball(void)
 {
 
-    if (HAL_GPIO_ReadPin(ball_in1_GPIO_Port, ball_in1_Pin) == GPIO_PIN_SET)
-    {
-        return ballcome;
-    }
-    else
-    {
-        return balldown;
-    }
 
+        return (HAL_GPIO_ReadPin(ball_in1_GPIO_Port, ball_in1_Pin));
 }
 
-
-
-
 //运球
-void handling_ball(int hand, int paw,int angle) 
+void handling_ball(int hand, int angle) 
 {
     GO_TURN_ball(TURN,go,angle);//停止运球
-    paw =ball_in();//获取爪子有无球状态
+     int ball_state =0;//有球
-    if (hand == 1 && paw == 1)
+     int paw_state =0;//获取爪子开合状态
+    if( hand==1)
     {
-        //爪子
-        delay_us(100);
         HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_SET);
-        osDelay(2);
+        paw_state =1;//爪子开
-        delay_us(100);
+     //   osDelay(5);
-        //下推  推下立马收回
+        HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET);//打出
-        HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET);
+        ball_state =1;//无球
-        osDelay(2);
+        osDelay(500);
-        HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET);
+        HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET);//收回
-
+        ball_state =ball_in();//获取爪子有无球状态
-        hand=0;//清除手指状态
     }
-    else if(hand == 0 && paw == 1)
+    if (paw_state==1&&ball_state==0)
     {
-        
+        HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET);//爪子合
-        //闭合爪子
+        osDelay(500);
-        HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET);
+        ball_state=1;//有球清除标志
-        osDelay(2);
+        paw_state=0;//爪子合清除标志
+	    hand=0;
 
     }
     
-
+    
 }
 
-
-
 //接球
 int catch_ball(int inball) 
 {

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,int angle);
+void handling_ball(int hand, int angle);
 int catch_ball(int inball);
 
 

User/module/dji.c

@@ -16,14 +16,11 @@ pid_type_def angle_pid;//3508位置环pid结构体
 
 //pid
 
-fp32 M3508_speed_PID[3]={5.0,0.3,0.0};	//P,I,D(速度环)
+fp32 M3508_speed_PID[3]={50.0,0.3,0.0};	//P,I,D(速度环)
-fp32 M3508_angle_PID[3]={25.0,0.0,1.5};	//P,I,D(角度环)
+fp32 M3508_angle_PID[3]={50.0,0.0,1.5};	//P,I,D(角度环)
 //速度环pid参数
 fp32  M3508_PID[3]={4.9,0.01,0.0};
 
-int16_t result; //速度环
-float angleSet[MOTOR_NUM]; //位置环
-
 //trigger
 //编码器数据
 const motor_measure_t *trigger_motor_data;//3508电机数据
@@ -32,14 +29,12 @@ const motor_measure_t *trigger_motor_data;//3508电机数据
 pid_type_def t_speed_pid;//2006速度环pid结构体
 pid_type_def t_angle_pid;//2006位置环pid结构体
 
-fp32 M2006_speed_PID[3]={15.0,0.3,0.0};	//P,I,D(速度环)
+fp32 M2006_speed_PID[3]={10.0,0.1,0.0};	//P,I,D(速度环)
-fp32 M2006_angle_PID[3]={25.0,0.0,1.5};	//P,I,D(角度环)
+fp32 M2006_angle_PID[3]={5.0,0.0,0.05};	//P,I,D(角度环)
 //速度环pid参数
 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电机数据
 
@@ -55,24 +50,22 @@ 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);
+    trigger_motor_data=get_2006_motor_point(0);
     GM6020_motor_data=get_6020_motor_point(0);
 
 	
-	  // PID_init(&speed_pid,PID_POSITION,M3508_speed_PID,500000,500000);
+    PID_init(&t_speed_pid,PID_POSITION,M2006_speed_PID,1600,600);
-    // PID_init(&angle_pid,PID_POSITION,M3508_angle_PID,500000,100000);
+    PID_init(&t_angle_pid,PID_POSITION,M2006_angle_PID,500,200);
-
-    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(&speed_6020_pid,PID_POSITION,PID_6020_speed,16000, 6000);//6020 pid初始化
-    PID_init(&angle_6020_pid,PID_POSITION,PID_6020_angle,600,500);
+    PID_init(&angle_6020_pid,PID_POSITION,PID_6020_angle,5000, 2000);
 
+    t_result=0;
 
 }
 
-float trigger_angle = 0;
+
 void trigger_pos(fp32 angle)
 {
  
@@ -81,27 +74,10 @@ 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);
 
 }
 
-   
-void motor_speed(fp32 speed)
-{
- 
-     result=PID_calc(&speed_pid,motor_3508_data->speed_rpm,speed);
 
-}
-
-void motor_pos(fp32 angle)
-{
-  int16_t delta[1];
-  
-  delta[0]=PID_calc(&angle_pid,motor_3508_data->total_angle,angle);
-  result=PID_calc(&speed_pid,motor_3508_data->speed_rpm,delta[0]);
-
-}
 
 //6020控制
 void my_GM6020_control(fp32 angle)

User/module/dji.h

@@ -15,9 +15,9 @@ typedef enum
 }motorput_e;
 
 
+//float t_angleSet[1]; //位置环
+
     void motor_init(void);
-    void motor_speed(fp32 speed);
-    void motor_pos(fp32 angle);
 
     void trigger_pos(fp32 angle);
     void my_GM6020_control(fp32 angle);

User/module/go.c

@@ -23,11 +23,9 @@ void GO_Init(void)
 
         go.angleSetgo[i] = 0;
         go.offestAngle[i] = 0;
-			//	GO_M8010_send_data(&huart1, i,0,0,0,0,0,0);
         GO_M8010_send_data(&huart6, i,0,0,0,0,0,0);
 				go.offestAngle[i] = go.goData[i]->Pos;
 				HAL_Delay(100);
-			//	GO_M8010_send_data(&huart1, i,0,0,0,0,0,0);
         GO_M8010_send_data(&huart6, i,0,0,0,0,0,0);
 				go.offestAngle[i] = go.goData[i]->Pos;
 				HAL_Delay(100);
@@ -43,7 +41,7 @@ void gimbalFlow(void)
     //串口6发送数据没有问题
   //  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);
   
   

User/module/go.h

@@ -4,8 +4,8 @@
 #include "GO_M8010_6_Driver.h"
 
 typedef enum{
-    TURN = 0,
+    GIMBAL = 0,
-    GIMBAL = 1,
+    TURN = 1,
     OTHER = 2
   }GO_ID_t;
 

User/module/led.c

@@ -4,10 +4,9 @@
 
 void LED_red(void)
 {
-  //GPIO_PIN_12
+   //GPIO_PIN_12
-   //HAL_GPIO_TogglePin(ledBlue_GPIO_Port, ledBlue_Pin);
     HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_SET);
-    osDelay(1000);
+    osDelay(500);
     HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET);
   
 }

User/module/shoot.c

@@ -1,90 +1,14 @@
 #include "shoot.h"
 #include "remote_control.h"
+#include "go.h"
+
 
 extern RC_mess_t RC_mess;
 extern motor_measure_t *trigger_motor_data;//3508电机数据
-
+extern GO go;
-
-#define GO1_SHOOT 0
-#define ODRIVE_SHOOT 1
 
 #define KP 0.12
 #define KD 0.008
 
 
-#if GO1_SHOOT == 1
- GO_SHOOT goshoot;
-
-void shooterInit(void)
-{
-    int i;
-    GO_M8010_init();
-    for(i = 0;i < GO_NUM;i ++)
-    {
-        goshoot.goData[i] = getGoPoint(i);//获取电机数据指针
-
-        goshoot.angleSetgo[i] = 0;
-        goshoot.offestAngle[i] = 0;
-			//	GO_M8010_send_data(&huart1, i,0,0,0,0,0,0);
-        GO_M8010_send_data(&huart6, i,0,0,0,0,0,0);
-				goshoot.offestAngle[i] = goshoot.goData[i]->Pos;
-				HAL_Delay(100);
-			//	GO_M8010_send_data(&huart1, i,0,0,0,0,0,0);
-        GO_M8010_send_data(&huart6, i,0,0,0,0,0,0);
-             goshoot.offestAngle[i] = goshoot.goData[i]->Pos;
-				HAL_Delay(100);
-    }
-
-}
-
-
-void shoot_ball(int key)
-{
-
-
-    //蓄力
-    if(trigger_motor_data->real_angle ==60)//扳机已闭合
-    {
-        goshoot.angleSetgo[0] = 10;
-        GO_M8010_send_data(&huart6, 0,0,0,goshoot.angleSetgo[0],1,KP,KD);
-    }
-    if (key ==2)//上升准备蓄力
-    {
-        goshoot.angleSetgo[0] = 0;
-        GO_M8010_send_data(&huart6, 0,0,0,goshoot.angleSetgo[0],1,KP,KD);   
-    }
-    
-
-}
-
-
-#elif ODRIVE_SHOOT == 1
-//初始位置在最上面, 有球放过来, 电机转动到最下面等待发射
-//发射指令,当到最高点时,扳机离合
-//返回扳机,远动到最下面
-
-extern float angle_encoder;
-extern volatile int32_t multi_turn_angle ;
-extern volatile uint16_t last_angle ;
-
-void shoot_odrive(int angle)
-{
-
-             odrive_accel_cmd(AXIS0_NODE,500,500);
-             osDelay(2);
-             odrive_pos_cmd(AXIS0_NODE,angle);
-
-}
-
-void shoot_back(int angle)
-{
-
-             odrive_accel_cmd(AXIS0_NODE,100,100);
-             osDelay(2);
-             odrive_pos_cmd(AXIS0_NODE,angle);
-
-}
-
-
-#endif
 

User/module/shoot.h

@@ -20,9 +20,6 @@ typedef struct
 	float allowRange;
 }GO_SHOOT;
 
-void shooterInit(void);
+
-void shoot_ball(int key);
-void shoot_odrive(int angle);
-void shoot_back(int angle);
 
 #endif

User/task/ball_task.c

@@ -5,12 +5,15 @@
 #include <cmsis_os2.h>
 #include "remote_control.h"
 #include "ball.h"
+#include "go.h"
+
 
 extern RC_mess_t RC_mess;
+extern GO go;
 
 BASKETBALL basketball;
 int bb=0;
-int aa=8;
+int Turn_pos=0;
 void Task_Ball(void *argument) 
 {
     (void)argument; /* 未使用argument，消除警告 */
@@ -18,18 +21,27 @@ void Task_Ball(void *argument)
 
     uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
         basketball.hand=0;
-			  basketball.paw=0;
+	    Turn_pos=20;
+        GO_TURN_ball(TURN,go,Turn_pos); //发射舵机位置
+        HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET);//收回
     while(1)
     {
 
-     //   handling_ball(basketball.hand, basketball.paw,aa);
+       // Turn_pos=20;
+      
+        handling_ball(basketball.hand,Turn_pos);
+        bb=ball_in();
         if(bb==1)
         {
-            HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_SET);
+       //     HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_SET);
+       //     HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_SET);
+       //     HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET);
         }
         else
         {
-            HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_RESET);
+       //     HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_RESET);
+       //     HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET);
+      //      HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET);
         }
      
 

User/task/can_task.c

@@ -25,7 +25,7 @@ void Task_Can(void *argument)
     {
         waitNewDji();//等待新数据
         djiMotorEncode();//数据解析
-        
+        vescMotorEncode();//数据解析
         //将can数据添加到消息队列
 
 

User/task/dji_task.c

@@ -7,20 +7,17 @@
 #include "dji.h"
 #include "read_spi.h"
 #include "vofa.h"
-
+#include "buzzer.h"
 
 extern RC_mess_t RC_mess;
-extern int16_t result;
+
 extern int16_t t_result;
 extern motor_measure_t *trigger_motor_data;
 Encoder_t encoder;
-float vofa[8];
 
-int speed=0;
+int shoot_f=0;
-float m=0;
+int pos2006=0;
-float trigger_angle_o=0;
+
-int mode=0;
-int angle_6020=0;
 /**
  * \brief 电机任务
  *
@@ -39,44 +36,23 @@ void Task_Motor(void *argument)
 
    while(1)
   {
-  //   //收到消息队列新数据
+
+   if(RC_mess.RC_data.sw[4]==306||shoot_f==3)
+   {
+     pos2006=200;
+     trigger_pos(pos2006);
+     
+   }
+  else if(RC_mess.RC_data.sw[4]==1694)
+   {
+     pos2006=0;
+     trigger_pos(pos2006);
+   }
+
   
-  //   // 更新编码器数据
-  //   Update_Encoder(&encoder);
-	// 	m=trigger_angle_o*(8191/360);
-  //   trigger_angle_o=-2.5;//
-  //   if( mode == THREE )
-  //   {
 
-  //     //当最高点时进入离合
+   CAN_cmd_1FF(t_result,0,0,0,&hcan1);
-  //     if(encoder.round_cnt>=6)
+   osDelay(1);
-  //     {
-  //       trigger_angle_o=-2.5;
-  //       trigger_pos(m);
-  //     }
-  //     else
-  //     {
-  //       trigger_angle_o=0;
-  //       trigger_pos(m);
-  //     }
-
-  //   }
-  // else if( mode == DZ )
-  //   {
-  //     //退出离合
-  //     trigger_angle_o=0;
-  //     trigger_pos(m);
-
-  //   }
- 
-  //   my_GM6020_control(angle_6020);
-
-  //   CAN_cmd_200(t_result,0,0,0,&hcan1);//在module写好
-  //   osDelay(2);
-
-//     vofa[0]=motor_3508_data->speed_rpm;
-//     vofa[1]=speed;
-//     vofa_tx_main(vofa);
 
    tick += delay_tick;
    osDelayUntil(tick);

User/task/go_task.c

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

User/task/led_task.c

@@ -4,17 +4,16 @@
 #include "main.h"
 #include "led.h"
 #include "buzzer.h"
-
+int a1=0;
 void Task_Led(void *argument) {
   (void)argument; /* 未使用argument，消除警告 */
 
-  for (;;) {
+  while(1) {
 
     // LED_red();
-    //  LED_bule();
+    // LED_bule();   
-    // see_you_again();
+		LED_green();
-      LED_green();
+     osDelay(1);
-    
+
-    osDelay(500);
   }
 }

User/task/rc_task.c

@@ -7,6 +7,7 @@
 #include "remote_control.h"
 
 extern RC_mess_t RC_mess;
+// sw[0]2 下306上1694 sw[5]3前306后1694 sw[4]4前1694后306 sw[1]xuan1 sw[3]xuan2 
 /**
  * \brief 遥控器任务
  *

User/task/shoot_task.c

@@ -13,22 +13,16 @@
 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;
+int speed_5065=0;
+
 
-#define GO1_SHOOT 0
-#define ODRIVE_SHOOT 0
 #define VESC_SHOOT 1
 
-//odrive发射
-#define SHOOT3 12
-#define TOP 6
-#define MIDDLE 3
-#define BOTTOM 0
 
 void Task_Shoot(void *argument)
 {
@@ -41,52 +35,47 @@ void Task_Shoot(void *argument)
 
  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
 
-	 go.angleSetgo[0] = 0; //id暂时未知
+	 //go.angleSetgo[0] = 0; //id暂时未知
 	  GO_TURN_ball(TURN,go,gopos); //发射舵机位置
-	osDelay(1000);
+	  osDelay(500);
     while(1)
     {
-     #if GO1_SHOOT == 1
+    
-   
-    shoot_ball(0);
-
-     #elif ODRIVE_SHOOT == 1
-     
-    if(mode == THREE)
-    {
-      shoot_odrive(SHOOT3);    
-    }	
-    else if(mode == DZ)
-    {
-      if(trigger_motor_data->total_angle ==0)//扳机已闭合
-      {
-        shoot_back(DZ);  
-      }
-     
-    }
-   
-
-     #endif
       #if VESC_SHOOT == 1
-     gopos=35;
+      gopos=35;//云台位置
-      GO_TURN_ball(TURN,go,gopos); //发射舵机位置
+      GO_TURN_ball(GIMBAL,go,gopos); //锁云台
-      if(shoot_flag==1)
+
+
+      if(shoot_flag==1||RC_mess.RC_data.sw[0]==306)//加速
       {
-        CAN_VESC_RPM(0, 10000); //发射电机转速
+
-        CAN_VESC_RPM(1, 10000); //发射电机转速
+        if(HAL_GPIO_ReadPin(ball_in1_GPIO_Port, ball_in1_Pin) == GPIO_PIN_RESET)
-        if(trigger_flag == 1)
+        {
-         {
+          CAN_VESC_HEAD(1);
-          trigger_pos(pos); //扳机电机转速
+          CAN_VESC_HEAD(2);
-          trigger_flag = 0;
+        }
-         }
+        else
+        {
+          speed_5065=35000;
+          CAN_VESC_RPM(1, speed_5065); //发射电机转速
+          CAN_VESC_RPM(2, speed_5065); //发射电机转速
+          
+        }
 
 
+      }
+      else if(shoot_flag == 2 || RC_mess.RC_data.sw[5]==1694)//返回//不拨动为306
+      {
+        speed_5065=-5000;
+        CAN_VESC_RPM(1, speed_5065); //发射电机转速
+        CAN_VESC_RPM(2, speed_5065); //发射电机转速
+        
       }
       else if(shoot_flag == 0)
       {
-        CAN_VESC_RPM(0, 0); //发射电机转速
         CAN_VESC_RPM(1, 0); //发射电机转速
-        trigger_pos(0); //扳机电机转速
+        CAN_VESC_RPM(2, 0); //发射电机转速
+       
       }