完善阿克曼控制与高速串口遥测
- 校正舵机中位、转向符号和阿克曼后轮差速模型\n- 增加航向角速度辅助及遥控通道调试开关\n- 将速度环提升至 200Hz，并按实际 dt 计算 PI 积分\n- 将 IMU 启动校准缩短为 2 秒\n- 为 USART3 增加 DMA 发送和 MCU 采样时间戳
This commit is contained in:
cyy_mac
File diff suppressed because it is too large
Load Diff
@@ -22,10 +22,10 @@ float target_limit_float(float insert,float low,float high);
|
||||
int target_limit_int(int insert,int low,int high);
|
||||
u8 Turn_Off( int voltage);
|
||||
u32 myabs(long int a);
|
||||
int Incremental_PI_A (float Encoder,float Target);
|
||||
int Incremental_PI_B (float Encoder,float Target);
|
||||
int Incremental_PI_C (float Encoder,float Target);
|
||||
int Incremental_PI_D (float Encoder,float Target);
|
||||
int Incremental_PI_A (float Encoder,float Target,float dt);
|
||||
int Incremental_PI_B (float Encoder,float Target,float dt);
|
||||
int Incremental_PI_C (float Encoder,float Target,float dt);
|
||||
int Incremental_PI_D (float Encoder,float Target,float dt);
|
||||
void Get_RC(void);
|
||||
void Remote_Control(void);
|
||||
void Drive_Motor(float Vx,float Vy,float Vz);
|
||||
@@ -36,4 +36,3 @@ void PS2_control(void);
|
||||
float float_abs(float insert);
|
||||
void robot_mode_check(void);
|
||||
#endif
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -26,8 +26,8 @@ void show_task(void *pvParameters)
|
||||
|
||||
//开机时蜂鸣器短暂蜂鸣，开机提醒
|
||||
//The buzzer will beep briefly when the machine is switched on
|
||||
if(Time_count<50)Buzzer=1;
|
||||
else if(Time_count>=51 && Time_count<100)Buzzer=0;
|
||||
if(Time_count<(CONTROL_FREQUENCY/2))Buzzer=1;
|
||||
else if(Time_count>=(CONTROL_FREQUENCY/2) && Time_count<CONTROL_FREQUENCY)Buzzer=0;
|
||||
|
||||
if(LowVoltage_1==1 || LowVoltage_2==1)Buzzer_count=0;
|
||||
if(Buzzer_count<5)Buzzer_count++;
|
||||
@@ -340,5 +340,3 @@ void APP_Show(void)
|
||||
printf("{B%d:%d:%d}$",(int)gyro[0],(int)gyro[1],(int)gyro[2]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
124
BALANCE/system.c
124
BALANCE/system.c
@@ -1,197 +1,203 @@
|
||||
#include "system.h"
|
||||
|
||||
//Robot software fails to flag bits
|
||||
//机器人软件失能标志位
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD>ܱ<EFBFBD>־λ
|
||||
u8 Flag_Stop=1;
|
||||
|
||||
//The ADC value is variable in segments, depending on the number of car models. Currently there are 6 car models
|
||||
//ADC值分段变量，取决于小车型号数量，目前有6种小车型号
|
||||
//ADCֵ<EFBFBD>ֶα<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD>ͺ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀǰ<EFBFBD><EFBFBD>6<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD>ͺ<EFBFBD>
|
||||
int Divisor_Mode;
|
||||
|
||||
// Robot type variable
|
||||
//机器人型号变量
|
||||
//0=Mec_Car，1=Omni_Car，2=Akm_Car，3=Diff_Car，4=FourWheel_Car，5=Tank_Car
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͺű<EFBFBD><EFBFBD><EFBFBD>
|
||||
//0=Mec_Car<EFBFBD><EFBFBD>1=Omni_Car<EFBFBD><EFBFBD>2=Akm_Car<EFBFBD><EFBFBD>3=Diff_Car<EFBFBD><EFBFBD>4=FourWheel_Car<EFBFBD><EFBFBD>5=Tank_Car
|
||||
u8 Car_Mode=4;
|
||||
|
||||
//Servo control PWM value, Ackerman car special
|
||||
//舵机控制PWM值，阿克曼小车专用
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PWMֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD>ר<EFBFBD><EFBFBD>
|
||||
int Servo = SERVO_INIT;
|
||||
|
||||
//Default speed of remote control car, unit: mm/s
|
||||
//遥控小车的默认速度，单位：mm/s
|
||||
//ң<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĭ<EFBFBD><EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>mm/s
|
||||
float RC_Velocity=500;
|
||||
|
||||
//Vehicle three-axis target moving speed, unit: m/s
|
||||
//小车三轴目标运动速度，单位：m/s
|
||||
//С<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><EFBFBD>ٶȣ<EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>m/s
|
||||
float Move_X, Move_Y, Move_Z;
|
||||
|
||||
//PID parameters of Speed control
|
||||
//速度控制PID参数
|
||||
float Velocity_KP=300,Velocity_KI=800;
|
||||
//Speed PI parameters: Ki is in PWM/(m/s*s) and multiplies measured dt.
|
||||
//<EFBFBD>ٶ<EFBFBD>PI<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ki<EFBFBD><EFBFBD>λΪPWM/(m/s*s)<29><><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5>dt<64><74><EFBFBD>
|
||||
float Velocity_KP=300,Velocity_KI=80000;
|
||||
|
||||
//Smooth control of intermediate variables, dedicated to omni-directional moving cars
|
||||
//平滑控制中间变量，全向移动小车专用
|
||||
//ƽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȫ<EFBFBD><EFBFBD><EFBFBD>ƶ<EFBFBD>С<EFBFBD><EFBFBD>ר<EFBFBD><EFBFBD>
|
||||
Smooth_Control smooth_control;
|
||||
|
||||
//The parameter structure of the motor
|
||||
//电机的参数结构体
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><EFBFBD>
|
||||
Motor_parameter MOTOR_A,MOTOR_B,MOTOR_C,MOTOR_D;
|
||||
|
||||
/************ 小车型号相关变量 **************************/
|
||||
/************ С<EFBFBD><EFBFBD><EFBFBD>ͺ<EFBFBD><EFBFBD><EFBFBD>ر<EFBFBD><EFBFBD><EFBFBD> **************************/
|
||||
/************ Variables related to car model ************/
|
||||
//Encoder accuracy
|
||||
//编码器精度
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
float Encoder_precision;
|
||||
//Wheel circumference, unit: m
|
||||
//轮子周长，单位：m
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>m
|
||||
float Wheel_perimeter;
|
||||
//Drive wheel base, unit: m
|
||||
//主动轮轮距，单位：m
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־࣬<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>m
|
||||
float Wheel_spacing;
|
||||
//The wheelbase of the front and rear axles of the trolley, unit: m
|
||||
//小车前后轴的轴距，单位：m
|
||||
//С<EFBFBD><EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>࣬<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>m
|
||||
float Axle_spacing;
|
||||
//All-directional wheel turning radius, unit: m
|
||||
//全向轮转弯半径，单位：m
|
||||
//ȫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>뾶<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>m
|
||||
float Omni_turn_radiaus;
|
||||
/************ 小车型号相关变量 **************************/
|
||||
/************ С<EFBFBD><EFBFBD><EFBFBD>ͺ<EFBFBD><EFBFBD><EFBFBD>ر<EFBFBD><EFBFBD><EFBFBD> **************************/
|
||||
/************ Variables related to car model ************/
|
||||
|
||||
//PS2 controller, Bluetooth APP, aircraft model controller, CAN communication, serial port 1, serial port 5 communication control flag bit.
|
||||
//These 6 flag bits are all 0 by default, representing the serial port 3 control mode
|
||||
//PS2手柄、蓝牙APP、航模手柄、CAN通信、串口1、串口5通信控制标志位。这6个标志位默认都为0，代表串口3控制模式
|
||||
//PS2<EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>APP<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD>CANͨ<EFBFBD>š<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>5ͨ<EFBFBD>ſ<EFBFBD><EFBFBD>Ʊ<EFBFBD>־λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>6<EFBFBD><EFBFBD><EFBFBD><EFBFBD>־λĬ<EFBFBD>϶<EFBFBD>Ϊ0<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ
|
||||
u8 PS2_ON_Flag=0, APP_ON_Flag=0, Remote_ON_Flag=0, CAN_ON_Flag=0, Usart1_ON_Flag, Usart5_ON_Flag;
|
||||
|
||||
//Bluetooth remote control associated flag bits
|
||||
//蓝牙遥控相关的标志位
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ң<EFBFBD><EFBFBD><EFBFBD><EFBFBD>صı<EFBFBD>־λ
|
||||
u8 Flag_Left, Flag_Right, Flag_Direction=0, Turn_Flag;
|
||||
|
||||
//Sends the parameter's flag bit to the Bluetooth APP
|
||||
//向蓝牙APP发送参数的标志位
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>APP<EFBFBD><EFBFBD><EFBFBD>Ͳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ı<EFBFBD>־λ
|
||||
u8 PID_Send;
|
||||
|
||||
//The PS2 gamepad controls related variables
|
||||
//PS2手柄控制相关变量
|
||||
//PS2<EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ر<EFBFBD><EFBFBD><EFBFBD>
|
||||
float PS2_LX,PS2_LY,PS2_RX,PS2_RY,PS2_KEY;
|
||||
|
||||
//Self-check the relevant flag variables
|
||||
//自检相关标志变量
|
||||
//<EFBFBD>Լ<EFBFBD><EFBFBD><EFBFBD>ر<EFBFBD>־<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
int Check=0, Checking=0, Checked=0, CheckCount=0, CheckPhrase1=0, CheckPhrase2=0;
|
||||
|
||||
//Check the result code
|
||||
//自检结果代码
|
||||
//<EFBFBD>Լ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
long int ErrorCode=0;
|
||||
|
||||
void systemInit(void)
|
||||
{
|
||||
|
||||
// //Interrupt priority group setti ng
|
||||
// //中断优先级分组设置
|
||||
// //<EFBFBD>ж<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
|
||||
//
|
||||
// //Delay function initialization
|
||||
// //延时函数初始化
|
||||
// //<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>
|
||||
delay_init(168);
|
||||
|
||||
//Initialize the hardware interface connected to the LED lamp
|
||||
//初始化与LED灯连接的硬件接口
|
||||
//<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>LED<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>ӿ<EFBFBD>
|
||||
LED_Init();
|
||||
|
||||
//Initialize the hardware interface connected to the buzzer
|
||||
//初始化与蜂鸣器连接的硬件接口
|
||||
//<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>ӿ<EFBFBD>
|
||||
Buzzer_Init();
|
||||
|
||||
//Initialize the hardware interface connected to the enable switch
|
||||
//初始化与使能开关连接的硬件接口
|
||||
//<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD>ܿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>ӿ<EFBFBD>
|
||||
Enable_Pin();
|
||||
|
||||
//Initialize the hardware interface connected to the OLED display
|
||||
//初始化与OLED显示屏连接的硬件接口
|
||||
//<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>OLED<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>ӿ<EFBFBD>
|
||||
OLED_Init();
|
||||
|
||||
//Initialize the hardware interface connected to the user's key
|
||||
//初始化与用户按键连接的硬件接口
|
||||
//<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>ӿ<EFBFBD>
|
||||
KEY_Init();
|
||||
|
||||
//Serial port 1 initialization, communication baud rate 115200,
|
||||
//can be used to communicate with ROS terminal
|
||||
//串口1初始化，通信波特率115200，可用于与ROS端通信
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD>Ų<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>115200<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ROS<EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD>
|
||||
uart1_init(115200);
|
||||
|
||||
//Serial port 2 initialization, communication baud rate 9600,
|
||||
//used to communicate with Bluetooth APP terminal
|
||||
//串口2初始化，通信波特率9600，用于与蓝牙APP端通信
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD>Ų<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>9600<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>APP<EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD>
|
||||
uart2_init(9600);
|
||||
|
||||
//Serial port 3 is initialized and the baud rate is 115200.
|
||||
//Serial port 3 is the default port used to communicate with ROS terminal
|
||||
//串口3初始化，通信波特率115200，串口3为默认用于与ROS端通信的串口
|
||||
uart3_init(115200);
|
||||
//Serial port 3 is initialized and the baud rate is 921600.
|
||||
//Serial port 3 is the default port used to communicate with ROS terminal.
|
||||
//NOTE: the ROS side must open this port at 921600 to match.
|
||||
//<2F><><EFBFBD><EFBFBD>3<EFBFBD><33>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>ͨ<EFBFBD>Ų<EFBFBD><C5B2><EFBFBD><EFBFBD><EFBFBD>921600<30><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3ΪĬ<CEAA><C4AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ROS<4F><53>ͨ<EFBFBD>ŵĴ<C5B5><C4B4>ڡ<EFBFBD>
|
||||
//ע<>⣺ROS <20>˴<CBB4><F2BFAAB4><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD> 921600<30><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0>벻ƥ<EBB2BB>䡣
|
||||
uart3_init(921600);
|
||||
|
||||
//Serial port 5 initialization, communication baud rate 115200,
|
||||
//can be used to communicate with ROS terminal
|
||||
//串口5初始化，通信波特率115200，可用于与ROS端通信
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>5<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD>Ų<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>115200<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ROS<EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD>
|
||||
uart5_init(115200);
|
||||
|
||||
//ADC pin initialization, used to read the battery voltage and potentiometer gear,
|
||||
//potentiometer gear determines the car after the boot of the car model
|
||||
//ADC引脚初始化，用于读取电池电压与电位器档位，电位器档位决定小车开机后的小车适配型号
|
||||
//ADC<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><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͺ<EFBFBD>
|
||||
Adc_Init();
|
||||
Adc_POWER_Init();
|
||||
|
||||
//Initialize the CAN communication interface
|
||||
//CAN通信接口初始化
|
||||
//CANͨ<EFBFBD>Žӿڳ<EFBFBD>ʼ<EFBFBD><EFBFBD>
|
||||
CAN1_Mode_Init(1,7,6,3,0);
|
||||
|
||||
//According to the tap position of the potentiometer, determine which type of car needs to be matched,
|
||||
//and then initialize the corresponding parameters
|
||||
//根据电位器的档位判断需要适配的是哪一种型号的小车，然后进行对应的参数初始化
|
||||
//<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>IJ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>
|
||||
Robot_Select();
|
||||
|
||||
//Encoder A is initialized to read the real time speed of motor C
|
||||
//编码器A初始化，用于读取电机C的实时速度
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>A<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD>C<EFBFBD><EFBFBD>ʵʱ<EFBFBD>ٶ<EFBFBD>
|
||||
Encoder_Init_TIM2();
|
||||
//Encoder B is initialized to read the real time speed of motor D
|
||||
//编码器B初始化，用于读取电机D的实时速度
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>B<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD>D<EFBFBD><EFBFBD>ʵʱ<EFBFBD>ٶ<EFBFBD>
|
||||
Encoder_Init_TIM3();
|
||||
//Encoder C is initialized to read the real time speed of motor B
|
||||
//编码器C初始化，用于读取电机B的实时速度
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>C<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD>B<EFBFBD><EFBFBD>ʵʱ<EFBFBD>ٶ<EFBFBD>
|
||||
Encoder_Init_TIM4();
|
||||
//Encoder D is initialized to read the real time speed of motor A
|
||||
//编码器D初始化，用于读取电机A的实时速度
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>D<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD>A<EFBFBD><EFBFBD>ʵʱ<EFBFBD>ٶ<EFBFBD>
|
||||
Encoder_Init_TIM5();
|
||||
|
||||
//定时器12用作舵机的PWM接口
|
||||
TIM12_SERVO_Init(9999,84-1); //APB1的时钟频率为84M , 频率=84M/((9999+1)*(83+1))=100Hz
|
||||
//<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>12<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PWM<EFBFBD>ӿ<EFBFBD>
|
||||
TIM12_SERVO_Init(9999,84-1); //APB1<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>Ϊ84M , Ƶ<EFBFBD><EFBFBD>=84M/((9999+1)*(83+1))=100Hz
|
||||
|
||||
//普通小车默认定时器8用作航模接口
|
||||
// TIM8_SERVO_Init(9999,168-1);//APB2的时钟频率为168M , 频率=168M/((9999+1)*(167+1))=100Hz
|
||||
//Initialize the model remote control interface
|
||||
//初始化航模遥控接口
|
||||
TIM8_Cap_Init(9999,168-1); //高级定时器TIM8的时钟频率为168M
|
||||
//<EFBFBD><EFBFBD>ͨС<EFBFBD><EFBFBD>Ĭ<EFBFBD>϶<EFBFBD>ʱ<EFBFBD><EFBFBD>8<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD>ӿ<EFBFBD>
|
||||
// TIM8_SERVO_Init(9999,168-1);//APB2<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>Ϊ168M , Ƶ<EFBFBD><EFBFBD>=168M/((9999+1)*(167+1))=100Hz
|
||||
//Initialize the model remote control interface
|
||||
//<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģң<EFBFBD>ؽӿ<EFBFBD>
|
||||
TIM8_Cap_Init(9999,168-1); //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>TIM8<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>Ϊ168M
|
||||
|
||||
//Free-running 1 MHz microsecond time base for sensor sample timestamps.
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD> 1MHz <>뼶<EFBFBD><EBBCB6><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><D7BC>
|
||||
TIM7_Init();
|
||||
|
||||
//Initialize motor speed control and, for controlling motor speed, PWM frequency 10kHz
|
||||
//初始化电机速度控制以及，用于控制电机速度，PWM频率10KHZ
|
||||
//APB2时钟频率为168M，满PWM为16799，频率=168M/((16799+1)*(0+1))=10k
|
||||
//<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڿ<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD><EFBFBD>ٶȣ<EFBFBD>PWMƵ<EFBFBD><EFBFBD>10KHZ
|
||||
//APB2ʱ<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>Ϊ168M<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PWMΪ16799<EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>=168M/((16799+1)*(0+1))=10k
|
||||
TIM1_PWM_Init(16799,0);
|
||||
TIM9_PWM_Init(16799,0);
|
||||
TIM10_PWM_Init(16799,0);
|
||||
TIM11_PWM_Init(16799,0);
|
||||
|
||||
//IIC initialization for MPU6050
|
||||
//IIC初始化，用于MPU6050
|
||||
//IIC<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>MPU6050
|
||||
I2C_GPIOInit();
|
||||
|
||||
//MPU6050 is initialized to read the vehicle's three-axis attitude,
|
||||
//three-axis angular velocity and three-axis acceleration information
|
||||
//MPU6050 初始化，用于读取小车三轴姿态、三轴角速度、三轴加速度信息
|
||||
//MPU6050 <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><EFBFBD>Ϣ
|
||||
MPU6050_initialize();
|
||||
|
||||
//Initialize the hardware interface to the PS2 controller
|
||||
//初始化与PS2手柄连接的硬件接口
|
||||
//<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PS2<EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><EFBFBD><EFBFBD>ӿ<EFBFBD>
|
||||
PS2_Init();
|
||||
|
||||
//PS2 gamepad configuration is initialized and configured in analog mode
|
||||
//PS2手柄配置初始化,配置为模拟量模式
|
||||
//PS2<EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ó<EFBFBD>ʼ<EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>Ϊģ<CEAA><C4A3><EFBFBD><EFBFBD>ģʽ
|
||||
PS2_SetInit();
|
||||
}
|
||||
|
||||
@@ -94,9 +94,11 @@ extern long int ErrorCode;
|
||||
void systemInit(void);
|
||||
|
||||
/***Macros define***/ /***宏定义***/
|
||||
//After starting the car (1000/100Hz =10) for seconds, it is allowed to control the car to move
|
||||
//开机(1000/100hz=10)秒后才允许控制小车进行运动
|
||||
#define CONTROL_DELAY 1000
|
||||
//Collect IMU zero-bias samples for 2 seconds at 100 Hz before enabling control.
|
||||
//开机后以100Hz采集2秒IMU零偏，再允许控制小车运动
|
||||
#define CONTROL_DELAY 200
|
||||
//Balance_task counter needed to represent the same 2 second startup delay.
|
||||
#define CONTROL_STARTUP_DELAY_COUNT (2 * CONTROL_FREQUENCY)
|
||||
//The number of robot types to determine the value of Divisor_Mode. There are currently 6 car types
|
||||
//机器人型号数量，决定Divisor_Mode的值，目前有6种小车类型
|
||||
#define CAR_NUMBER 6
|
||||
|
||||
Reference in New Issue
Block a user