增加达妙驱动,测试可用，需上车调试
This commit is contained in:
parent
0eaf1bd9d8
commit
28234a395f
|
|
@ -223,14 +223,14 @@ static void RemoteControlSet()
|
|||
if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],小陀螺
|
||||
{
|
||||
shoot_cmd_send.friction_mode = FRICTION_ON;
|
||||
// chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
|
||||
// gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
}
|
||||
else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘跟随云台
|
||||
{
|
||||
shoot_cmd_send.friction_mode = FRICTION_OFF;
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
}
|
||||
|
||||
// 云台参数,确定云台控制数据
|
||||
|
|
|
|||
|
|
@ -6,9 +6,11 @@
|
|||
#include "general_def.h"
|
||||
|
||||
#include "bmi088.h"
|
||||
#include "dm4310.h"
|
||||
|
||||
static attitude_t *gimba_IMU_data; // 云台IMU数据
|
||||
static DJIMotorInstance *yaw_motor, *pitch_motor;
|
||||
static DJIMotorInstance *pitch_motor;
|
||||
static DMMotorInstance *yaw_motor;
|
||||
|
||||
static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd)
|
||||
static Subscriber_t *gimbal_sub; // cmd控制消息订阅者
|
||||
|
|
@ -22,22 +24,23 @@ void GimbalInit()
|
|||
// YAW
|
||||
Motor_Init_Config_s yaw_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan2,
|
||||
.tx_id = 1,
|
||||
.can_handle = &hcan1,
|
||||
.tx_id = 0x01,
|
||||
.rx_id = 0x43,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kp = 4, // 8
|
||||
.Kp = 0.5, // 8
|
||||
.Ki = 0,
|
||||
.Kd = 0.3,
|
||||
.Kd = 0,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.IntegralLimit = 100,
|
||||
|
||||
.MaxOut = 500,
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = 5000, // 70
|
||||
.Ki = 1000, // 200
|
||||
.Kp = 0.5f, // 70
|
||||
.Ki = 0.2f, // 200
|
||||
.Kd = 0,//10
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.IntegralLimit = 3000,
|
||||
|
|
@ -48,13 +51,13 @@ void GimbalInit()
|
|||
.other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2],
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = OTHER_FEED,
|
||||
.speed_feedback_source = OTHER_FEED,
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.outer_loop_type = ANGLE_LOOP,
|
||||
.close_loop_type = ANGLE_LOOP |SPEED_LOOP,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||
},
|
||||
.motor_type = GM6020
|
||||
.motor_type = DM4310
|
||||
};
|
||||
// PITCH
|
||||
Motor_Init_Config_s pitch_config = {
|
||||
|
|
@ -94,7 +97,7 @@ void GimbalInit()
|
|||
.motor_type = M3508,
|
||||
};
|
||||
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
|
||||
yaw_motor = DJIMotorInit(&yaw_config);
|
||||
yaw_motor = DMMotorInit(&yaw_config);
|
||||
pitch_motor = DJIMotorInit(&pitch_config);
|
||||
|
||||
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
|
||||
|
|
@ -118,41 +121,28 @@ void GimbalTask()
|
|||
{
|
||||
// 停止
|
||||
case GIMBAL_ZERO_FORCE:
|
||||
DJIMotorStop(yaw_motor);
|
||||
DMMotorStop(yaw_motor);
|
||||
DJIMotorStop(pitch_motor);
|
||||
break;
|
||||
// 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
|
||||
case GIMBAL_GYRO_MODE: // 后续只保留此模式
|
||||
DJIMotorEnable(yaw_motor);
|
||||
DMMotorEnable(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中处理好多圈和单圈
|
||||
DMMotorSetRef(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;
|
||||
case ADJUST_PITCH_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);
|
||||
horizontal_check();
|
||||
// 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;
|
||||
|
|
|
|||
|
|
@ -199,7 +199,7 @@ typedef struct
|
|||
typedef struct
|
||||
{
|
||||
attitude_t gimbal_imu_data;
|
||||
uint16_t yaw_motor_single_round_angle;
|
||||
float yaw_motor_single_round_angle;
|
||||
float pitch_motor_total_angle;
|
||||
} Gimbal_Upload_Data_s;
|
||||
|
||||
|
|
|
|||
|
|
@ -156,9 +156,8 @@ void ShootTask()
|
|||
{
|
||||
// 停止拨盘
|
||||
case LOAD_STOP:
|
||||
DJIMotorStop(loader);
|
||||
// DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
|
||||
// DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快
|
||||
DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
|
||||
DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快
|
||||
break;
|
||||
// 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
|
||||
case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
|
||||
|
|
|
|||
|
|
@ -168,7 +168,6 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
|
|||
instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等
|
||||
|
||||
// motor controller init 电机控制器初始化
|
||||
PIDInit(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
|
||||
PIDInit(&instance->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
|
||||
PIDInit(&instance->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
|
||||
instance->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
|
||||
|
|
|
|||
|
|
@ -47,9 +47,7 @@ static void DMMotorDecode(CANInstance *motor_can)
|
|||
measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16);
|
||||
|
||||
measure->angle_single_round = ECD_ANGLE_COEF_DM * (measure->position+3.141593);
|
||||
if(measure->angle_single_round > 360) measure->angle_single_round -= 360;
|
||||
if(measure->angle_single_round < 0) measure->angle_single_round += 360 ;
|
||||
|
||||
|
||||
tmp = (uint16_t)((rxbuff[3] << 4) | rxbuff[4] >> 4);
|
||||
measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12);
|
||||
|
||||
|
|
@ -58,6 +56,14 @@ static void DMMotorDecode(CANInstance *motor_can)
|
|||
|
||||
measure->T_Mos = (float)rxbuff[6];
|
||||
measure->T_Rotor = (float)rxbuff[7];
|
||||
|
||||
// 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了
|
||||
if (measure->position - measure->last_position > 3.1425926)
|
||||
measure->total_round--;
|
||||
else if (measure->position - measure->last_position < -3.1415926)
|
||||
measure->total_round++;
|
||||
|
||||
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
|
||||
}
|
||||
|
||||
static void DMMotorLostCallback(void *motor_ptr)
|
||||
|
|
@ -76,12 +82,14 @@ DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
|
|||
DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance));
|
||||
memset(motor, 0, sizeof(DMMotorInstance));
|
||||
|
||||
config->can_init_config.rx_id = config->can_init_config.rx_id;
|
||||
config->can_init_config.tx_id = config->can_init_config.tx_id;
|
||||
motor->motor_settings = config->controller_setting_init_config;
|
||||
PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID);
|
||||
PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID);
|
||||
PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID);
|
||||
motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
|
||||
motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
|
||||
PIDInit(&motor->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
|
||||
PIDInit(&motor->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
|
||||
PIDInit(&motor->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
|
||||
motor->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
|
||||
motor->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
|
||||
|
||||
config->can_init_config.can_module_callback = DMMotorDecode;
|
||||
config->can_init_config.id = motor;
|
||||
|
|
@ -103,7 +111,7 @@ DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
|
|||
|
||||
void DMMotorSetRef(DMMotorInstance *motor, float ref)
|
||||
{
|
||||
motor->pid_ref = ref;
|
||||
motor->motor_controller.pid_ref = ref;
|
||||
}
|
||||
|
||||
void DMMotorEnable(DMMotorInstance *motor)
|
||||
|
|
@ -122,37 +130,69 @@ void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type)
|
|||
}
|
||||
|
||||
|
||||
//位置控制
|
||||
//还得使用力矩控制
|
||||
void DMMotorTask(void const *argument)
|
||||
{
|
||||
float pid_ref, set;
|
||||
float pid_ref, set, pid_measure;
|
||||
|
||||
DMMotorInstance *motor = (DMMotorInstance *)argument;
|
||||
//DM_Motor_Measure_s *measure = &motor->measure;
|
||||
Motor_Controller_s *motor_controller; // 电机控制器
|
||||
DM_Motor_Measure_s *measure = &motor->measure;
|
||||
motor_controller = &motor->motor_controller;
|
||||
Motor_Control_Setting_s *setting = &motor->motor_settings;
|
||||
//CANInstance *motor_can = motor->motor_can_instance;
|
||||
//uint16_t tmp;
|
||||
DMMotor_Send_s motor_send_mailbox;
|
||||
while (1)
|
||||
{
|
||||
pid_ref = motor->pid_ref;
|
||||
pid_ref = motor->motor_controller.pid_ref;//保存设定值
|
||||
|
||||
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
|
||||
pid_ref *= -1;
|
||||
|
||||
// pid_ref会顺次通过被启用的闭环充当数据的载体
|
||||
// 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
|
||||
if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
|
||||
{
|
||||
if (setting->angle_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor_controller->other_angle_feedback_ptr;
|
||||
else
|
||||
pid_measure = measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
|
||||
// 更新pid_ref进入下一个环
|
||||
pid_ref = PIDCalculate(&motor_controller->angle_PID, pid_measure, pid_ref);
|
||||
}
|
||||
|
||||
// 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
|
||||
|
||||
|
||||
if ((setting->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
|
||||
{
|
||||
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
|
||||
pid_ref += *motor_controller->speed_feedforward_ptr;
|
||||
if (setting->speed_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor_controller->other_speed_feedback_ptr;
|
||||
else // MOTOR_FEED
|
||||
pid_measure = measure->velocity;
|
||||
// 更新pid_ref进入下一个环
|
||||
pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
|
||||
}
|
||||
|
||||
if (setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
|
||||
pid_ref *= -1;
|
||||
|
||||
set = pid_ref;
|
||||
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
|
||||
set *= -1;
|
||||
|
||||
if (motor->stop_flag == MOTOR_STOP)
|
||||
set = 0;
|
||||
|
||||
LIMIT_MIN_MAX(set, DM_T_MIN, DM_T_MAX);
|
||||
motor_send_mailbox.position_des = float_to_uint(0, DM_P_MIN, DM_P_MAX, 16);
|
||||
motor_send_mailbox.velocity_des = float_to_uint(set, DM_V_MIN, DM_V_MAX, 12);
|
||||
motor_send_mailbox.torque_des = float_to_uint(0, DM_T_MIN, DM_T_MAX, 12);
|
||||
motor_send_mailbox.Kp = motor->angle_PID.Kp;
|
||||
motor_send_mailbox.Kd = motor->angle_PID.Kd;
|
||||
motor_send_mailbox.velocity_des = float_to_uint(0, DM_V_MIN, DM_V_MAX, 12);
|
||||
motor_send_mailbox.torque_des = float_to_uint(set, DM_T_MIN, DM_T_MAX, 12);
|
||||
motor_send_mailbox.Kp = 0;//只使用力矩控制无需kpkd参数
|
||||
motor_send_mailbox.Kd = 0;//只使用力矩控制无需kpkd参数
|
||||
|
||||
|
||||
if(motor->stop_flag == MOTOR_STOP)
|
||||
{
|
||||
motor_send_mailbox.position_des = float_to_uint(0, DM_P_MIN, DM_P_MAX, 16);
|
||||
motor_send_mailbox.velocity_des = float_to_uint(0, DM_V_MIN, DM_V_MAX, 12);
|
||||
motor_send_mailbox.torque_des = float_to_uint(0, DM_T_MIN, DM_T_MAX, 12);
|
||||
}
|
||||
|
||||
motor->motor_can_instance->tx_buff[0] = (uint8_t)(motor_send_mailbox.position_des >> 8);
|
||||
motor->motor_can_instance->tx_buff[1] = (uint8_t)(motor_send_mailbox.position_des);
|
||||
|
|
@ -163,6 +203,9 @@ void DMMotorTask(void const *argument)
|
|||
motor->motor_can_instance->tx_buff[6] = (uint8_t)(((motor_send_mailbox.Kd & 0xF) << 4) | (motor_send_mailbox.torque_des >> 8));
|
||||
motor->motor_can_instance->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des);
|
||||
|
||||
|
||||
|
||||
|
||||
CANTransmit(motor->motor_can_instance, 1);
|
||||
|
||||
osDelay(2);
|
||||
|
|
|
|||
|
|
@ -8,8 +8,8 @@
|
|||
|
||||
#define DM_MOTOR_CNT 1
|
||||
|
||||
#define DM_P_MIN (-12.5f)
|
||||
#define DM_P_MAX 12.5f
|
||||
#define DM_P_MIN (-3.1415926f)
|
||||
#define DM_P_MAX 3.1415926f
|
||||
#define DM_V_MIN (-45.0f)
|
||||
#define DM_V_MAX 45.0f
|
||||
#define DM_T_MIN (-18.0f)
|
||||
|
|
@ -20,14 +20,15 @@ typedef struct
|
|||
{
|
||||
uint8_t id;
|
||||
uint8_t state;
|
||||
float velocity;
|
||||
float last_position;
|
||||
float position;
|
||||
float torque;
|
||||
float T_Mos;
|
||||
float T_Rotor;
|
||||
float angle_single_round;
|
||||
int32_t total_round;
|
||||
float velocity; //速度
|
||||
float last_position; //上次位置
|
||||
float position; //位置
|
||||
float torque; //力矩
|
||||
float T_Mos; //mos温度
|
||||
float T_Rotor; //电机温度
|
||||
float angle_single_round; //单圈角度
|
||||
int32_t total_round; //总圈数
|
||||
float total_angle; //总角度
|
||||
}DM_Motor_Measure_s;
|
||||
|
||||
typedef struct
|
||||
|
|
@ -42,16 +43,13 @@ typedef struct
|
|||
{
|
||||
DM_Motor_Measure_s measure;
|
||||
Motor_Control_Setting_s motor_settings;
|
||||
PIDInstance current_PID;
|
||||
PIDInstance speed_PID;
|
||||
PIDInstance angle_PID;
|
||||
float *other_angle_feedback_ptr;
|
||||
float *other_speed_feedback_ptr;
|
||||
float *speed_feedforward_ptr;
|
||||
float *current_feedforward_ptr;
|
||||
float pid_ref;
|
||||
Motor_Controller_s motor_controller;//电机控制器
|
||||
|
||||
CANInstance *motor_can_instance;//电机can实例
|
||||
|
||||
Motor_Type_e motor_type;//电机类型
|
||||
Motor_Working_Type_e stop_flag;
|
||||
CANInstance *motor_can_instance;
|
||||
|
||||
DaemonInstance* motor_daemon;
|
||||
uint32_t lost_cnt;
|
||||
}DMMotorInstance;
|
||||
|
|
|
|||
|
|
@ -116,6 +116,7 @@ HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
|
|||
|
||||
config->can_init_config.can_module_callback = HTMotorDecode;
|
||||
config->can_init_config.id = motor;
|
||||
config->can_init_config.rx_id = config->can_init_config.tx_id;
|
||||
motor->motor_can_instace = CANRegister(&config->can_init_config);
|
||||
|
||||
Daemon_Init_Config_s conf = {
|
||||
|
|
|
|||
|
|
@ -109,6 +109,7 @@ typedef struct
|
|||
typedef enum
|
||||
{
|
||||
MOTOR_TYPE_NONE = 0,
|
||||
DM4310,
|
||||
GM6020,
|
||||
M3508,
|
||||
M2006,
|
||||
|
|
|
|||
Loading…
Reference in New Issue