#include "odrive_shoot.h"
#include "remote_control.h"
#include "FreeRTOS.h"
#include <cmsis_os2.h>
#include "read_spi.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 10//梯形加速度
#define ACCEL_BACK 5//返回时的加速度
#define DELAY 1000

fp32 shoot = 10;


void shootStep(void)
{


  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);


//     osDelay(5000);
//    odrive_accel_cmd(AXIS0_NODE,5,5); 
//   odrive_pos_cmd(AXIS0_NODE,10); 
 // osDelay(DELAY);
//   odrive_accel_cmd(AXIS1_NODE,ACCEL_BACK,ACCEL_BACK);
//   odrive_pos_cmd(AXIS1_NODE,POS_ZERO);

}

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