重力补偿、功率限制
This commit is contained in:
parent
ef5c502f81
commit
229add4878
|
|
@ -84,6 +84,7 @@ void ChassisInit() {
|
|||
.speed_feedback_source = MOTOR_FEED,
|
||||
.outer_loop_type = SPEED_LOOP,
|
||||
.close_loop_type = SPEED_LOOP, //| CURRENT_LOOP,
|
||||
.power_limit_flag = POWER_LIMIT_ON,
|
||||
},
|
||||
.motor_type = M3508,
|
||||
};
|
||||
|
|
@ -114,6 +115,8 @@ void ChassisInit() {
|
|||
}};
|
||||
cap = SuperCapInit(&cap_conf); // 超级电容初始化
|
||||
|
||||
// PowerMeter_Init_Config_s
|
||||
|
||||
// 发布订阅初始化,如果为双板,则需要can comm来传递消息
|
||||
#ifdef CHASSIS_BOARD
|
||||
Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
|
||||
|
|
@ -165,20 +168,47 @@ static void OmniCalculate() {
|
|||
vt_lf /= (WHEEL_BASE * 1.414f);
|
||||
}
|
||||
|
||||
static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f};
|
||||
///依据3508电机功率模型，预测电机输出功率
|
||||
static float EstimatePower(DJIMotorInstance* chassis_motor)
|
||||
{
|
||||
|
||||
float I_cmd = chassis_motor->motor_controller.current_PID.Output;
|
||||
float w = chassis_motor->measure.speed_aps /6 ;//aps to rpm
|
||||
|
||||
float power = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w + motor_power_K[2]*I_cmd*I_cmd;
|
||||
|
||||
return power;
|
||||
}
|
||||
float vofa_send_data[6];
|
||||
/**
|
||||
* @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
|
||||
*
|
||||
*/
|
||||
static void LimitChassisOutput() {
|
||||
// 功率限制待添加
|
||||
// referee_data->PowerHeatData.chassis_power;
|
||||
// referee_data->PowerHeatData.chassis_power_buffer;
|
||||
static void LimitChassisOutput()
|
||||
{
|
||||
float P_cmd = motor_rf->motor_controller.motor_power_predict +
|
||||
motor_rb->motor_controller.motor_power_predict +
|
||||
motor_lb->motor_controller.motor_power_predict +
|
||||
motor_lf->motor_controller.motor_power_predict + 3.6f;
|
||||
float P_max = 60 - 10;
|
||||
float K = P_max/P_cmd;
|
||||
vofa_send_data[2] = P_cmd;
|
||||
|
||||
motor_rf->motor_controller.motor_power_scale = K;
|
||||
motor_rb->motor_controller.motor_power_scale = K;
|
||||
motor_lf->motor_controller.motor_power_scale = K;
|
||||
motor_lb->motor_controller.motor_power_scale = K;
|
||||
|
||||
{
|
||||
DJIMotorSetRef(motor_lf, vt_lf);
|
||||
DJIMotorSetRef(motor_rf, vt_rf);
|
||||
DJIMotorSetRef(motor_lb, vt_lb);
|
||||
DJIMotorSetRef(motor_rb, vt_rb);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 完成功率限制后进行电机参考输入设定
|
||||
DJIMotorSetRef(motor_lf, vt_lf);
|
||||
DJIMotorSetRef(motor_rf, vt_rf);
|
||||
DJIMotorSetRef(motor_lb, vt_lb);
|
||||
DJIMotorSetRef(motor_rb, vt_rb);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -235,8 +265,7 @@ void ChassisTask() {
|
|||
static float sin_theta, cos_theta;
|
||||
cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
|
||||
sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
|
||||
// sin_theta = 0;
|
||||
// cos_theta = 1;
|
||||
|
||||
chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
|
||||
chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
|
||||
|
||||
|
|
|
|||
|
|
@ -118,12 +118,12 @@ static void RemoteControlSet() {
|
|||
// 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据?
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],小陀螺
|
||||
{
|
||||
// chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
|
||||
// gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
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)) // 右侧开关状态[中],,底盘跟随云台
|
||||
{
|
||||
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@ static Subscriber_t *gimbal_sub; // cmd控制消息订阅者
|
|||
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
|
||||
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息
|
||||
|
||||
|
||||
static float gravity_current = 0;
|
||||
static void LimitPitchAngleAuto() {
|
||||
/** 注意电机角度与陀螺仪角度方向是否相同
|
||||
* 目前 add > 0, 向下转动
|
||||
|
|
@ -97,7 +97,7 @@ void GimbalInit() {
|
|||
.MaxOut = 500,
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = -800, // 50
|
||||
.Kp = 0,//-800, // 50
|
||||
.Ki = 0, // 350
|
||||
.Kd = 0, // 0
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
|
|
@ -107,6 +107,7 @@ void GimbalInit() {
|
|||
.other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
|
||||
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
|
||||
.other_speed_feedback_ptr = &gimba_IMU_data->Gyro[0],
|
||||
.current_feedforward_ptr = &gravity_current,
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = OTHER_FEED,
|
||||
|
|
@ -114,6 +115,7 @@ void GimbalInit() {
|
|||
.outer_loop_type = ANGLE_LOOP,
|
||||
.close_loop_type = SPEED_LOOP | ANGLE_LOOP,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||
.feedforward_flag = CURRENT_FEEDFORWARD,
|
||||
},
|
||||
.motor_type = GM6020,
|
||||
.motor_control_type = CURRENT_CONTROL
|
||||
|
|
@ -170,6 +172,8 @@ void GimbalTask() {
|
|||
// 在合适的地方添加pitch重力补偿前馈力矩
|
||||
// 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
|
||||
// ...
|
||||
float theta = - gimba_IMU_data->Pitch/180*PI;
|
||||
gravity_current = (5000)*arm_cos_f32(theta);
|
||||
|
||||
// float vofa_send_data[4];
|
||||
// vofa_send_data[0] = pitch_motor->motor_controller.speed_PID.Ref;
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@ void RobotInit()
|
|||
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
|
||||
RobotCMDInit();
|
||||
GimbalInit();
|
||||
ShootInit();
|
||||
//ShootInit();
|
||||
#endif
|
||||
|
||||
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
|
||||
|
|
@ -50,7 +50,7 @@ void RobotTask()
|
|||
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
|
||||
RobotCMDTask();
|
||||
GimbalTask();
|
||||
ShootTask();
|
||||
//ShootTask();
|
||||
#endif
|
||||
|
||||
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@
|
|||
|
||||
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
|
||||
// 云台参数
|
||||
#define YAW_CHASSIS_ALIGN_ECD 3055 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
|
||||
#define YAW_CHASSIS_ALIGN_ECD 7856 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
|
||||
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
|
||||
#define PITCH_HORIZON_ECD 1670 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
|
||||
#define PITCH_MAX_ANGLE 25 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
|
||||
|
|
@ -201,7 +201,7 @@ typedef struct
|
|||
typedef struct
|
||||
{
|
||||
attitude_t gimbal_imu_data;
|
||||
uint16_t yaw_motor_single_round_angle;
|
||||
float yaw_motor_single_round_angle;
|
||||
} Gimbal_Upload_Data_s;
|
||||
|
||||
typedef struct
|
||||
|
|
|
|||
|
|
@ -2,6 +2,7 @@
|
|||
#include "general_def.h"
|
||||
#include "bsp_dwt.h"
|
||||
#include "bsp_log.h"
|
||||
#include "user_lib.h"
|
||||
|
||||
static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
|
||||
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
|
||||
|
|
@ -246,6 +247,18 @@ void DJIMotorSetRef(DJIMotorInstance *motor, float ref) {
|
|||
motor->motor_controller.pid_ref = ref;
|
||||
}
|
||||
|
||||
static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f};
|
||||
//依据3508电机功率模型，预测电机输出功率
|
||||
static float EstimatePower(DJIMotorInstance* chassis_motor,float output)
|
||||
{
|
||||
float I_cmd = chassis_motor->motor_controller.current_PID.Output;
|
||||
float w = chassis_motor->measure.speed_aps /6 ;//aps to rpm
|
||||
|
||||
float power = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w + motor_power_K[2]*I_cmd*I_cmd;
|
||||
return power;
|
||||
}
|
||||
|
||||
|
||||
// 为所有电机实例计算三环PID,发送控制报文
|
||||
void DJIMotorControl() {
|
||||
// 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性
|
||||
|
|
@ -303,6 +316,38 @@ void DJIMotorControl() {
|
|||
if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
|
||||
pid_ref *= -1;
|
||||
|
||||
//功率限制
|
||||
if(motor_setting->power_limit_flag == POWER_LIMIT_ON)
|
||||
{
|
||||
float I_cmd = pid_ref;
|
||||
float w = measure->speed_aps /6 ;//aps to rpm
|
||||
motor_controller->motor_power_predict = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w + motor_power_K[2]*I_cmd*I_cmd;
|
||||
//这里K应该使用所有底盘电机一起计算 (在底盘任务中)
|
||||
//float K = motor_controller->motor_power_max / motor_controller->motor_power_predict;
|
||||
float K = motor_controller->motor_power_scale;
|
||||
if(K>=1 || motor_controller->motor_power_predict < 0)//未超过最大功率 或做负功的电机 不做限制
|
||||
{
|
||||
|
||||
}
|
||||
else if(K<1)
|
||||
{
|
||||
float P_cmd = K * motor_controller->motor_power_predict; //对输出功率进行缩放
|
||||
|
||||
float a = motor_power_K[2] ;
|
||||
float b = motor_power_K[0] * w;
|
||||
float c = motor_power_K[1] * w * w - P_cmd;
|
||||
|
||||
if(pid_ref < 0)
|
||||
pid_ref = (-b - sqrtf(b*b-4*a*c))/(2*a);
|
||||
else
|
||||
pid_ref = (-b + sqrtf(b*b-4*a*c))/(2*a);
|
||||
}
|
||||
//if( motor_controller->motor_power_predict < )
|
||||
}
|
||||
|
||||
// 获取最终输出 此处注意set不要超过int16_t能表达的最大数 +-32767
|
||||
|
||||
pid_ref = float_constrain(pid_ref,-30000,30000);
|
||||
// 获取最终输出
|
||||
set = (int16_t) pid_ref;
|
||||
|
||||
|
|
|
|||
|
|
@ -68,6 +68,12 @@ typedef enum
|
|||
MOTOR_ENALBED = 1,
|
||||
} Motor_Working_Type_e;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
NO_POWER_LIMIT = 0,
|
||||
POWER_LIMIT_ON = 1,
|
||||
} Power_Limit_Type_e;
|
||||
|
||||
/* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
|
||||
typedef struct
|
||||
{
|
||||
|
|
@ -78,6 +84,7 @@ typedef struct
|
|||
Feedback_Source_e angle_feedback_source; // 角度反馈类型
|
||||
Feedback_Source_e speed_feedback_source; // 速度反馈类型
|
||||
Feedfoward_Type_e feedforward_flag; // 前馈标志
|
||||
Power_Limit_Type_e power_limit_flag; //功率限制标志
|
||||
|
||||
} Motor_Control_Setting_s;
|
||||
|
||||
|
|
@ -104,6 +111,10 @@ typedef struct
|
|||
PIDInstance angle_PID;
|
||||
|
||||
float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环
|
||||
|
||||
float motor_power_max; //每个电机分配的功率上限
|
||||
float motor_power_predict; //根据模型预测的电机功率
|
||||
float motor_power_scale; //电机功率缩放比例
|
||||
} Motor_Controller_s;
|
||||
|
||||
/* 电机类型枚举 */
|
||||
|
|
|
|||
Loading…
Reference in New Issue