#include "go.h"
#include "TopDefine.h"
#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 0.3f
extern RC_mess_t RC_mess;

GO go;

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

        go.angleSetgo[i] = 0;
        go.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);
				go.offestAngle[i] = go.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);
				go.offestAngle[i] = go.goData[i]->Pos;
				HAL_Delay(100);
    }
		
}


void gimbalFlow(void)
{
    //用485模块发送数据有问题
    GO_M8010_send_data(&huart1, 0,0,0,go.angleSetgo[0],1,KP,KD);
    //串口6发送数据没有问题
    GO_M8010_send_data(&huart6, 0,0,0,go.angleSetgo[0],1,KP,KD);
		osDelay(1);
  
  
}


void gimbalZero(void)
{
    GO_M8010_send_data(&huart1, 0,0,0,0,0,0,0);
		osDelay(1);
    GO_M8010_send_data(&huart1, 1,0,0,0,0,0,0);
}