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

!2 delete +3

Browse Source 
Merge pull request !2 from TuxMonkey/dev
This commit is contained in:
TuxMonkey
2026-03-09 18:25:42 +00:00
committed by Gitee
parent 5d0256e947 a22fa03e1d
commit 351dfc6b71
9 changed files with 131 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -12,7 +12,7 @@ set(CMAKE_OBJCOPY arm-none-eabi-objcopy)
set(CMAKE_OBJDUMP arm-none-eabi-objdump) set(CMAKE_OBJDUMP arm-none-eabi-objdump)
set(SIZE arm-none-eabi-size) set(SIZE arm-none-eabi-size)
set(CMAKE_AR arm-none-eabi-ar) set(CMAKE_AR arm-none-eabi-ar)
set(CMAKE_C_STANDARD 11) set(CMAKE_C_STANDARD 23)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
# 指定工程名称和语言类型 # 指定工程名称和语言类型

2
README.md
View File

@@ -1,4 +1,4 @@
# YueLu2022/2023 EC basic_framework-dev # basic_framework-dev
湖南大学RoboMaster跃鹿战队2022-2023电控通用嵌入式框架。 湖南大学RoboMaster跃鹿战队2022-2023电控通用嵌入式框架。

114
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 Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
static SuperCapInstance *cap; // 超级电容 static SuperCapInstance *cap; // 超级电容
static LKMotorInstance *motor_lf,*motor_rb; // 轮电机9015 static LKMotorInstance *motor_lb,*motor_rf; // 轮电机9015
static DJIMotorInstance *motor_steering_lf,*motor_steering_rb; // 舵电机6020 static DJIMotorInstance *motor_steering_lb,*motor_steering_rf; // 舵电机6020
/* 用于自旋变速策略的时间变量 */ /* 用于自旋变速策略的时间变量 */
// static float t; // static float t;
/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */ /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘 static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘
static float vt_lf, vt_rb; // 底盘速度解算后的临时输出,待进行限幅 static float vt_lb, vt_rf; // 底盘速度解算后的临时输出,待进行限幅
static float at_lf, at_rb; // 底盘的角度解算后的临时输出,待进行限幅 static float at_lb, at_rf; // 底盘的角度解算后的临时输出,待进行限幅
static PIDInstance chassis_follow_pid; // 底盘跟随云台的PID static PIDInstance chassis_follow_pid; // 底盘跟随云台的PID
void ChassisInit() void ChassisInit()
@@ -102,11 +102,11 @@ void ChassisInit()
chassis_motor_config.can_init_config.tx_id = 1; chassis_motor_config.can_init_config.tx_id = 1;
// chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; // chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
motor_rb = LKMotorInit(&chassis_motor_config); motor_rf = LKMotorInit(&chassis_motor_config);
chassis_motor_config.can_init_config.tx_id = 2; chassis_motor_config.can_init_config.tx_id = 2;
// chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; // chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
motor_lf = LKMotorInit(&chassis_motor_config); motor_lb = LKMotorInit(&chassis_motor_config);
// 舵电机6020初始化 // 舵电机6020初始化
Motor_Init_Config_s chassis_motor_steering_config = { Motor_Init_Config_s chassis_motor_steering_config = {
@@ -145,16 +145,16 @@ void ChassisInit()
}; };
chassis_motor_steering_config.can_init_config.tx_id = 1; 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; 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 = { PID_Init_Config_s chassis_follow_pid_conf = {
.Kp = 20, // 6 .Kp = 1.05, // 6
.Ki = 0, .Ki = 0,
.Kd = 3, // 0.5 .Kd = 0.1, // 0.495
.DeadBand = 0.5f, .DeadBand = 0.5f,// 启用死区，值为0.5度
//.CoefA = 0.2, //.CoefA = 0.2,
//.CoefB = 0.3, //.CoefB = 0.3,
//.Improve = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement, //.Improve = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement,
@@ -232,7 +232,7 @@ static void MinmizeRotation(float *angle, const float *last_angle, float *speed)
while (norm_rotation < -180.0f) { while (norm_rotation < -180.0f) {
norm_rotation += 360.0f; norm_rotation += 360.0f;
} }
float threshold = 90.0f; float threshold = 110.0f;
float target_single=fmodf(target_angle,360.0f); float target_single=fmodf(target_angle,360.0f);
if(target_single<0.0f) target_single+=360.0f; if(target_single<0.0f) target_single+=360.0f;
@@ -240,16 +240,16 @@ static void MinmizeRotation(float *angle, const float *last_angle, float *speed)
if(current_single<0.0f) current_single+=360.0f; if(current_single<0.0f) current_single+=360.0f;
if (fabsf(norm_rotation) > threshold) { if (norm_rotation > threshold) {
int32_t round_diff = (int32_t)(target_angle / 360.0f) - (int32_t)(actual_angle / 360.0f); int32_t round_diff = (int32_t)((target_angle-actual_angle) / 360.0f);
*angle = actual_angle + norm_rotation-180.0f+round_diff*360.0f; *angle = actual_angle + norm_rotation-180.0f+round_diff*360.0f;
*speed = -(*speed); *speed = -(*speed);
}else if(norm_rotation<-threshold){ }else if(norm_rotation<-threshold){
int32_t round_diff = (int32_t)(target_angle / 360.0f) - (int32_t)(actual_angle / 360.0f); int32_t round_diff = (int32_t)((target_angle-actual_angle) / 360.0f);
*angle = actual_angle + norm_rotation+180.0f+round_diff*360.0f; *angle = actual_angle + norm_rotation+180.0f+round_diff*360.0f;
*speed = -(*speed); *speed = -(*speed);
} }
ANGLE_LIMIT_360_TO_180_ABS(*angle); // ANGLE_LIMIT_360_TO_180_ABS(*angle);
// float rotation = *angle - *last_angle; // float rotation = *angle - *last_angle;
// ANGLE_LIMIT_360_TO_180_ABS(rotation); // ANGLE_LIMIT_360_TO_180_ABS(rotation);
// if (rotation > 90) { // if (rotation > 90) {
@@ -268,44 +268,57 @@ static void MinmizeRotation(float *angle, const float *last_angle, float *speed)
*/ */
static void SteeringWheelCalculate() static void SteeringWheelCalculate()
{ {
float offset_lf, offset_rb; // 用于计算舵轮的角度 float offset_rf, offset_lb; // 用于计算舵轮的角度
float at_lf_last, at_rb_last; // 上次的角度 float at_rf_last, at_lb_last; // 上次的角度
at_lf_last = motor_steering_lf->measure.total_angle; at_lb_last = motor_steering_lb->measure.total_angle;
at_rb_last = motor_steering_rb->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) { if (chassis_vx == 0 && chassis_vy == 0 && chassis_cmd_recv.wz == 0) {
// at_lf = at_lf_last; // at_lf = at_lf_last;
// at_rf = at_rf_last; // at_rf = at_rf_last;
// at_lb = at_lb_last; // at_lb = at_lb_last;
// at_rb = at_rb_last; // at_rb = at_rb_last;
vt_lf = 0, vt_rb = 0; vt_lb = 0, vt_rf = 0;
} else { } else {
// 生成预计算变量，减少计算量，空间换时间 // 生成预计算变量，减少计算量，空间换时间
float w = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2; float w = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2;
float temp_x = chassis_vx - w, temp_y = chassis_vy - w; 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- // 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;与上次相同因此注释 // 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- 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;与上次相同因此注释 temp_x = 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_y = chassis_vy - w; // temp_x = chassis_vy + w;与上次相同因此注释 // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rb); // rb: y+ , x+
// arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rf); // rf: 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_lf = atan2f(chassis_vy - w, chassis_vx - w) * RAD_2_DEGREE; // lf: y- , x-
offset_rb = atan2f(chassis_vx + w, chassis_vy + w) * RAD_2_DEGREE; // rb: 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_lf = STEERING_CHASSIS_ALIGN_ANGLE_LF + offset_lf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度
at_rb = STEERING_CHASSIS_ALIGN_ANGLE_RB + offset_rb; 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_lf);
ANGLE_LIMIT_360_TO_180_ABS(at_rb); 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_lf, &at_lf_last, &vt_lf);
MinmizeRotation(&at_rb, &at_rb_last, &vt_rb); 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_lf, at_lf);
DJIMotorSetRef(motor_steering_rb, at_rb); DJIMotorSetRef(motor_steering_rf, at_rf);
DJIMotorSetRef(motor_steering_lb, at_lb);
// DJIMotorSetRef(motor_steering_rb, at_rb);
} }
/** /**
@@ -335,8 +348,8 @@ static void LimitChassisOutput()
// LKMotorSetRef( motor_lf , vt_lf ); // LKMotorSetRef( motor_lf , vt_lf );
// LKMotorSetRef( motor_rb , vt_rb ); // LKMotorSetRef( motor_rb , vt_rb );
LKMotorSetRef( motor_lf , vt_lf ); LKMotorSetRef( motor_rf , vt_rf );
LKMotorSetRef( motor_rb , vt_rb ); LKMotorSetRef( motor_lb , vt_lb );
} }
/** /**
@@ -365,18 +378,18 @@ void ChassisTask()
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
{ // 如果出现重要模块离线或遥控器设置为急停,让电机停止 { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
LKMotorStop(motor_lf); LKMotorStop(motor_rf);
LKMotorStop(motor_rb); LKMotorStop(motor_lb);
DJIMotorStop(motor_steering_lf); DJIMotorStop(motor_steering_rf);
DJIMotorStop(motor_steering_rb); DJIMotorStop(motor_steering_lb);
} }
else else
{ {
// 正常工作 // 正常工作
LKMotorEnable(motor_lf); LKMotorEnable(motor_rf);
LKMotorEnable(motor_rb); LKMotorEnable(motor_lb);
DJIMotorEnable(motor_steering_lf); DJIMotorEnable(motor_steering_rf);
DJIMotorEnable(motor_steering_rb); DJIMotorEnable(motor_steering_lb);
} }
// 根据控制模式设定旋转速度 // 根据控制模式设定旋转速度
@@ -386,8 +399,8 @@ void ChassisTask()
chassis_cmd_recv.wz = 0; chassis_cmd_recv.wz = 0;
break; break;
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
// float single_round_offset = chassis_cmd_recv.offset_angle;
float single_round_offset = chassis_cmd_recv.offset_angle; float single_round_offset = chassis_cmd_recv.offset_angle;
// float single_round_offset = fmodf(chassis_cmd_recv.offset_angle, 360.0f);
if(single_round_offset > 180.0f) if(single_round_offset > 180.0f)
{ {
single_round_offset -= 360.0f; single_round_offset -= 360.0f;
@@ -395,7 +408,6 @@ void ChassisTask()
else if(single_round_offset < -180.0f) else if(single_round_offset < -180.0f)
{ {
single_round_offset += 360.0f; single_round_offset += 360.0f;
} }
chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/100; chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/100;
// chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0)/100; // chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0)/100;
@@ -413,7 +425,7 @@ void ChassisTask()
cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD); 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 = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta; 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; chassis_vy = -chassis_cmd_recv.vx * sin_theta - chassis_cmd_recv.vy * cos_theta;
// 根据控制模式进行正运动学解算,计算底盘输出 // 根据控制模式进行正运动学解算,计算底盘输出
SteeringWheelCalculate(); SteeringWheelCalculate();

54
application/cmd/robot_cmd.c
View File

@@ -15,7 +15,7 @@
// 私有宏,自动将编码器转换成角度值 // 私有宏,自动将编码器转换成角度值
#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360 #define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360
#define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360 #define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360
uint16_t around=0;
/* cmd应用包含的模块实例指针和交互信息存储*/ /* cmd应用包含的模块实例指针和交互信息存储*/
#ifdef GIMBAL_BOARD // 对双板的兼容,条件编译 #ifdef GIMBAL_BOARD // 对双板的兼容,条件编译
#include "can_comm.h" #include "can_comm.h"
@@ -30,8 +30,8 @@ static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信
static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等 static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回 static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回
static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回 // static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回
static Vision_Send_s vision_send_data; // 视觉发送数据 // static Vision_Send_s vision_send_data; // 视觉发送数据
static Publisher_t *gimbal_cmd_pub; // 云台控制消息发布者 static Publisher_t *gimbal_cmd_pub; // 云台控制消息发布者
static Subscriber_t *gimbal_feed_sub; // 云台反馈信息订阅者 static Subscriber_t *gimbal_feed_sub; // 云台反馈信息订阅者
@@ -48,7 +48,7 @@ static Robot_Status_e robot_state; // 机器人整体工作状态
void RobotCMDInit() void RobotCMDInit()
{ {
rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个 rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
vision_recv_data = VisionInit(&huart1); // 视觉通信串口 // vision_recv_data = VisionInit(&huart1); // 视觉通信串口
gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
@@ -80,13 +80,26 @@ void RobotCMDInit()
* @brief 根据gimbal app传回的当前电机角度计算和零位的误差 * @brief 根据gimbal app传回的当前电机角度计算和零位的误差
* 单圈绝对角度的范围是0~360,说明文档中有图示 * 单圈绝对角度的范围是0~360,说明文档中有图示
* *
*/ *///static float base_total_angle = 0.0f; // 用于计算偏移的基准角度
static void CalcOffsetAngle() static void CalcOffsetAngle()
{ {
static float last_offset_angle = 0; static float last_offset_angle = 0;
const float smooth_factor = 0.3f; // 平滑因子,0~1之间,值越大平滑效果越明显 const float smooth_factor = 0.3f; // 平滑因子,0~1之间,值越大平滑效果越明显
const float deadband = 3.0f; // 死区值
// 新增：跟踪模式变化，用于重置基准角度
// static chassis_mode_e last_chassis_mode = CHASSIS_NO_FOLLOW;//
//
//
// // 当从旋转模式切换到跟随模式时，重置基准角度
// if (last_chassis_mode == CHASSIS_ROTATE && chassis_cmd_send.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW)
// {
// base_total_angle = gimbal_fetch_data.yaw_motor_total_angle;
// last_offset_angle = 0.0f; // 同时重置历史偏移值
// }
float new_offset = gimbal_fetch_data.yaw_motor_total_angle - YAW_ALIGN_ANGLE; float new_offset = gimbal_fetch_data.yaw_motor_total_angle - YAW_ALIGN_ANGLE;
#if YAW_CHASSIS_REVERSE #if YAW_ECD_GREATER_THAN_4096
new_offset = -new_offset; new_offset = -new_offset;
#endif #endif
float angle_diff = new_offset - last_offset_angle; float angle_diff = new_offset - last_offset_angle;
@@ -94,8 +107,25 @@ static void CalcOffsetAngle()
new_offset -= 360.0f; new_offset -= 360.0f;
else if (angle_diff < -180.0f) else if (angle_diff < -180.0f)
new_offset += 360.0f; new_offset += 360.0f;
/////
// 应用死区
if (fabs(new_offset) < deadband) {
new_offset = 0.0f;
} else if (new_offset > 0) {
new_offset -= deadband;
} else {
new_offset += deadband;
}//////
chassis_cmd_send.offset_angle = last_offset_angle *(1.0f - smooth_factor) + new_offset * smooth_factor; chassis_cmd_send.offset_angle = last_offset_angle *(1.0f - smooth_factor) + new_offset * smooth_factor;
last_offset_angle = chassis_cmd_send.offset_angle; last_offset_angle = chassis_cmd_send.offset_angle;
// // 更新模式状态
// last_chassis_mode = chassis_cmd_send.chassis_mode;
// // 当处于旋转模式时，更新基准角度以防止累积过大误差
// if (chassis_cmd_send.chassis_mode == CHASSIS_ROTATE)
// {
// base_total_angle = gimbal_fetch_data.yaw_motor_total_angle;
// }
// static float angle; // static float angle;
// static float last_diff = 0.0f; // 用于在±180°附近保持上次符号，避免来回跳变 // static float last_diff = 0.0f; // 用于在±180°附近保持上次符号，避免来回跳变
// /* 使用 yaw 电机的单圈角度与 `YAW_ALIGN_ANGLE` 对齐（保持原实现的参考系） // /* 使用 yaw 电机的单圈角度与 `YAW_ALIGN_ANGLE` 对齐（保持原实现的参考系）
@@ -151,11 +181,15 @@ static void RemoteControlSet()
// 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据? // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据?
if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台 if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台
{ {
// around = 0;
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
around= chassis_cmd_send.offset_angle/360;
} }
else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动
{ {
//gimbal_fetch_data.yaw_motor_total_angle = gimbal_fetch_data.yaw_motor_total_angle-around*360;
chassis_cmd_send.offset_angle = chassis_cmd_send.offset_angle - 360*around;
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
} }
@@ -167,7 +201,7 @@ static void RemoteControlSet()
// ... // ...
} }
// 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制 // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制
if (switch_is_down(rc_data[TEMP].rc.switch_left) || vision_recv_data->target_state == NO_TARGET) if (switch_is_down(rc_data[TEMP].rc.switch_left) )
{ // 按照摇杆的输出大小进行角度增量,增益系数需调整 { // 按照摇杆的输出大小进行角度增量,增益系数需调整
gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_; gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_;
gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1; gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1;
@@ -175,8 +209,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_r_; // _水平方向
chassis_cmd_send.vx = 0.15f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向 chassis_cmd_send.vy = 0.15f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向
// // 发射参数 // // 发射参数
// if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 // if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
@@ -347,5 +381,5 @@ void RobotCMDTask()
#endif // GIMBAL_BOARD #endif // GIMBAL_BOARD
PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send); PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send); PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
VisionSend(&vision_send_data); // VisionSend(&vision_send_data);
} }

