cyy/origincar_controller
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

完善阿克曼控制与高速串口遥测

Browse Source 
- 校正舵机中位、转向符号和阿克曼后轮差速模型\n- 增加航向角速度辅助及遥控通道调试开关\n- 将速度环提升至 200Hz，并按实际 dt 计算 PI 积分\n- 将 IMU 启动校准缩短为 2 秒\n- 为 USART3 增加 DMA 发送和 MCU 采样时间戳
This commit is contained in:
cyy_mac
2026-08-12 18:51:48 +08:00
parent 576784bb4d
commit 93ca37e36c
17 changed files with 2151 additions and 932 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
BALANCE/robot_select_init.h
View File

@@ -4,19 +4,19 @@
#include "system.h"
//Parameter structure of robot
//机器人参数结构体
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><EFBFBD>
typedef struct
{
float WheelSpacing; //Wheelspacing, Mec_Car is half wheelspacing //轮距 麦轮车为半轮距
float AxleSpacing; //Axlespacing, Mec_Car is half axlespacing //轴距 麦轮车为半轴距
int GearRatio; //Motor_gear_ratio //电机减速比
int EncoderAccuracy; //Number_of_encoder_lines //编码器精度(编码器线数)
float WheelDiameter; //Diameter of driving wheel //主动轮直径
float OmniTurnRadiaus; //Rotation radius of omnidirectional trolley //全向轮小车旋转半径
float WheelSpacing; //Wheelspacing, Mec_Car is half wheelspacing //<EFBFBD>־<EFBFBD> <20><><EFBFBD>ֳ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>־<EFBFBD>
float AxleSpacing; //Axlespacing, Mec_Car is half axlespacing //<EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>ֳ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD>
int GearRatio; //Motor_gear_ratio //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٱ<EFBFBD>
int EncoderAccuracy; //Number_of_encoder_lines //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
float WheelDiameter; //Diameter of driving wheel //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><EFBFBD>
float OmniTurnRadiaus; //Rotation radius of omnidirectional trolley //ȫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>뾶
}Robot_Parament_InitTypeDef;
// Encoder structure
//编码器结构体
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><EFBFBD>
typedef struct
{
int A;
@@ -27,28 +27,74 @@ typedef struct
//The minimum turning radius of Ackermann models is determined by the mechanical structure:
//the maximum Angle of the wheelbase, wheelbase and front wheels
//阿克曼车型的最小转弯半径，由机械结构决定：轮距、轴距、前轮最大转角
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>͵<EFBFBD><EFBFBD><EFBFBD>Сת<EFBFBD><EFBFBD>뾶<EFBFBD><EFBFBD><EFBFBD>ɻ<EFBFBD>е<EFBFBD>ṹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־ࡢ<EFBFBD><EFBFBD>ࡢǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>
#define MINI_AKM_MIN_TURN_RADIUS 0.350f
//Wheelspacing, Mec_Car is half wheelspacing
//轮距 麦轮是一半
//<EFBFBD>־<EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
//#define MEC_wheelspacing 0.109
#define MEC_wheelspacing 0.0930 //修正2021.03.30
#define Akm_wheelspacing 0.162f
#define MEC_wheelspacing 0.0930 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>2021.03.30
#define Akm_wheelspacing 0.160f
#define Diff_wheelSpacing 0.177f
#define Four_Mortor_wheelSpacing 0.26f
#define Tank_wheelSpacing 0.235f
//Axlespacing, Mec_Car is half axlespacing
//轴距 麦轮是一半
//<EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
#define MEC_axlespacing 0.085
#define Akm_axlespacing 0.158f
#define Akm_axlespacing 0.160f
// Set to 1 to drive the Ackermann servo directly from TIM8 channel 1 (bench debug).
// Set to 0 to use the calibrated curvature->servo model in balance.c.
#define AKM_SERVO_DEBUG_REMOTE_CH1 0
// Ackermann control law selector (mutually exclusive with the debug switch above):
// 0 = calibrated kinematic model: kappa = wz/Vx -> quadratic servo fit,
// rear wheels get Ackermann differential. Physically correct.
// 1 = direct passthrough (tuning/debug): Vz is linearly mapped across the
// full servo travel, Vx is sent to both drive wheels unchanged (no
// differential, no curvature math). Handy for isolating servo/motor.
#define AKM_DIRECT_MAP 0
// Direct-map input span: |Vz| >= AKM_DIRECT_VZ_FULL maps to the servo end stop.
// Vz > 0 = left (ROS), which maps toward AKM_SERVO_MIN (left end).
// TUNING KNOB (Mode 1 & Mode 2 share it): set this to the MAX angular.z [rad/s]
// your commander actually sends, so a full stick/command uses the full servo
// travel. Too high -> steering stays small; too low -> servo saturates (always
// full lock) and loses proportional control. Vz arrives in rad/s (usartx.c
// XYZ_Target_Speed_transition: raw/1000).
#define AKM_DIRECT_VZ_FULL 1.0f
// Ackermann yaw-rate closed-loop assist (Mode 2), mutually exclusive with
// AKM_DIRECT_MAP (direct-map wins if both are 1). "Front wheel does the main
// steering, rear wheels add a yaw-rate differential" -- a simplified torque-
// vectoring / yaw-rate closed loop:
// path -> (Vx, kappa_cmd) -> servo main steering (calibrated fit)
// + IMU yaw-rate PI differential on the rear wheels.
// Degenerates EXACTLY to the Mode-0 Ackermann differential when
// AKM_YAW_FF_ALPHA = 1 and AKM_YAW_KP = AKM_YAW_KI = 0.
#define AKM_YAW_ASSIST 1
// PI gains on the yaw-rate error e_r = r_ref - r_imu [rad/s], output in m/s.
#define AKM_YAW_KP 0.10f
#define AKM_YAW_KI 0.00f
// Feedforward blend: 0 = pure IMU feedback, 1 = full geometric differential.
#define AKM_YAW_FF_ALPHA 0.00f
// Below this |Vx| the yaw loop is frozen (integrator reset, no differential).
#define AKM_YAW_MIN_SPEED 0.10f
// |dv| clamp as a fraction of |Vx|, so the differential cannot stall a wheel.
#define AKM_YAW_MAX_DIFF_RATIO 0.35f
// gyro[2] LSB -> rad/s at FS +-500 dps (see MPU6050.c: FS_500 -> /3754.9).
#define AKM_GYRO_Z_TO_RADPS 3754.9f
// Light first-order low-pass on the measured yaw rate (0 = none, 1 = no filter
// lag). r_f += beta*(r_meas - r_f). ~0.3 gives gentle smoothing at 200 Hz.
#define AKM_YAW_IMU_LPF 0.30f
// Flip to -1.0f if the IMU +z spins opposite to the ROS convention (+ = left).
// MUST be verified on hardware: command a left turn and confirm gyro[2] > 0.
#define AKM_GYRO_Z_SIGN (+1.0f)
#define Diff_axlespacing 0.155f
#define Four_Mortor__axlespacing 0.28f
#define Tank_axlespacing 0.222f
//Motor_gear_ratio
//电机减速比
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٱ<EFBFBD>
#define HALL_30F 30
#define HALL_60F 60
#define MD36N_5_18 5.18
@@ -59,12 +105,12 @@ typedef struct
#define MD60N_47 47
//Number_of_encoder_lines
//编码器精度
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define Photoelectric_500 500
#define Hall_13 13
//Mecanum wheel tire diameter series
//麦轮轮胎直径
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ̥<EFBFBD><EFBFBD>
#define Mecanum_60 0.060f
#define Mecanum_75 0.075f
#define Mecanum_100 0.100f
@@ -72,7 +118,7 @@ typedef struct
#define Mecanum_152 0.152f
//Omni wheel tire diameter series
//轮径全向轮直径系列
//<EFBFBD>־<EFBFBD>ȫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD>
#define FullDirecion_60 0.060
#define FullDirecion_75 0.075
#define FullDirecion_127 0.127
@@ -81,26 +127,26 @@ typedef struct
#define FullDirecion_217 0.217
//Black tire, tank_car wheel diameter
//黑色轮胎、履带车轮直径
//<EFBFBD><EFBFBD>ɫ<EFBFBD><EFBFBD>̥<EFBFBD><EFBFBD><EFBFBD>Ĵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><EFBFBD>
#define Black_WheelDiameter 0.065
//#define Tank_WheelDiameter 0.047
#define Tank_WheelDiameter 0.043
//Rotation radius of omnidirectional trolley
//全向轮小车旋转半径
//ȫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>뾶
#define Omni_Turn_Radiaus_109 0.109
#define Omni_Turn_Radiaus_164 0.164
#define Omni_Turn_Radiaus_180 0.180
#define Omni_Turn_Radiaus_290 0.290
//The encoder octave depends on the encoder initialization Settings
//编码器倍频数，取决于编码器初始化设置
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD>ڱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define EncoderMultiples 4
//Encoder data reading frequency
//编码器数据读取频率
#define CONTROL_FREQUENCY 100
//Wheel-speed control and encoder reading frequency
//<EFBFBD><EFBFBD><EFBFBD>ٿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡƵ<EFBFBD><EFBFBD>
#define CONTROL_FREQUENCY 200
//#define PI 3.1415f //PI //圆周率
//#define PI 3.1415f //PI //Բ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void Robot_Select(void);
void Robot_Init(double wheelspacing, float axlespacing, float omni_turn_radiaus, float gearratio,float Accuracy,float tyre_diameter);