cyy/origincar_controller
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
4dadbeeb5cb59459a6ad213e1af5a17edeffe8c0
origincar_controller/BALANCE/robot_select_init.h
cyy_mac 93ca37e36c 完善阿克曼控制与高速串口遥测 
- 校正舵机中位、转向符号和阿克曼后轮差速模型\n- 增加航向角速度辅助及遥控通道调试开关\n- 将速度环提升至 200Hz，并按实际 dt 计算 PI 积分\n- 将 IMU 启动校准缩短为 2 秒\n- 为 USART3 增加 DMA 发送和 MCU 采样时间戳
2026-08-12 18:51:48 +08:00

155 lines
6.4 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
#ifndef __ROBOTSELECTINIT_H
#define __ROBOTSELECTINIT_H
#include "sys.h"
#include "system.h"
//Parameter structure of robot
//<2F><><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>ṹ<EFBFBD><E1B9B9>
typedef struct
{
float WheelSpacing; //Wheelspacing, Mec_Car is half wheelspacing //<2F>־<EFBFBD> <20><><EFBFBD>ֳ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>־<EFBFBD>
float AxleSpacing; //Axlespacing, Mec_Car is half axlespacing //<2F><><EFBFBD> <20><><EFBFBD>ֳ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD>
int GearRatio; //Motor_gear_ratio //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٱ<EFBFBD>
int EncoderAccuracy; //Number_of_encoder_lines //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
float WheelDiameter; //Diameter of driving wheel //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>
float OmniTurnRadiaus; //Rotation radius of omnidirectional trolley //ȫ<><C8AB><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD>ת<EFBFBD>뾶
}Robot_Parament_InitTypeDef;
// Encoder structure
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><E1B9B9>
typedef struct
{
int A;
int B;
int C;
int D;
}Encoder;
//The minimum turning radius of Ackermann models is determined by the mechanical structure:
//the maximum Angle of the wheelbase, wheelbase and front wheels
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>͵<EFBFBD><CDB5><EFBFBD>Сת<D0A1><D7AA>뾶<EFBFBD><EBBEB6><EFBFBD>ɻ<EFBFBD>е<EFBFBD>ṹ<EFBFBD><E1B9B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־ࡢ<D6BE><E0A1A2>ࡢǰ<E0A1A2><C7B0><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
#define MINI_AKM_MIN_TURN_RADIUS 0.350f
//Wheelspacing, Mec_Car is half wheelspacing
//<2F>־<EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
//#define MEC_wheelspacing 0.109
#define MEC_wheelspacing 0.0930 //<2F><><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
//<2F><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
#define MEC_axlespacing 0.085
#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
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٱ<EFBFBD>
#define HALL_30F 30
#define HALL_60F 60
#define MD36N_5_18 5.18
#define MD36N_27 27
#define MD36N_51 51
#define MD36N_71 71
#define MD60N_18 18
#define MD60N_47 47
//Number_of_encoder_lines
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define Photoelectric_500 500
#define Hall_13 13
//Mecanum wheel tire diameter series
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ̥<CCA5><D6B1>
#define Mecanum_60 0.060f
#define Mecanum_75 0.075f
#define Mecanum_100 0.100f
#define Mecanum_127 0.127f
#define Mecanum_152 0.152f
//Omni wheel tire diameter series
//<2F>־<EFBFBD>ȫ<EFBFBD><C8AB><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>ϵ<EFBFBD><CFB5>
#define FullDirecion_60 0.060
#define FullDirecion_75 0.075
#define FullDirecion_127 0.127
#define FullDirecion_152 0.152
#define FullDirecion_203 0.203
#define FullDirecion_217 0.217
//Black tire, tank_car wheel diameter
//<2F><>ɫ<EFBFBD><C9AB>̥<EFBFBD><CCA5><EFBFBD>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>
#define Black_WheelDiameter 0.065
//#define Tank_WheelDiameter 0.047
#define Tank_WheelDiameter 0.043
//Rotation radius of omnidirectional trolley
//ȫ<><C8AB><EFBFBD><EFBFBD>С<EFBFBD><D0A1><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
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD>ڱ<EFBFBD><DAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define EncoderMultiples 4
//Wheel-speed control and encoder reading frequency
//<2F><><EFBFBD>ٿ<EFBFBD><D9BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡƵ<C8A1><C6B5>
#define CONTROL_FREQUENCY 200
//#define PI 3.1415f //PI //Բ<><D4B2><EFBFBD><EFBFBD>
void Robot_Select(void);
void Robot_Init(double wheelspacing, float axlespacing, float omni_turn_radiaus, float gearratio,float Accuracy,float tyre_diameter);
#endif
Reference in New Issue View Git Blame Copy Permalink