为电机增加了启动和停止模式,完成了gimbal的框架,模式过渡尚未完成
This commit is contained in:
NeoZng
parent
92f376656c
commit
8e32fc0e6f
|
|
@ -207,19 +207,6 @@ void ChassisTask()
|
|||
// 获取新的控制信息
|
||||
SubGetMessage(chassis_sub, &chassis_cmd_recv);
|
||||
|
||||
// 如果出现重要模块离线或遥控器设置为急停,让电机停止
|
||||
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
|
||||
{
|
||||
DJIMotorStop();
|
||||
}
|
||||
|
||||
// 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
|
||||
// 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
|
||||
chassis_vx = chassis_cmd_recv.vx * arm_cos_f32(chassis_cmd_recv.offset_angle) -
|
||||
chassis_cmd_recv.vy * arm_sin_f32(chassis_cmd_recv.offset_angle);
|
||||
chassis_vy = chassis_cmd_recv.vx * arm_sin_f32(chassis_cmd_recv.offset_angle) -
|
||||
chassis_cmd_recv.vy * arm_cos_f32(chassis_cmd_recv.offset_angle);
|
||||
|
||||
// 根据控制模式设定旋转速度
|
||||
switch (chassis_cmd_recv.chassis_mode)
|
||||
{
|
||||
|
|
@ -232,10 +219,20 @@ void ChassisTask()
|
|||
case CHASSIS_ROTATE:
|
||||
// chassis_cmd_recv.wz // 当前维持定值,后续增加不规则的变速策略
|
||||
break;
|
||||
case CHASSIS_ZERO_FORCE:
|
||||
DJIMotorStop(); // 如果出现重要模块离线或遥控器设置为急停,让电机停止
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
// 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
|
||||
// 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
|
||||
chassis_vx = chassis_cmd_recv.vx * arm_cos_f32(chassis_cmd_recv.offset_angle) -
|
||||
chassis_cmd_recv.vy * arm_sin_f32(chassis_cmd_recv.offset_angle);
|
||||
chassis_vy = chassis_cmd_recv.vx * arm_sin_f32(chassis_cmd_recv.offset_angle) -
|
||||
chassis_cmd_recv.vy * arm_cos_f32(chassis_cmd_recv.offset_angle);
|
||||
|
||||
// 根据控制模式进行正运动学解算,计算底盘输出
|
||||
MecanumCalculate();
|
||||
|
||||
|
|
|
|||
|
|
@ -4,22 +4,22 @@
|
|||
#include "ins_task.h"
|
||||
#include "message_center.h"
|
||||
|
||||
// 需要将控制值统一修改为角度制而不是编码器ECD值
|
||||
#define YAW_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机编码器值
|
||||
#define PITCH_HORIZON_ECD 0 // 云台处于水平位置时编码器值
|
||||
|
||||
#define YAW_ALIGN_ECD 0
|
||||
static attitude_t *Gimbal_IMU_data; // 云台IMU数据
|
||||
static dji_motor_instance *yaw_motor; // yaw电机
|
||||
static dji_motor_instance *pitch_motor; // pitch电机
|
||||
|
||||
static attitude_t* Gimbal_IMU_data; // 云台IMU数据
|
||||
static dji_motor_instance* yaw_motor; // yaw电机
|
||||
static dji_motor_instance *pitch_motor; // pitch电机
|
||||
|
||||
static Publisher_t* gimbal_pub;
|
||||
static Publisher_t *gimbal_pub;
|
||||
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给gimbal_cmd的云台状态信息
|
||||
static Subscriber_t* gimbal_sub;
|
||||
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自gimbal_cmd的控制信息
|
||||
|
||||
static Subscriber_t *gimbal_sub;
|
||||
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自gimbal_cmd的控制信息
|
||||
|
||||
void GimbalInit()
|
||||
{
|
||||
Gimbal_IMU_data=INS_Init();
|
||||
Gimbal_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
|
||||
|
||||
// YAW
|
||||
Motor_Init_Config_s yaw_config = {
|
||||
|
|
@ -36,7 +36,9 @@ void GimbalInit()
|
|||
.speed_PID = {
|
||||
|
||||
},
|
||||
.other_angle_feedback_ptr=&Gimbal_IMU_data->YawTotalAngle,
|
||||
.other_angle_feedback_ptr = &Gimbal_IMU_data->YawTotalAngle,
|
||||
// 还需要增加角速度额外反馈指针
|
||||
// .other_speed_feedback_ptr=&Gimbal_IMU_data.wz;
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
|
|
@ -60,6 +62,9 @@ void GimbalInit()
|
|||
.speed_PID = {
|
||||
|
||||
},
|
||||
.other_angle_feedback_ptr = &Gimbal_IMU_data->Pitch,
|
||||
// 还需要增加角速度额外反馈指针
|
||||
// .other_speed_feedback_ptr=&Gimbal_IMU_data.wy,
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
|
|
@ -69,13 +74,74 @@ void GimbalInit()
|
|||
},
|
||||
.motor_type = GM6020};
|
||||
|
||||
yaw_motor=DJIMotorInit(&yaw_config);
|
||||
pitch_motor=DJIMotorInit(&pitch_config);
|
||||
yaw_motor = DJIMotorInit(&yaw_config);
|
||||
pitch_motor = DJIMotorInit(&pitch_config);
|
||||
|
||||
gimbal_pub=PubRegister("gimbal_feed",sizeof(Gimbal_Upload_Data_s));
|
||||
gimbal_sub=SubRegister("gimbal_cmd",sizeof(Gimbal_Ctrl_Cmd_s));
|
||||
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
|
||||
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
|
||||
}
|
||||
|
||||
// /**
|
||||
// * @brief
|
||||
// *
|
||||
// */
|
||||
// static void TransitionMode()
|
||||
// {
|
||||
|
||||
// }
|
||||
|
||||
void GimbalTask()
|
||||
{
|
||||
// 获取云台控制数据
|
||||
// 后续增加未收到数据的处理
|
||||
SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
|
||||
|
||||
// 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref
|
||||
// 是否要增加不同模式之间的过渡?
|
||||
switch (gimbal_cmd_recv.gimbal_mode)
|
||||
{
|
||||
case GIMBAL_ZERO_FORCE:
|
||||
DJIMotorStop();
|
||||
break;
|
||||
case GIMBAL_GYRO_MODE:
|
||||
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);
|
||||
DJIMotorSetRef(pitch_motor,gimbal_cmd_recv.pitch);
|
||||
break;
|
||||
//是否考虑直接让云台和底盘重合,其他模式都使用陀螺仪反馈?
|
||||
case GIMBAL_FREE_MODE:
|
||||
DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, MOTOR_FEED);
|
||||
DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, MOTOR_FEED);
|
||||
DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, MOTOR_FEED);
|
||||
DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, MOTOR_FEED);
|
||||
DJIMotorSetRef(yaw_motor,YAW_ALIGN_ECD);
|
||||
DJIMotorSetRef(pitch_motor,PITCH_HORIZON_ECD);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
// 过渡示例:
|
||||
/* 需要给每个case增加如下判断,并添加一个过渡行为函数和过渡标志位
|
||||
case xxx:
|
||||
if(last_mode!=xxx)
|
||||
{
|
||||
transition_flag=1;
|
||||
}
|
||||
break;
|
||||
|
||||
void TransitMode()
|
||||
{
|
||||
motor_output=lpf_coef * last_output+(1 - lpf_coef) * concur_output;
|
||||
}
|
||||
*/
|
||||
|
||||
// 获取反馈数据
|
||||
gimbal_feedback_data.gimbal_imu_data=*Gimbal_IMU_data;
|
||||
gimbal_feedback_data.yaw_motor_ecd=pitch_motor->motor_measure.ecd;
|
||||
|
||||
// 推送消息
|
||||
PubPushMessage(gimbal_pub,&gimbal_feedback_data);
|
||||
}
|
||||
|
|
@ -53,6 +53,10 @@ typedef enum
|
|||
} App_Status_e;
|
||||
|
||||
// 底盘模式设置
|
||||
/**
|
||||
* @brief 后续考虑修改为云台跟随底盘,而不是让底盘去追云台,云台的惯量比底盘小.
|
||||
*
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
CHASSIS_ZERO_FORCE, // 电流零输入
|
||||
|
|
@ -65,9 +69,8 @@ typedef enum
|
|||
typedef enum
|
||||
{
|
||||
GIMBAL_ZERO_FORCE, // 电流零输入
|
||||
GIMBAL_FREE_MODE, // 云台自由运动模式，反馈值为电机total_angle
|
||||
GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式，反馈值为陀螺仪pitch,total_yaw_angle
|
||||
GIMBAL_VISUAL_MODE, // 视觉模式，反馈值为陀螺仪，输入为视觉数据
|
||||
GIMBAL_FREE_MODE, // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle
|
||||
GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
|
||||
} gimbal_mode_e;
|
||||
|
||||
// 发射模式设置
|
||||
|
|
@ -173,6 +176,7 @@ typedef struct
|
|||
typedef struct
|
||||
{
|
||||
attitude_t gimbal_imu_data;
|
||||
uint16_t yaw_motor_ecd;
|
||||
} Gimbal_Upload_Data_s;
|
||||
|
||||
typedef struct
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@
|
|||
|
||||
typedef struct
|
||||
{
|
||||
// 还需要增加角速度数据
|
||||
float Roll;
|
||||
float Pitch;
|
||||
float Yaw;
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
#define ECD_ANGLE_COEF 0.043945f // 360/8192
|
||||
|
||||
static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
|
||||
static uint8_t stop_flag=0;
|
||||
static uint8_t stop_flag = 0;
|
||||
|
||||
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
|
||||
static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL};
|
||||
|
|
@ -123,7 +123,7 @@ static void DecodeDJIMotor(can_instance *_instance)
|
|||
{
|
||||
// 由于需要多次变址访存,直接将buff和measure地址保存在寄存器里避免多次存取
|
||||
static uint8_t *rxbuff;
|
||||
static dji_motor_measure *measure;
|
||||
static dji_motor_measure *measure;
|
||||
|
||||
for (size_t i = 0; i < DJI_MOTOR_CNT; i++)
|
||||
{
|
||||
|
|
@ -169,7 +169,7 @@ dji_motor_instance *DJIMotorInit(Motor_Init_Config_s *config)
|
|||
MotorSenderGrouping(&config->can_init_config);
|
||||
// register motor to CAN bus
|
||||
config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback
|
||||
dji_motor_info[idx]->motor_can_instance=CANRegister(&config->can_init_config);
|
||||
dji_motor_info[idx]->motor_can_instance = CANRegister(&config->can_init_config);
|
||||
|
||||
return dji_motor_info[idx++];
|
||||
}
|
||||
|
|
@ -246,7 +246,7 @@ void DJIMotorControl()
|
|||
group = motor->sender_group;
|
||||
num = motor->message_num;
|
||||
sender_assignment[group].tx_buff[num] = 0xff & set >> 8;
|
||||
sender_assignment[group].tx_buff[2*num + 1] = 0xff & set;
|
||||
sender_assignment[group].tx_buff[2 * num + 1] = 0xff & set;
|
||||
}
|
||||
else // 遇到空指针说明所有遍历结束,退出循环
|
||||
break;
|
||||
|
|
@ -257,9 +257,9 @@ void DJIMotorControl()
|
|||
{
|
||||
if (sender_enable_flag[i])
|
||||
{
|
||||
if(stop_flag) // 如果紧急停止,将所有发送信息置零
|
||||
if (stop_flag) // 如果紧急停止,将所有发送信息置零
|
||||
{
|
||||
memset(sender_assignment[i].tx_buff,0,8);
|
||||
memset(sender_assignment[i].tx_buff, 0, 8);
|
||||
}
|
||||
CANTransmit(&sender_assignment[i]);
|
||||
}
|
||||
|
|
@ -268,5 +268,10 @@ void DJIMotorControl()
|
|||
|
||||
void DJIMotorStop()
|
||||
{
|
||||
stop_flag=1;
|
||||
stop_flag = 1;
|
||||
}
|
||||
|
||||
void DJIMotorEnable()
|
||||
{
|
||||
stop_flag = 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -111,4 +111,11 @@ void DJIMotorControl();
|
|||
*/
|
||||
void DJIMotorStop();
|
||||
|
||||
/**
|
||||
* @brief 启动所有电机,此时电机会响应设定值
|
||||
* 初始化时不需要此函数,因为stop_flag的默认值为0
|
||||
*
|
||||
*/
|
||||
void DJIMotorEnable();
|
||||
|
||||
#endif // !DJI_MOTOR_H
|
||||
|
|
|
|||
|
|
@ -60,10 +60,14 @@ typedef struct
|
|||
} Motor_Control_Setting_s;
|
||||
|
||||
/* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/
|
||||
// 后续增加前馈数据指针
|
||||
typedef struct
|
||||
{
|
||||
float *other_angle_feedback_ptr;
|
||||
float *other_speed_feedback_ptr;
|
||||
// float *angle_foward_ptr;
|
||||
// float *speed_foward_ptr;
|
||||
// float *current_foward_ptr;
|
||||
|
||||
PID_t current_PID;
|
||||
PID_t speed_PID;
|
||||
|
|
|
|||
Loading…
Reference in New Issue