115 lines
2.1 KiB
C++
115 lines
2.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;
|
|
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
|