#include "shoot.h"
#include "remote_control.h"

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


#define GO1_SHOOT 1
#define ODRIVE_SHOOT 0

#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

void shoot_odrive(void)
{
    //扳机
    if(trigger_motor_data->real_angle == 60)
    {
        trigger_pos(0);
    }
    else
    {
        trigger_pos(60);
    }
}

#endif