TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-23 19:25:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

小陀螺

Browse Source
This commit is contained in:
chenfu
2024-05-23 13:41:20 +08:00
parent b42e603e0e
commit 845942b3d0
4 changed files with 45 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
application/chassis/balance.c
View File

@@ -394,9 +394,16 @@ static void ParamAssemble()
static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
{
float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw);
PIDCalculate(&steer_v_pid, chassis.wz, p_ref);
if(chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG ||
chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW)
{
float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw);
PIDCalculate(&steer_v_pid, chassis.wz, p_ref);
}
else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE)
{
PIDCalculate(&steer_v_pid, chassis.wz, 6);
}
l_side.T_wheel -= steer_v_pid.Output;
r_side.T_wheel += steer_v_pid.Output;
@@ -467,6 +474,6 @@ void BalanceTask()
return; // 复位模态或急停,直接退出
// 运动模态,电机输出映射和限幅
// WattLimitSet();
WattLimitSet();
// CANCommSend(cmd_can_comm, (void *)&chassis_feedback_data);
}

36
application/cmd/robot_cmd.c
View File

@@ -72,6 +72,7 @@ void RobotCMDInit()
shoot_cmd_send.shoot_mode = SHOOT_OFF;
shoot_cmd_send.load_mode = LOAD_STOP;
shoot_cmd_send.friction_mode = FRICTION_OFF;
chassis_direction = CHASSIS_ALIGN;
robot_state = ROBOT_STOP; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
}
@@ -85,6 +86,14 @@ static void CalcOffsetAngle()
{
//@todo:相差一整圈时会出问题，待修复
// 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
if(chassis_direction == CHASSIS_ALIGN)
{
yaw_chassis_align_ecd = 2716;
}
else if(chassis_direction == CHASSIS_SIDLE)
{
yaw_chassis_align_ecd = 765;
}
static float angle;
yaw_align_angle = yaw_chassis_align_ecd * ECD_ANGLE_COEF_DJI; // 从底盘获取的yaw电机对齐角度
angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
@@ -127,12 +136,10 @@ static void RemoteControlSet()
if (abs(rc_data[TEMP].rc.rocker_r1) > 500)
{
chassis_direction = CHASSIS_ALIGN;
yaw_chassis_align_ecd = 2716;
}
else if (abs(rc_data[TEMP].rc.rocker_r_) > 500)
{
chassis_direction = CHASSIS_SIDLE;
yaw_chassis_align_ecd = 765;
}
// 右拨杆拨下去,底盘旋转
@@ -151,7 +158,7 @@ static void RemoteControlSet()
gimbal_cmd_send.pitch -= 0.0006f * (float)rc_data[TEMP].rc.rocker_l1;
// 摇杆控制的软件限位
gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE);
// gimbal_cmd_send.yaw = float_constrain(gimbal_cmd_send.yaw, -90.0f, 90+.0f);
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
// 摩擦轮控制,拨轮向上打为负,向下为正
@@ -188,23 +195,22 @@ static void MouseKeySet()
{
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
gimbal_cmd_send.yaw -= 0.1f * rc_data[TEMP].mouse.x;
gimbal_cmd_send.pitch -= 0.1f * rc_data[TEMP].mouse.y;
gimbal_cmd_send.yaw -= (float)(rc_data[TEMP].mouse.x + rc_data[LAST].mouse.x) / 660.0f * 4.0f ; // 系数待测
gimbal_cmd_send.pitch += (float)(rc_data[TEMP].mouse.y + rc_data[TEMP].mouse.y) / 660.0f * 4.0f;
gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE);
chassis_cmd_send.vx = BALANCE_MAX_SPEED * (float)(rc_data[TEMP].key[KEY_PRESS].w - rc_data[TEMP].key[KEY_PRESS].s
+ rc_data[TEMP].key[KEY_PRESS].a - rc_data[TEMP].key[KEY_PRESS].d);
chassis_cmd_send.vx = BALANCE_MAX_SPEED * (float)(rc_data[TEMP].key[KEY_PRESS].w - rc_data[TEMP].key[KEY_PRESS].s - rc_data[TEMP].key[KEY_PRESS].a + rc_data[TEMP].key[KEY_PRESS].d);
chassis_cmd_send.delta_leglen = (float)(rc_data[TEMP].key[KEY_PRESS].e - rc_data[TEMP].key[KEY_PRESS].c) * 0.001f;
if (rc_data[TEMP].key[KEY_PRESS].w || rc_data[TEMP].key[KEY_PRESS].s)
{
chassis_cmd_send.direction = CHASSIS_ALIGN;
chassis_direction = CHASSIS_ALIGN;
}
else if (rc_data[TEMP].key[KEY_PRESS].a || rc_data[TEMP].key[KEY_PRESS].d)
{
chassis_cmd_send.direction = CHASSIS_SIDLE;
chassis_direction = CHASSIS_SIDLE;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q 小陀螺
@@ -227,13 +233,11 @@ static void MouseKeySet()
break;
}
if(rc_data[TEMP].mouse.press_r)
if (rc_data[TEMP].mouse.press_r)
{
gimbal_cmd_send.yaw = ( vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw );
gimbal_cmd_send.pitch =( vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch );
gimbal_cmd_send.yaw = (vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw);
gimbal_cmd_send.pitch = (vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch);
}
}
/**
@@ -278,7 +282,6 @@ static void EmergencyHandler()
shoot_cmd_send.shoot_mode = SHOOT_ON;
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
chassis_cmd_send.direction = CHASSIS_ALIGN;
break;
default:
@@ -288,6 +291,7 @@ static void EmergencyHandler()
shoot_cmd_send.shoot_mode = SHOOT_OFF;
shoot_cmd_send.friction_mode = FRICTION_OFF;
shoot_cmd_send.load_mode = LOAD_STOP;
chassis_cmd_send.direction = CHASSIS_ALIGN;
gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步，避免恢复时疯转
gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch;
@@ -313,6 +317,8 @@ void RobotCMDTask()
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
chassis_cmd_send.direction = chassis_direction;
CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send);
PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);

20
application/gimbal/gimbal.c
View File

@@ -26,26 +26,26 @@ void GimbalInit()
},
.controller_param_init_config = {
.angle_PID = {
.Kp = 0.8, //0.5
.Ki = 6.0,//0
.Kd = 0.0,//0
.CoefA =10.0,//0
.CoefB = 0.5,//0
.Kp = 0.5, //1.0
.Ki = 0.0,//0.8
.Kd = 0.0,//0.005
.CoefA =0.0,//4.0
.CoefB = 0.0,//0.5
.Output_LPF_RC = 0,
.DeadBand = 0.0,//0
.Derivative_LPF_RC=0.0,//0
.Derivative_LPF_RC=0,//0.01
.Improve = PID_Trapezoid_Intergral |PID_ChangingIntegrationRate| PID_Integral_Limit |PID_Derivative_On_Measurement | PID_OutputFilter |PID_DerivativeFilter,
.IntegralLimit = 5.0,
.IntegralLimit = 4.0,
.MaxOut = 20,
},
.speed_PID = {
.Kp = 25000,//14000
.Kp = 14000,//22500
.Ki = 0,//
.Kd =0,
// .CoefA = 0.8,
// .CoefB = 0.1,
.Output_LPF_RC = 0.0,//0
.Output_LPF_RC = 0.002,//0
.Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement | PID_OutputFilter,
.IntegralLimit = 0,
.MaxOut = 20000,
@@ -84,7 +84,7 @@ void GimbalInit()
.MaxOut = 10,//600
},
.speed_PID = {
.Kp=13000,//10000
.Kp=10000,//10000
.Ki =0,//0
.Kd =0.0,//0
.CoefA =0,//0

5
application/robot_def.h
View File

@@ -18,8 +18,8 @@
/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
// #define ONE_BOARD // 单板控制整车
// #define CHASSIS_BOARD //底盘板
#define GIMBAL_BOARD //云台板
#define CHASSIS_BOARD //底盘板
// #define GIMBAL_BOARD //云台板
#define VISION_USE_VCP // 使用虚拟串口发送视觉数据
// #define VISION_USE_UART // 使用串口发送视觉数据
@@ -27,6 +27,7 @@
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
// 云台参数
#define YAW_CHASSIS_ALIGN_ECD 2716 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
#define YAW_CHASSIS_SIDE_ECD 716 // 云台和底盘侧面对齐时的电机编码器值,若对云台有机械改动需要修改
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
#define PITCH_MAX_ANGLE (30.0f) // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)