#include "go.h"
#include "TopDefine.h"
#include "remote_control.h"
#include "calc_lib.h"
#include "FreeRTOS.h"
#include <cmsis_os2.h>
#define KP 0.9
#define KD 0.008
//可活动角度
#define ANGLE_ALLOW 0.3f
extern RC_mess_t RC_mess;

GO go;

//初始化            
void GO_Init(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)
{
	go.angleSetgo[0]=1;
   
    //串口6发送数据没有问题
  //  GO_M8010_send_data(&huart6, 0,0,0,go.angleSetgo[0],1,KP,KD);
    GO_M8010_send_data(&huart6, 1,0,0,go.angleSetgo[0],1,KP,KD);
//    GO_M8010_send_data(&huart6, 2,0,0,go.angleSetgo[0],1,KP,KD);
		osDelay(1);
  
  
}

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

void GO_TURN_ball(GO_ID_t go_id,GO go_set,int angle)
{

  go_set.angleSetgo[go_id] = angle;
  go_set.Kp = KP;
  go_set.Kd = KD;
  GO_M8010_send_data(&huart6, go_id,0,0,go_set.angleSetgo[go_id],1,go_set.Kp,go_set.Kd);

}