#include "shoot_task.h"
#include "TopDefine.h"
#include "FreeRTOS.h"
#include "attrTask.h"
#include <cmsis_os2.h>
#include "remote_control.h"
#include "shoot.h"
#include "odrive_shoot.h"
#include "read_spi.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 0
#define VESC_SHOOT 1

//odrive发射
#define SHOOT3 12
#define TOP 6
#define MIDDLE 3
#define BOTTOM 0

void Task_Shoot(void *argument)
{
    (void)argument; /* 未使用argument，消除警告 */
   const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
   
   #if GO1_SHOOT == 1   
   shooterInit();
   #endif

 uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */

	 go.angleSetgo[0] = 0; //id暂时未知
	  GO_TURN_ball(TURN,go,gopos); //发射舵机位置
	osDelay(1000);
    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;
      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); /* 运行结束，等待下一个周期唤醒 */
    }

}