68 lines
1.5 KiB
C
68 lines
1.5 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.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);
|
|
|
|
}
|