TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

更改了宏定义的文件

Browse Source
This commit is contained in:
NeoZng
2022-12-05 15:03:45 +08:00
parent 8e7935876e
commit 3c8ffda61d
8 changed files with 70 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
application/chassis/chassis.c
View File

@@ -18,18 +18,12 @@
#include "message_center.h"
#include "referee.h"
#include "general_def.h"
#include "bsp_dwt.h"
#include "arm_math.h"
/* 需要根据机器人底盘修改的参数,单位为mm(毫米) */
#define WHEEL_BASE 300 // 纵向轴距(前进后退方向)
#define TRACK_WIDTH 300 // 横向轮距(左右平移方向)
#define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0
#define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0
#define RADIUS_WHEEL 60 // 轮子半径
#define REDUCTION_RATIO 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
/* 自动计算的参数 */
/* 根据robot_def.h中的macro自动计算的参数 */
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f)
#define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI)
@@ -54,6 +48,9 @@ static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
static Subscriber_t *chassis_sub;
static Chassis_Upload_Data_s chassis_feedback_data;
/* 用于自旋变速策略的时间变量,后续考虑查表加速 */
static float t;
/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘
static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
@@ -173,8 +170,8 @@ static void MecanumCalculate()
{
vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER;
vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER;
vt_lb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * LB_CENTER;
vt_rb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
vt_lb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * LB_CENTER;
vt_rb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
}
/**
@@ -215,31 +212,31 @@ void ChassisTask()
// 后续增加不同状态的过渡模式?
switch (chassis_cmd_recv.chassis_mode)
{
case CHASSIS_NO_FOLLOW:
chassis_cmd_recv.wz = 0; // 云台任意移动模式,不旋转,一般用于调整云台姿态
break;
case CHASSIS_FOLLOW_GIMBAL_YAW:
chassis_cmd_recv.wz = 0.05f * powf(chassis_cmd_recv.wz, 2.0f); // 跟随,不开pid,以误差角平方为输出
break;
case CHASSIS_ROTATE:
// chassis_cmd_recv.wz // 当前维持定值,后续增加不规则的变速策略
break;
case CHASSIS_ZERO_FORCE:
DJIMotorStop(motor_lf); // 如果出现重要模块离线或遥控器设置为急停,让电机停止
DJIMotorStop(motor_rf);
DJIMotorStop(motor_lb);
DJIMotorStop(motor_rb);
break;
case CHASSIS_NO_FOLLOW:
chassis_cmd_recv.wz = 0; // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
break;
case CHASSIS_FOLLOW_GIMBAL_YAW:
chassis_cmd_recv.wz = 0.05f * powf(chassis_cmd_recv.wz, 2.0f); // 跟随,不单独设置pid,以误差角平方为速度输出
break;
case CHASSIS_ROTATE:
// chassis_cmd_recv.wz=sin(t) // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
break;
default:
break;
}
// 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
// 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
chassis_vx = chassis_cmd_recv.vx * arm_cos_f32(chassis_cmd_recv.offset_angle) -
chassis_cmd_recv.vy * arm_sin_f32(chassis_cmd_recv.offset_angle);
chassis_vy = chassis_cmd_recv.vx * arm_sin_f32(chassis_cmd_recv.offset_angle) -
chassis_cmd_recv.vy * arm_cos_f32(chassis_cmd_recv.offset_angle);
chassis_vx = chassis_cmd_recv.vx * arm_cos_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD) -
chassis_cmd_recv.vy * arm_sin_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD);
chassis_vy = chassis_cmd_recv.vx * arm_sin_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD) -
chassis_cmd_recv.vy * arm_cos_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD);
// 根据控制模式进行正运动学解算,计算底盘输出
MecanumCalculate();
@@ -251,8 +248,9 @@ void ChassisTask()
EstimateSpeed();
// 获取裁判系统数据
// 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
chassis_feedback_data.enemy_color = referee_data->GameRobotStat.robot_id > 7 ? 1 : 0; //
// 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
chassis_feedback_data.enemy_color = referee_data->GameRobotStat.robot_id > 7 ? 1 : 0;
// 当前只做了17mm的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
chassis_feedback_data.bullet_speed = referee_data->GameRobotStat.shooter_id1_17mm_speed_limit;
chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;