From 2ee9f9e6d565e9c06ccc3faf2d27534cda672e16 Mon Sep 17 00:00:00 2001 From: userName Date: Sat, 9 Mar 2024 22:06:29 +0800 Subject: [PATCH] =?UTF-8?q?=E9=87=8D=E6=96=B0=E4=BC=98=E5=8C=96=E6=8B=A8?= =?UTF-8?q?=E5=BC=B9=E5=92=8C=E5=BA=95=E7=9B=98PID?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/chassis.c | 34 +++++++++++++++--------------- application/cmd/robot_cmd.c | 23 +++++++++++--------- application/robot_def.h | 2 +- application/shoot/shoot.c | 12 +++++------ modules/motor/DJImotor/dji_motor.c | 4 ++-- modules/referee/referee_task.c | 4 +++- 6 files changed, 42 insertions(+), 37 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 887f748..22ebe57 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -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(如果有)计算真实速度 diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 646a76a..1493db1 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -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: diff --git a/application/robot_def.h b/application/robot_def.h index b14054c..defd436 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -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 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index cb85170..3787e92 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -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; // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流) diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index 6d0162d..a6e88e2 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -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; } } diff --git a/modules/referee/referee_task.c b/modules/referee/referee_task.c index 9950066..4827507 100644 --- a/modules/referee/referee_task.c +++ b/modules/referee/referee_task.c @@ -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]);