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

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


#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)
{
    // if(ball)
    // {
    //     if(multi_turn_angle==8)
    //     {
    //         //di
    //         odrive_accel_cmd(AXIS0_NODE,200,200);
    //         odrive_pos_cmd(AXIS0_NODE,0);
    //     }
    //     else if(multi_turn_angle==0)
    //     {
           
    //     }
    // }
             odrive_accel_cmd(AXIS0_NODE,200,200);
             osDelay(2);
             odrive_pos_cmd(AXIS0_NODE,angle);

}

#endif