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

204 lines
9.0 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.
#include "system.h"
//Robot software fails to flag bits
//<2F><><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ֵ<43>ֶα<D6B6><CEB1><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD>ͺ<EFBFBD><CDBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀǰ<C4BF><C7B0>6<EFBFBD><36>С<EFBFBD><D0A1><EFBFBD>ͺ<EFBFBD>
int Divisor_Mode;
// Robot type variable
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͺű<CDBA><C5B1><EFBFBD>
//0=Mec_Car<61><72>1=Omni_Car<61><72>2=Akm_Car<61><72>3=Diff_Car<61><72>4=FourWheel_Car<61><72>5=Tank_Car
u8 Car_Mode=4;
//Servo control PWM value, Ackerman car special
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PWMֵ<4D><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>ר<EFBFBD><D7A8>
int Servo = SERVO_INIT;
//Default speed of remote control car, unit: mm/s
//ң<><D2A3>С<EFBFBD><D0A1><EFBFBD><EFBFBD>Ĭ<EFBFBD><C4AC><EFBFBD>ٶȣ<D9B6><C8A3><EFBFBD>λ<EFBFBD><CEBB>mm/s
float RC_Velocity=500;
//Vehicle three-axis target moving speed, unit: m/s
//С<><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>˶<EFBFBD><CBB6>ٶȣ<D9B6><C8A3><EFBFBD>λ<EFBFBD><CEBB>m/s
float Move_X, Move_Y, Move_Z;
//Speed PI parameters: Ki is in PWM/(m/s*s) and multiplies measured dt.
//<2F>ٶ<EFBFBD>PI<50><49><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ki<4B><69>λΪ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
//ƽ<><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȫ<EFBFBD><C8AB><EFBFBD>ƶ<EFBFBD>С<EFBFBD><D0A1>ר<EFBFBD><D7A8>
Smooth_Control smooth_control;
//The parameter structure of the motor
//<2F><><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD>ṹ<EFBFBD><E1B9B9>
Motor_parameter MOTOR_A,MOTOR_B,MOTOR_C,MOTOR_D;
/************ С<><D0A1><EFBFBD>ͺ<EFBFBD><CDBA><EFBFBD>ر<EFBFBD><D8B1><EFBFBD> **************************/
/************ Variables related to car model ************/
//Encoder accuracy
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Encoder_precision;
//Wheel circumference, unit: m
//<2F><><EFBFBD><EFBFBD><EFBFBD>ܳ<EFBFBD><DCB3><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>m
float Wheel_perimeter;
//Drive wheel base, unit: m
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־࣬<D6BE><E0A3AC>λ<EFBFBD><CEBB>m
float Wheel_spacing;
//The wheelbase of the front and rear axles of the trolley, unit: m
//С<><D0A1>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>࣬<EFBFBD><E0A3AC>λ<EFBFBD><CEBB>m
float Axle_spacing;
//All-directional wheel turning radius, unit: m
//ȫ<><C8AB><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>뾶<EFBFBD><EBBEB6><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>m
float Omni_turn_radiaus;
/************ С<><D0A1><EFBFBD>ͺ<EFBFBD><CDBA><EFBFBD>ر<EFBFBD><D8B1><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<53>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>APP<50><50><EFBFBD><EFBFBD>ģ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>CANͨ<4E>š<EFBFBD><C5A1><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>5ͨ<35>ſ<EFBFBD><C5BF>Ʊ<EFBFBD>־λ<D6BE><CEBB><EFBFBD><EFBFBD>6<EFBFBD><36><EFBFBD><EFBFBD>־λĬ<CEBB>϶<EFBFBD>Ϊ0<CEAA><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><33><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
//<2F><><EFBFBD><EFBFBD>ң<EFBFBD><D2A3><EFBFBD><EFBFBD>صı<D8B5>־λ
u8 Flag_Left, Flag_Right, Flag_Direction=0, Turn_Flag;
//Sends the parameter's flag bit to the Bluetooth APP
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>APP<50><50><EFBFBD>Ͳ<EFBFBD><CDB2><EFBFBD><EFBFBD>ı<EFBFBD>־λ
u8 PID_Send;
//The PS2 gamepad controls related variables
//PS2<53>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ر<EFBFBD><D8B1><EFBFBD>
float PS2_LX,PS2_LY,PS2_RX,PS2_RY,PS2_KEY;
//Self-check the relevant flag variables
//<2F>Լ<EFBFBD><D4BC><EFBFBD>ر<EFBFBD>־<EFBFBD><D6BE><EFBFBD><EFBFBD>
int Check=0, Checking=0, Checked=0, CheckCount=0, CheckPhrase1=0, CheckPhrase2=0;
//Check the result code
//<2F>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
long int ErrorCode=0;
void systemInit(void)
{
// //Interrupt priority group setti ng
// //<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ȼ<EFBFBD><C8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
//
// //Delay function initialization
// //<2F><>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
delay_init(168);
//Initialize the hardware interface connected to the LED lamp
//<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>LED<45><44><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><D3B2><EFBFBD>ӿ<EFBFBD>
LED_Init();
//Initialize the hardware interface connected to the buzzer
//<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><D3B2><EFBFBD>ӿ<EFBFBD>
Buzzer_Init();
//Initialize the hardware interface connected to the enable switch
//<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>ʹ<EFBFBD>ܿ<EFBFBD><DCBF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><D3B2><EFBFBD>ӿ<EFBFBD>
Enable_Pin();
//Initialize the hardware interface connected to the OLED display
//<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>OLED<45><44>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><D3B2><EFBFBD>ӿ<EFBFBD>
OLED_Init();
//Initialize the hardware interface connected to the user's key
//<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><D3B2><EFBFBD>ӿ<EFBFBD>
KEY_Init();
//Serial port 1 initialization, communication baud rate 115200,
//can be used to communicate with ROS terminal
//<2F><><EFBFBD><EFBFBD>1<EFBFBD><31>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>ͨ<EFBFBD>Ų<EFBFBD><C5B2><EFBFBD><EFBFBD><EFBFBD>115200<30><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ROS<4F><53>ͨ<EFBFBD><CDA8>
uart1_init(115200);
//Serial port 2 initialization, communication baud rate 9600,
//used to communicate with Bluetooth APP terminal
//<2F><><EFBFBD><EFBFBD>2<EFBFBD><32>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>ͨ<EFBFBD>Ų<EFBFBD><C5B2><EFBFBD><EFBFBD><EFBFBD>9600<30><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>APP<50><50>ͨ<EFBFBD><CDA8>
uart2_init(9600);
//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
//<2F><><EFBFBD><EFBFBD>5<EFBFBD><35>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>ͨ<EFBFBD>Ų<EFBFBD><C5B2><EFBFBD><EFBFBD><EFBFBD>115200<30><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ROS<4F><53>ͨ<EFBFBD><CDA8>
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<44><43><EFBFBD>ų<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>ȡ<EFBFBD><C8A1>ص<EFBFBD>ѹ<EFBFBD><D1B9><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͺ<EFBFBD>
Adc_Init();
Adc_POWER_Init();
//Initialize the CAN communication interface
//CANͨ<4E>Žӿڳ<D3BF>ʼ<EFBFBD><CABC>
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
//<2F><><EFBFBD>ݵ<EFBFBD>λ<EFBFBD><CEBB><EFBFBD>ĵ<EFBFBD>λ<EFBFBD>ж<EFBFBD><D0B6><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD>ͺŵ<CDBA>С<EFBFBD><D0A1><EFBFBD><EFBFBD>Ȼ<EFBFBD><C8BB><EFBFBD><EFBFBD>ж<EFBFBD>Ӧ<EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
Robot_Select();
//Encoder A is initialized to read the real time speed of motor C
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>A<EFBFBD><41>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD>C<EFBFBD><43>ʵʱ<CAB5>ٶ<EFBFBD>
Encoder_Init_TIM2();
//Encoder B is initialized to read the real time speed of motor D
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>B<EFBFBD><42>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD>D<EFBFBD><44>ʵʱ<CAB5>ٶ<EFBFBD>
Encoder_Init_TIM3();
//Encoder C is initialized to read the real time speed of motor B
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>C<EFBFBD><43>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD>B<EFBFBD><42>ʵʱ<CAB5>ٶ<EFBFBD>
Encoder_Init_TIM4();
//Encoder D is initialized to read the real time speed of motor A
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>D<EFBFBD><44>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD>A<EFBFBD><41>ʵʱ<CAB5>ٶ<EFBFBD>
Encoder_Init_TIM5();
//<2F><>ʱ<EFBFBD><CAB1>12<31><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PWM<57>ӿ<EFBFBD>
TIM12_SERVO_Init(9999,84-1); //APB1<42><31>ʱ<EFBFBD><CAB1>Ƶ<EFBFBD><C6B5>Ϊ84M , Ƶ<><C6B5>=84M/((9999+1)*(83+1))=100Hz
//<2F><>ͨС<CDA8><D0A1>Ĭ<EFBFBD>϶<EFBFBD>ʱ<EFBFBD><CAB1>8<EFBFBD><38><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD>ӿ<EFBFBD>
// TIM8_SERVO_Init(9999,168-1);//APB2<42><32>ʱ<EFBFBD><CAB1>Ƶ<EFBFBD><C6B5>Ϊ168M , Ƶ<><C6B5>=168M/((9999+1)*(167+1))=100Hz
//Initialize the model remote control interface
//<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>ģң<C4A3>ؽӿ<D8BD>
TIM8_Cap_Init(9999,168-1); //<2F>߼<EFBFBD><DFBC><EFBFBD>ʱ<EFBFBD><CAB1>TIM8<4D><38>ʱ<EFBFBD><CAB1>Ƶ<EFBFBD><C6B5>Ϊ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
//<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȿ<D9B6><C8BF><EFBFBD><EFBFBD>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڿ<EFBFBD><DABF>Ƶ<EFBFBD><C6B5><EFBFBD>ٶȣ<D9B6>PWMƵ<4D><C6B5>10KHZ
//APB2ʱ<32><CAB1>Ƶ<EFBFBD><C6B5>Ϊ168M<38><4D><EFBFBD><EFBFBD>PWMΪ16799<39><39>Ƶ<EFBFBD><C6B5>=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<49><43>ʼ<EFBFBD><CABC><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 <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD>ȡС<C8A1><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȡ<D9B6><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD>Ϣ
MPU6050_initialize();
//Initialize the hardware interface to the PS2 controller
//<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>PS2<53>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>ӵ<EFBFBD>Ӳ<EFBFBD><D3B2><EFBFBD>ӿ<EFBFBD>
PS2_Init();
//PS2 gamepad configuration is initialized and configured in analog mode
//PS2<53>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>ó<EFBFBD>ʼ<EFBFBD><CABC>,<2C><><EFBFBD><EFBFBD>Ϊģ<CEAA><C4A3><EFBFBD><EFBFBD>ģʽ
PS2_SetInit();
}
Reference in New Issue View Git Blame Copy Permalink