#include "odrive_shoot.h"
#include "remote_control.h"
#include "FreeRTOS.h"
#include <cmsis_os2.h>
#include "read_spi.h"

#include "vofa.h"

extern RC_mess_t RC_mess;

extern float angle_encoder;
extern volatile int32_t multi_turn_angle ;
extern volatile uint16_t last_angle ;

#define POS_SET -10.6
#define POS_ZERO 2.705
#define ACCEL_SEND 100//梯形加速度
#define ACCEL_BACK 100//返回时的加速度
#define DELAY 1000

fp32 shoot = 0;
fp32 back_angle = 0;
float vofa_spi[8];

void shootStep(void)
{

  // back_angle=Get_Angle();
  // angle_encoder=AS5047_read(ANGLEUNC);
  // Update_MultiTurn_Angle();

  odrive_accel_cmd(AXIS0_NODE,ACCEL_SEND,ACCEL_SEND); 
  //  osDelay(2);//不知道为什么要延时
  odrive_pos_cmd(AXIS0_NODE,shoot);

	  // vofa_spi[0]=shoot;
    //  vofa_spi[0]=multi_turn_angle;
    //  vofa_tx_main(vofa_spi);

}

void shootBack(void)
{
    odrive_accel_cmd(AXIS0_NODE,ACCEL_BACK,ACCEL_BACK);
		osDelay(2);
    odrive_pos_cmd(AXIS0_NODE,POS_ZERO);

}

