#include "TopDefine.h"
#include "gimbal.hpp"
#include "remote_control.h"
#include "calc_lib.h"
#include "FreeRTOS.h"
#include <cmsis_os2.h>

#define KP 0.12
#define KD 0.008
//可活动角度
#define ANGLE_ALLOW 1.0f
extern RC_ctrl_t rc_ctrl;
int angle1 = 0;
NUC_t nuc;

const fp32 Gimbal:: Gimbal_speed_PID[3] = {50, 0.1,  0};
const fp32 Gimbal:: Gimbal_angle_PID[3]=  { 5, 0.01, 0};

#if GM6020ING ==1
Gimbal::Gimbal()
{
  GM6020_Motor = get_motor_point(6);
  GM6020_Motor->type = GM6020;
  PID_init(&speed_pid,PID_POSITION,Gimbal_speed_PID,16000, 6000);
  PID_init(&angle_pid,PID_POSITION,Gimbal_angle_PID,5000, 2000);

  result = 0;
  angleSet = 0;

}

void Gimbal::gimbalFlow()
{
  int16_t delta[1];
  angleSet = angle1;
  delta[0] = PID_calc(&angle_pid,GM6020_Motor->total_angle,angleSet);
  result = PID_calc(&speed_pid, GM6020_Motor->speed_rpm, delta[0]);

  CAN_cmd_1FF(0,0,result,0,&hcan1);
  osDelay(1);

}

void Gimbal::gimbalZero()
{
  angleSet=0;
  //gimbalFlow();

}

void Gimbal::gimbalVision(const NUC_t &nuc)
{
    int16_t delta[1];
    angleSet = nuc.vision.x;
    delta[0] = PID_calc(&angle_pid,GM6020_Motor->total_angle,angleSet);
    result = PID_calc(&speed_pid, GM6020_Motor->speed_rpm, delta[0]);

    CAN_cmd_1FF(0,0,result,0,&hcan1);
    osDelay(1);
}



#else
Gimbal::Gimbal()
{

			Kp = KP;
			Kd = KD;
			allowRange = ANGLE_ALLOW;
}   

void Gimbal::gimbalInit(void)
{
		int i;
    GO_M8010_init();
    for(i = 0;i < GO_NUM;i ++)
    {
        goData[i] = getGoPoint(i);//获取电机数据指针

        angleSet[i] = 0;
        offestAngle[i] = 0;
				GO_M8010_send_data(&huart6, i,0,0,0,0,0,0);
				offestAngle[i] = goData[i]->Pos;
				HAL_Delay(100);

    }
		
}

void Gimbal::gimbalFlow(void)
{

    //angleSet[0] = map_fp32((float)rc_ctrl.ch[3],-800.0f,800.0f,-allowRange,allowRange) + offestAngle[0];
    GO_M8010_send_data(&huart6, 0,0,0,angleSet[0],1,KP,KD);
		osDelay(1);


}


void Gimbal::gimbalZero(void)
{
  GO_M8010_send_data(&huart6, 0,0,0,0,0,0,0);
}

void Gimbal::gimbalVision(const NUC_t &nuc)
{
    angleSet[0] = nuc.vision.x;
    GO_M8010_send_data(&huart6, 0,0,0,angleSet[0],1,KP,KD);
    osDelay(1);
}

#endif