60 lines
1.4 KiB
C
60 lines
1.4 KiB
C
|
#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;
|
||
|
|
int angle=0;
|
||
|
|
//初始化
|
||
|
|
void gimbalInit(void)
|
||
|
|
{
|
||
|
|
int i;
|
||
|
|
GO_M8010_init();
|
||
|
|
for(i = 0;i < GO_NUM;i ++)
|
||
|
|
{
|
||
|
|
go.goData[i] = getGoPoint(i);//获取电机数据指针
|
||
|
|
// PID_init(&angle_pid[i],PID_POSITION,PID_angle,15,0.5);//pid初始化
|
||
|
|
// PID_init(&speed_pid[i],PID_POSITION,PID_speed,60,0.2);//pid初始化
|
||
|
|
|
||
|
|
// result[i] = 0;
|
||
|
|
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(2000);
|
||
|
|
// 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(2000);
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void gimbalFlow(void)
|
||
|
|
{
|
||
|
|
//用485模块发送数据有问题
|
||
|
|
GO_M8010_send_data(&huart1, 0,0,0,go.angleSetgo[0],10,KP,KD);
|
||
|
|
//串口6发送数据没有问题
|
||
|
|
GO_M8010_send_data(&huart6, 0,0,0,go.angleSetgo[0],10,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);
|
||
|
|
}
|