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

NUC_t nuc;
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);
}