TuxMonkey/half-steering-basic-framework
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/half-steering-basic-framework synced 2026-10-24 01:52:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

test

Browse Source
This commit is contained in:
Zorua
2026-01-18 10:48:06 +08:00
parent 734d04ec10
commit ba89c2044a
4 changed files with 66 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

100
application/chassis/chassis.c
View File

@@ -56,15 +56,15 @@ static referee_info_t* referee_data; // 用于获取裁判系统的数据
static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
static SuperCapInstance *cap; // 超级电容
static LKMotorInstance *motor_lf,*motor_rb; // 轮电机9015
static DJIMotorInstance *motor_steering_lf,*motor_steering_rb; // 舵电机6020
static LKMotorInstance *motor_lb,*motor_rf; // 轮电机9015
static DJIMotorInstance *motor_steering_lb,*motor_steering_rf; // 舵电机6020
/* 用于自旋变速策略的时间变量 */
// static float t;
/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘
static float vt_lf, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
static float at_lf, at_rb; // 底盘的角度解算后的临时输出,待进行限幅
static float vt_lb, vt_rf; // 底盘速度解算后的临时输出,待进行限幅
static float at_lb, at_rf; // 底盘的角度解算后的临时输出,待进行限幅
static PIDInstance chassis_follow_pid; // 底盘跟随云台的PID
void ChassisInit()
@@ -100,13 +100,13 @@ void ChassisInit()
};
// @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
chassis_motor_config.can_init_config.tx_id = 1;
// chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
motor_rb = LKMotorInit(&chassis_motor_config);
chassis_motor_config.can_init_config.tx_id = 2;
// chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
motor_lf = LKMotorInit(&chassis_motor_config);
motor_rf = LKMotorInit(&chassis_motor_config);
chassis_motor_config.can_init_config.tx_id = 1;
// chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
motor_lb = LKMotorInit(&chassis_motor_config);
// 舵电机6020初始化
Motor_Init_Config_s chassis_motor_steering_config = {
@@ -145,10 +145,10 @@ void ChassisInit()
};
chassis_motor_steering_config.can_init_config.tx_id = 1;
motor_steering_rb = DJIMotorInit(&chassis_motor_steering_config);
motor_steering_rf = DJIMotorInit(&chassis_motor_steering_config);
chassis_motor_steering_config.can_init_config.tx_id = 2;
motor_steering_lf = DJIMotorInit(&chassis_motor_steering_config);
motor_steering_lb = DJIMotorInit(&chassis_motor_steering_config);
PID_Init_Config_s chassis_follow_pid_conf = {
.Kp = 6, // 6
@@ -266,46 +266,58 @@ static void MinmizeRotation(float *angle, const float *last_angle, float *speed)
* @brief 舵轮电机角度解算
*
*/
static void SteeringWheelCalculate()
static void SteeringWheelCalculate()
{
float offset_lf, offset_rb; // 用于计算舵轮的角度
float at_lf_last, at_rb_last; // 上次的角度
at_lf_last = motor_steering_lf->measure.total_angle;
at_rb_last = motor_steering_rb->measure.total_angle;
float offset_rf, offset_lb; // 用于计算舵轮的角度
float at_rf_last, at_lb_last; // 上次的角度
at_lb_last = motor_steering_lb->measure.total_angle;
// at_lf_last = motor_steering_lf->measure.total_angle;
at_rf_last = motor_steering_rf->measure.total_angle;
// at_rb_last = motor_steering_rb->measure.total_angle;
if (chassis_vx == 0 && chassis_vy == 0 && chassis_cmd_recv.wz == 0) {
// at_lf = at_lf_last;
// at_rf = at_rf_last;
// at_lb = at_lb_last;
// at_rb = at_rb_last;
vt_lf = 0, vt_rb = 0;
vt_lb = 0, vt_rf = 0;
} else {
// 生成预计算变量，减少计算量，空间换时间
float w = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2;
float temp_x = chassis_vx - w, temp_y = chassis_vy - w;
arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lf); // lf：y- , x-
temp_y = chassis_vy + w; // 重复利用变量,temp_x = chassis_vy - w;与上次相同因此注释
// arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lb); // lb: y+ , x-
// temp_x = chassis_vx + w; // temp_y = chassis_vx + w;与上次相同因此注释
arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rb); // rb: y+ , x+
temp_y = chassis_vy - w; // temp_x = chassis_vy + w;与上次相同因此注释
// arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rf); // rf: y- , x+
// arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lf); // lf：y- , x-
// temp_y = chassis_vy + w; // 重复利用变量,temp_x = chassis_vy - w;与上次相同因此注释
arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lb); // lb: y+ , x-
temp_x = chassis_vx + w; // temp_y = chassis_vx + w;与上次相同因此注释
// arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rb); // rb: y+ , x+
// temp_y = chassis_vy - w; // temp_x = chassis_vy + w;与上次相同因此注释
arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rf); // rf: y- , x+
// 计算角度偏移
offset_lf = atan2f(chassis_vx - w, chassis_vy - w) * RAD_2_DEGREE; // lf: y- , x-
offset_rb = atan2f(chassis_vx + w, chassis_vy + w) * RAD_2_DEGREE; // rb: y+ , x+
// offset_lf = atan2f(chassis_vy - w, chassis_vx - w) * RAD_2_DEGREE; // lf: y- , x-
offset_rf = atan2f(chassis_vy - w, chassis_vx - w) * RAD_2_DEGREE; // rf: y- , x+
offset_lb = atan2f(chassis_vy + w, chassis_vx + w) * RAD_2_DEGREE; // lb: y+ , x-
// offset_rb = atan2f(chassis_vy + w, chassis_vx + w) * RAD_2_DEGREE; // rb: y+ , x+
at_lf = STEERING_CHASSIS_ALIGN_ANGLE_LF + offset_lf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度
at_rb = STEERING_CHASSIS_ALIGN_ANGLE_RB + offset_rb;
// at_lf = STEERING_CHASSIS_ALIGN_ANGLE_LF + offset_lf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度
at_rf = STEERING_CHASSIS_ALIGN_ANGLE_RF + offset_rf;
at_lb = STEERING_CHASSIS_ALIGN_ANGLE_LB + offset_lb;
// at_rb = STEERING_CHASSIS_ALIGN_ANGLE_RB + offset_rb;
ANGLE_LIMIT_360_TO_180_ABS(at_lf);
ANGLE_LIMIT_360_TO_180_ABS(at_rb);
// ANGLE_LIMIT_360_TO_180_ABS(at_lf);
ANGLE_LIMIT_360_TO_180_ABS(at_rf);
ANGLE_LIMIT_360_TO_180_ABS(at_lb);
// ANGLE_LIMIT_360_TO_180_ABS(at_rb);
MinmizeRotation(&at_lf, &at_lf_last, &vt_lf);
MinmizeRotation(&at_rb, &at_rb_last, &vt_rb);
// MinmizeRotation(&at_lf, &at_lf_last, &vt_lf);
MinmizeRotation(&at_rf, &at_rf_last, &vt_rf);
MinmizeRotation(&at_lb, &at_lb_last, &vt_lb);
// MinmizeRotation(&at_rb, &at_rb_last, &vt_rb);
}
DJIMotorSetRef(motor_steering_lf, at_lf);
DJIMotorSetRef(motor_steering_rb, at_rb);
// DJIMotorSetRef(motor_steering_lf, at_lf);
DJIMotorSetRef(motor_steering_rf, at_rf);
DJIMotorSetRef(motor_steering_lb, at_lb);
// DJIMotorSetRef(motor_steering_rb, at_rb);
}
/**
@@ -335,8 +347,8 @@ static void LimitChassisOutput()
// LKMotorSetRef( motor_lf , vt_lf );
// LKMotorSetRef( motor_rb , vt_rb );
LKMotorSetRef( motor_lf , vt_lf );
LKMotorSetRef( motor_rb , vt_rb );
LKMotorSetRef( motor_rf , vt_rf );
LKMotorSetRef( motor_lb , vt_lb );
}
/**
@@ -365,18 +377,18 @@ void ChassisTask()
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
{ // 如果出现重要模块离线或遥控器设置为急停,让电机停止
LKMotorStop(motor_lf);
LKMotorStop(motor_rb);
DJIMotorStop(motor_steering_lf);
DJIMotorStop(motor_steering_rb);
LKMotorStop(motor_rf);
LKMotorStop(motor_lb);
DJIMotorStop(motor_steering_rf);
DJIMotorStop(motor_steering_lb);
}
else
{
// 正常工作
LKMotorEnable(motor_lf);
LKMotorEnable(motor_rb);
DJIMotorEnable(motor_steering_lf);
DJIMotorEnable(motor_steering_rb);
LKMotorEnable(motor_rf);
LKMotorEnable(motor_lb);
DJIMotorEnable(motor_steering_rf);
DJIMotorEnable(motor_steering_lb);
}
// 根据控制模式设定旋转速度

6
application/cmd/robot_cmd.c
View File

@@ -155,7 +155,7 @@ static void RemoteControlSet()
}
else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动
{
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW;
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
}
@@ -174,8 +174,8 @@ static void RemoteControlSet()
// 云台软件限位
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
chassis_cmd_send.vy = -0.15f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向
chassis_cmd_send.vx = 0.15f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向
chassis_cmd_send.vx = 0.15f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向
chassis_cmd_send.vy = 0.15f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向
// // 发射参数
// if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开

2
application/gimbal/gimbal.c
View File

@@ -56,7 +56,7 @@ void GimbalInit()
.speed_feedback_source = OTHER_FEED,
.outer_loop_type = ANGLE_LOOP,
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
.motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
},
.motor_type = GM6020};
// PITCH

12
application/robot_def.h
View File

@@ -26,8 +26,8 @@
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
// 云台参数
#define YAW_CHASSIS_ALIGN_ECD 8192 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
#define YAW_ECD_GREATER_THAN_4096 1 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
#define YAW_CHASSIS_ALIGN_ECD 3938 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
#define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define PITCH_MIN_ANGLE 0 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
@@ -35,11 +35,11 @@
// #define CHASSIS_OMNI_WHEEL // 是否为全向轮底盘
// #define CHASSIS_MCNAMEE_WHEEL // 是否为麦克纳姆轮底盘
#define CHASSIS_STEERING_WHEEL // 是否为舵轮
#define STEERING_CHASSIS_ALIGN_ECD_LF 1221 // 舵电机 A 编码器值，若有机械改动需要修改
#define STEERING_CHASSIS_ALIGN_ECD_RB 7294 // 舵电机 D 编码器值，若有机械改动需要修改
#define STEERING_CHASSIS_ALIGN_ECD_RF 1221 // 舵电机 A 编码器值，若有机械改动需要修改
#define STEERING_CHASSIS_ALIGN_ECD_LB 7294 // 舵电机 D 编码器值，若有机械改动需要修改
#define STEERING_CHASSIS_ALIGN_ANGLE_LF STEERING_CHASSIS_ALIGN_ECD_LF / 8192.f * 360.f // 舵轮 A 对齐角度
#define STEERING_CHASSIS_ALIGN_ANGLE_RB STEERING_CHASSIS_ALIGN_ECD_RB / 8192.f * 360.f // 舵轮 D 对齐角度
#define STEERING_CHASSIS_ALIGN_ANGLE_RF STEERING_CHASSIS_ALIGN_ECD_RF / 8192.f * 360.f // 舵轮 A 对齐角度
#define STEERING_CHASSIS_ALIGN_ANGLE_LB STEERING_CHASSIS_ALIGN_ECD_LB / 8192.f * 360.f // 舵轮 D 对齐角度
// 发射参数
#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f