94 lines
3.9 KiB
C++
94 lines
3.9 KiB
C++
|
#ifdef __cplusplus
|
||
|
|
extern "C" {
|
||
|
|
#endif
|
||
|
#include "gimbal.h"
|
||
|
|
#include "robot_def.h"
|
||
|
|
#include "dji_motor.h"
|
||
|
|
#include "ins_task.h"
|
||
|
|
#include "message_center.h"
|
||
|
|
#include "general_def.h"
|
||
|
|
#include "bmi088.h"
|
||
|
|
#ifdef __cplusplus
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
#include "matrix.h"
|
||
|
|
#include "robotics.h"
|
||
|
|
|
||
|
|
static attitude_t *gimba_IMU_data; // 云台IMU数据
|
||
|
|
static DJIMotorInstance *yaw_motor, *pitch_motor;
|
||
|
|
|
||
|
|
static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd)
|
||
|
|
static Subscriber_t *gimbal_sub; // cmd控制消息订阅者
|
||
|
|
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
|
||
|
|
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息
|
||
|
|
|
||
|
|
void GimbalInit()
|
||
|
|
{
|
||
|
|
gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
|
||
|
|
// YAW
|
||
|
|
|
||
|
|
|
||
|
|
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
|
||
|
|
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
|
||
|
|
void GimbalTask()
|
||
|
|
{
|
||
|
|
/* Example 1: PUMA560 ------------------------------------------------------------------------*/
|
||
|
|
float m[6] = {0.2645, 0.17, 0.1705, 0, 0, 0};
|
||
|
|
float data[18]={0, -8.5e-2, 0, 0, 0, 0,
|
||
|
|
13.225e-2, 0, 0, 0, 0, 0,
|
||
|
|
0, 3.7e-2, 8.525e-2, 0, 0, 0};
|
||
|
|
Matrixf<3, 6> rc(data);
|
||
|
|
Matrixf<3, 3> I[6];
|
||
|
|
// 获取云台控制数据
|
||
|
|
// 后续增加未收到数据的处理
|
||
|
|
SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
|
||
|
|
|
||
|
|
// @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘
|
||
|
|
// 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref
|
||
|
|
switch (gimbal_cmd_recv.gimbal_mode)
|
||
|
|
{
|
||
|
|
// 停止
|
||
|
|
case GIMBAL_ZERO_FORCE:
|
||
|
|
DJIMotorStop(yaw_motor);
|
||
|
|
DJIMotorStop(pitch_motor);
|
||
|
|
break;
|
||
|
|
// 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
|
||
|
|
case GIMBAL_GYRO_MODE: // 后续只保留此模式
|
||
|
|
DJIMotorEnable(yaw_motor);
|
||
|
|
DJIMotorEnable(pitch_motor);
|
||
|
|
DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
|
||
|
|
DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
|
||
|
|
DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
|
||
|
|
DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
|
||
|
|
DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
|
||
|
|
DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
|
||
|
|
break;
|
||
|
|
// 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
|
||
|
|
case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)
|
||
|
|
DJIMotorEnable(yaw_motor);
|
||
|
|
DJIMotorEnable(pitch_motor);
|
||
|
|
DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
|
||
|
|
DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
|
||
|
|
DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
|
||
|
|
DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
|
||
|
|
DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
|
||
|
|
DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
// 在合适的地方添加pitch重力补偿前馈力矩
|
||
|
|
// 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
|
||
|
|
// ...
|
||
|
|
|
||
|
|
// 设置反馈数据,主要是imu和yaw的ecd
|
||
|
|
gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
|
||
|
|
gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
|
||
|
|
|
||
|
|
// 推送消息
|
||
|
|
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
|
||
|
|
}
|