重新优化拨弹和底盘PID
This commit is contained in:
userName
parent
e4b8e54574
commit
2ee9f9e6d5
|
|
@ -59,6 +59,7 @@ float chassis_power = 0;
|
|||
static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘
|
||||
static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
|
||||
float P_cmdall = 0;
|
||||
|
||||
void ChassisInit()
|
||||
{
|
||||
// 四个轮子的参数一样,改tx_id和反转标志位即可
|
||||
|
|
@ -66,9 +67,9 @@ void ChassisInit()
|
|||
.can_init_config.can_handle = &hcan1,
|
||||
.controller_param_init_config = {
|
||||
.speed_PID = {
|
||||
.Kp = 17, // 17
|
||||
.Ki = 3, // 3
|
||||
.Kd = 0.02f, // 0.02
|
||||
.Kp = 4, // 4
|
||||
.Ki = 1.2f, // 1.2
|
||||
.Kd = 0.01f, // 0.01
|
||||
.IntegralLimit = 3000,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.MaxOut = 12000,
|
||||
|
|
@ -86,14 +87,14 @@ void ChassisInit()
|
|||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.outer_loop_type = SPEED_LOOP,
|
||||
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
|
||||
//功率限制必须要用电流环
|
||||
.power_limit_flag = NO_POWER_LIMIT,
|
||||
.close_loop_type = SPEED_LOOP,
|
||||
|
||||
.power_limit_flag = POWER_LIMIT_ON,
|
||||
},
|
||||
.motor_type = M3508,
|
||||
};
|
||||
// @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
|
||||
chassis_motor_config.can_init_config.tx_id = 2;
|
||||
chassis_motor_config.can_init_config.tx_id = 3;
|
||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
motor_lf = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
|
|
@ -105,7 +106,7 @@ void ChassisInit()
|
|||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
motor_lb = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 3;
|
||||
chassis_motor_config.can_init_config.tx_id = 2;
|
||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
motor_rb = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
|
|
@ -119,13 +120,13 @@ void ChassisInit()
|
|||
}};
|
||||
cap = SuperCapInit(&cap_conf); // 超级电容初始化
|
||||
|
||||
PowerMeter_Init_Config_s power_conf = {
|
||||
.can_config = {
|
||||
.can_handle = &hcan2,
|
||||
.rx_id = 0x211,
|
||||
}
|
||||
};
|
||||
power_meter = PowerMeterInit(&power_conf);
|
||||
// PowerMeter_Init_Config_s power_conf = {
|
||||
// .can_config = {
|
||||
// .can_handle = &hcan2,
|
||||
// .rx_id = 0x211,
|
||||
// }
|
||||
// };
|
||||
// power_meter = PowerMeterInit(&power_conf);
|
||||
|
||||
// 发布订阅初始化,如果为双板,则需要can comm来传递消息
|
||||
#ifdef CHASSIS_BOARD
|
||||
|
|
@ -231,7 +232,6 @@ void ChassisTask()
|
|||
#ifdef CHASSIS_BOARD
|
||||
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
|
||||
#endif // CHASSIS_BOARD
|
||||
|
||||
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
|
||||
{ // 如果出现重要模块离线或遥控器设置为急停,让电机停止
|
||||
DJIMotorStop(motor_lf);
|
||||
|
|
@ -278,7 +278,7 @@ void ChassisTask()
|
|||
// 根据控制模式进行正运动学解算,计算底盘输出
|
||||
MecanumCalculate();
|
||||
//读取底盘功率，方便功率控制
|
||||
chassis_power = PowerMeterGet(power_meter);
|
||||
// chassis_power = PowerMeterGet(power_meter);
|
||||
// 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
|
||||
LimitChassisOutput();
|
||||
// 根据电机的反馈速度和IMU(如果有)计算真实速度
|
||||
|
|
|
|||
|
|
@ -119,7 +119,13 @@ static void CalcOffsetAngle()
|
|||
// chassis_cmd_send.offset_angle = chassis_cmd_send.offset_angle;
|
||||
}
|
||||
|
||||
|
||||
static void pitch_limit()
|
||||
{
|
||||
if(gimbal_fetch_data.pitch_motor_total_angle > horizontal_angle + 1500)
|
||||
gimbal_cmd_send.pitch -= 0.1f;
|
||||
if(gimbal_fetch_data.pitch_motor_total_angle < horizontal_angle - 4000)
|
||||
gimbal_cmd_send.pitch += 0.1f;
|
||||
}
|
||||
/**
|
||||
* @brief 控制输入为遥控器(调试时)的模式和控制量设置
|
||||
*
|
||||
|
|
@ -147,10 +153,7 @@ static void RemoteControlSet()
|
|||
{
|
||||
gimbal_cmd_send.yaw += 0.003f * (float)rc_data[TEMP].rc.rocker_l_;
|
||||
gimbal_cmd_send.pitch += 0.0007f * (float)rc_data[TEMP].rc.rocker_l1;
|
||||
if(gimbal_fetch_data.pitch_motor_total_angle > horizontal_angle + 1500)
|
||||
gimbal_cmd_send.pitch -= 0.1f;
|
||||
if(gimbal_fetch_data.pitch_motor_total_angle < horizontal_angle - 4000)
|
||||
gimbal_cmd_send.pitch += 0.1f;
|
||||
pitch_limit();
|
||||
|
||||
}
|
||||
// 左侧开关状态为[下],视觉模式
|
||||
|
|
@ -219,12 +222,12 @@ static void RemoteControlSet()
|
|||
*/
|
||||
static void MouseKeySet()
|
||||
{
|
||||
chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].w * 300 - rc_data[TEMP].key[KEY_PRESS].s * 300; // 系数待测
|
||||
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].s * 300 - rc_data[TEMP].key[KEY_PRESS].d * 300;
|
||||
|
||||
gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测
|
||||
gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10;
|
||||
chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 7000 - rc_data[TEMP].key[KEY_PRESS].d * 7000; // 系数待测
|
||||
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 7000 - rc_data[TEMP].key[KEY_PRESS].s * 7000;
|
||||
|
||||
gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 7; // 系数待测
|
||||
gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 7;
|
||||
pitch_limit();
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速
|
||||
{
|
||||
case 0:
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@
|
|||
|
||||
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
|
||||
// 云台参数
|
||||
#define YAW_CHASSIS_ALIGN_ECD 891 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
|
||||
#define YAW_CHASSIS_ALIGN_ECD 295 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
|
||||
#define YAW_ECD_GREATER_THAN_4096 1 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
|
||||
#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
|
||||
#define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
|
||||
|
|
|
|||
|
|
@ -64,7 +64,7 @@ void ShootInit()
|
|||
friction_r = DJIMotorInit(&friction_config);
|
||||
|
||||
friction_config.can_init_config.tx_id = 6; // 上摩擦轮,改txid和方向就行
|
||||
friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
friction_z = DJIMotorInit(&friction_config);
|
||||
|
||||
|
||||
|
|
@ -77,21 +77,21 @@ void ShootInit()
|
|||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
// 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
|
||||
.Kp = 130, // 10
|
||||
.Kp = 50, // 10
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 15000,
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = 5, // 10
|
||||
.Kp = 2.4f, // 10
|
||||
.Ki = 0, // 1
|
||||
.Kd = 0,
|
||||
.Improve = PID_Integral_Limit,
|
||||
.IntegralLimit = 5000,
|
||||
.MaxOut = 10000,
|
||||
.MaxOut = 28000,
|
||||
},
|
||||
.current_PID = {
|
||||
.Kp = 0.7f, // 0.7
|
||||
.Kp = 0, // 0.7
|
||||
.Ki = 0, // 0.1
|
||||
.Kd = 0,
|
||||
.Improve = PID_Integral_Limit,
|
||||
|
|
@ -175,7 +175,7 @@ void ShootTask()
|
|||
// 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
|
||||
case LOAD_BURSTFIRE:
|
||||
DJIMotorOuterLoop(loader, SPEED_LOOP);
|
||||
DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8);
|
||||
DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 5);
|
||||
// x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是)
|
||||
break;
|
||||
// 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
|
||||
|
|
|
|||
|
|
@ -323,13 +323,13 @@ void DJIMotorControl()
|
|||
if(pid_ref < 0)
|
||||
{
|
||||
float temp = (-b - sqrtf(b*b-4*a*c))/(2*a);
|
||||
if(temp < -15000) pid_ref = -15000;
|
||||
if(temp < -28000) pid_ref = -28000;
|
||||
else pid_ref = temp;
|
||||
}
|
||||
else
|
||||
{
|
||||
float temp = (-b + sqrtf(b*b-4*a*c))/(2*a);
|
||||
if(temp > 15000) pid_ref = 15000;
|
||||
if(temp > 28000) pid_ref = 28000;
|
||||
else pid_ref = temp;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -69,14 +69,16 @@ void MyUIInit()
|
|||
DeterminRobotID(); // 确定ui要发送到的目标客户端
|
||||
UIDelete(&referee_recv_info->referee_id, UI_Data_Del_ALL, 0); // 清空UI
|
||||
|
||||
|
||||
// 绘制发射基准线
|
||||
UILineDraw(&UI_shoot_line[0], "sl0", UI_Graph_ADD, 7, UI_Color_White, 3, 710, shoot_line_location[0], 1210, shoot_line_location[0]);
|
||||
UILineDraw(&UI_shoot_line[1], "sl1", UI_Graph_ADD, 7, UI_Color_White, 3, shoot_line_location[1], 340, shoot_line_location[1], 740);
|
||||
UILineDraw(&UI_shoot_line[1], "sl1", UI_Graph_ADD, 7, UI_Color_White, 3, 740, shoot_line_location[1], 340, shoot_line_location[1]);
|
||||
UILineDraw(&UI_shoot_line[2], "sl2", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[2], 1110, shoot_line_location[2]);
|
||||
UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[3], 1110, shoot_line_location[3]);
|
||||
UILineDraw(&UI_shoot_line[4], "sl4", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[4], 1110, shoot_line_location[4]);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_shoot_line[0], UI_shoot_line[1], UI_shoot_line[2], UI_shoot_line[3], UI_shoot_line[4]);
|
||||
|
||||
|
||||
// 绘制车辆状态标志指示
|
||||
UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 150, 750, "chassis:");
|
||||
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[0]);
|
||||
|
|
|
|||
Loading…
Reference in New Issue