63 lines
1.1 KiB
C++
63 lines
1.1 KiB
C++
#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);
|
|
}
|