14
application/gimbal/gimbal.c
View File

@@ -26,8 +26,10 @@ void GimbalInit()
}, },
.controller_param_init_config = { .controller_param_init_config = {
.angle_PID = { .angle_PID = {
//.Kp = 0.685, // 8
.Kp = 0.685, // 8 .Kp = 0.685, // 8
.Ki = 0.315, //.Ki = 0.315,
.Ki = 0.25,
.Kd = 0.022, .Kd = 0.022,
//.DeadBand = 0.1, //.DeadBand = 0.1,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_ChangingIntegrationRate, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_ChangingIntegrationRate,
@@ -39,6 +41,7 @@ void GimbalInit()
}, },
.speed_PID = { .speed_PID = {
.Kp = 8000, // 50 .Kp = 8000, // 50
//.Kp = 5000, // 50
.Ki = 5000, // 200 .Ki = 5000, // 200
.Kd = 0, .Kd = 0,
.CoefB = 1, .CoefB = 1,
@@ -56,7 +59,7 @@ void GimbalInit()
.speed_feedback_source = OTHER_FEED, .speed_feedback_source = OTHER_FEED,
.outer_loop_type = ANGLE_LOOP, .outer_loop_type = ANGLE_LOOP,
.close_loop_type = ANGLE_LOOP | SPEED_LOOP, .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, .motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
}, },
.motor_type = GM6020}; .motor_type = GM6020};
// PITCH // PITCH
@@ -93,7 +96,7 @@ void GimbalInit()
.close_loop_type = SPEED_LOOP | ANGLE_LOOP, .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
}, },
.motor_type = GM6020, .motor_type = M3508,
}; };
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
yaw_motor = DJIMotorInit(&yaw_config); yaw_motor = DJIMotorInit(&yaw_config);
@@ -151,8 +154,9 @@ void GimbalTask()
// 设置反馈数据,主要是imu和yaw的ecd // 设置反馈数据,主要是imu和yaw的ecd
gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
// gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round; gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.total_angle; gimbal_feedback_data.yaw_motor_total_angle = yaw_motor->measure.total_angle;
// gimbal_feedback_data.yaw_motor_total_angle = gimba_IMU_data->YawTotalAngle;
// 推送消息 // 推送消息
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data); PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
} }

