#include "shoot.h"
#include "remote_control.h"
#include "go.h"
#include "calc_lib.h"

extern RC_mess_t RC_mess;
extern motor_measure_t *trigger_motor_data;//3508电机数据
extern GO go;

int shoot_flag = 0;
int speed_5065=0;

#define KP 0.12
#define KD 0.008

//sw[1] 306--1694
void control_shoot(void)
{

    if(RC_mess.RC_data.sw[1]>800)
      {

        if(HAL_GPIO_ReadPin(ball_in1_GPIO_Port, ball_in1_Pin) == GPIO_PIN_RESET)
        {
          CAN_VESC_HEAD(1);
          CAN_VESC_HEAD(2);
        }
        else 
        {
          //speed_5065=5000;
          speed_5065=map_fp32(RC_mess.RC_data.ch[2],0,100,0,25000);
          CAN_VESC_RPM(1, speed_5065); //发射电机转速
          CAN_VESC_RPM(2, speed_5065); //发射电机转速
          
        }

      }
      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); //发射电机转速
        osDelay(2000);
        speed_5065=0;
        
      }

      osDelay(1);

}