12
application/robot_def.h
View File

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

2
application/shoot/shoot.c
View File

@@ -96,7 +96,7 @@ void ShootInit()
.close_loop_type = CURRENT_LOOP | SPEED_LOOP, .close_loop_type = CURRENT_LOOP | SPEED_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向 .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
}, },
.motor_type = M2006 // 英雄使用m3508 .motor_type = M3508 // 英雄使用m3508
}; };
loader = DJIMotorInit(&loader_config); loader = DJIMotorInit(&loader_config);

2
modules/led/led.h
View File

@@ -28,6 +28,6 @@ void LEDSet(LEDInstance *_led,uint8_t alpha,uint8_t color_value,uint8_t brightne
void LEDSwitch(LEDInstance *_led,uint8_t led_switch); void LEDSwitch(LEDInstance *_led,uint8_t led_switch);
void LEDShow(); void LEDShow(uint32_t aRGB);
#endif // !_LED_H_ #endif // !_LED_H_

2
modules/remote/remote_control.c
View File

@@ -40,7 +40,7 @@ static void sbus_to_rc(const uint8_t *sbus_buf)
rc_ctrl[TEMP].rc.rocker_l_ = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 2 rc_ctrl[TEMP].rc.rocker_l_ = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 2
rc_ctrl[TEMP].rc.rocker_l1 = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 3 rc_ctrl[TEMP].rc.rocker_l1 = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 3
rc_ctrl[TEMP].rc.dial = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF) - RC_CH_VALUE_OFFSET; rc_ctrl[TEMP].rc.dial = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF) - RC_CH_VALUE_OFFSET;
rc_ctrl[TEMP].rc.rocker_l_= rc_ctrl[TEMP].rc.rocker_l_+3; rc_ctrl[TEMP].rc.rocker_l_= rc_ctrl[TEMP].rc.rocker_l_;
// 左侧拨轮 // 左侧拨轮
RectifyRCjoystick(); RectifyRCjoystick();
// 开关,0左1右 // 开关,0左1右