在STM32的代码中，通过运动学方程，从上位机得到的目标速度：Move_X, Move_Y, Move_Z中求解出阿克曼小车电机MOTOR_A，MOTOR_B的目标转速和舵机Servo的PWM值。从而控制驱动轮的电机电压来通过PID达到目标速度，控制舵机的角度来控制方向轮的角度从而控制小车方向。
void Drive_Motor(float Vx,float Vy,float Vz) { float amplitude=3.5; //Wheel target speed limit //车轮目标速度限幅 //Ackermann structure car //阿克曼小车 if (Car_Mode==Akm_Car) { //Ackerman car specific related variables //阿克曼小车专用相关变量 int K=1000; float Ratio=1, Angle; // For Ackerman small car, Vz represents the front wheel steering Angle //对于阿克曼小车Vz代表前轮转向角度 Angle=Vz; // Front wheel steering Angle limit (front wheel steering Angle controlled by steering engine), unit: rad //前轮转向角度限幅(舵机控制前轮转向角度)，单位：rad Angle=target_limit_float(Angle,-0.35f,0.35f); // The software compensates for the front wheel steering Angle due to mechanical structure limitations //机械结构限制，软件对前轮转向角度进行补偿 if(Angle<0)Ratio=1.054; else if(Angle>0)Ratio=0.838; else Ratio=0; //Inverse kinematics //运动学逆解 MOTOR_A.Target = Vx*(1-Wheel_spacing*tan(Angle)/2/Axle_spacing); MOTOR_B.Target = Vx*(1+Wheel_spacing*tan(Angle)/2/Axle_spacing); // The PWM value of the servo controls the steering Angle of the front wheel //舵机PWM值，舵机控制前轮转向角度 Servo=(SERVO_INIT-Angle*K*Ratio); //Wheel (motor) target speed limit //车轮(电机)目标速度限幅 MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude); MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude); MOTOR_C.Target=0; //Out of use //没有使用到 MOTOR_D.Target=0; //Out of use //没有使用到 Servo=target_limit_int(Servo,900,2000); //Servo PWM value limit //舵机PWM值限幅 } } |
STM32系统中的定时任务Balance_task负责把收集到的电机速度通过PID算法，不停的转换成对应的PWM值，直到达到目标速度。
void Balance_task(void *pvParameters) { u32 lastWakeTime = getSysTickCnt(); while(1) { // This task runs at a frequency of 100Hz (10ms control once) //此任务以100Hz的频率运行（10ms控制一次） vTaskDelayUntil(&lastWakeTime, F2T(RATE_100_HZ)); //Time count is no longer needed after 30 seconds //时间计数，30秒后不再需要 if(Time_count<3000)Time_count++; //Get the encoder data, that is, the real time wheel speed, //and convert to transposition international units //获取编码器数据，即车轮实时速度，并转换位国际单位 Get_Velocity_Form_Encoder(); if(Check==0) //If self-check mode is not enabled //如果没有启动自检模式 { if (APP_ON_Flag) Get_RC(); //Handle the APP remote commands //处理APP遥控命令 else if (Remote_ON_Flag) Remote_Control(); //Handle model aircraft remote commands //处理航模遥控命令 else if (PS2_ON_Flag) PS2_control(); //Handle PS2 controller commands //处理PS2手柄控制命令 //CAN, Usart 1, Usart 3 control can directly get the three axis target speed, //without additional processing //CAN、串口1、串口3(ROS)控制直接得到三轴目标速度，无须额外处理 else Drive_Motor(Move_X, Move_Y, Move_Z); //Click the user button to update the gyroscope zero //单击用户按键更新陀螺仪零点 Key(); //If there is no abnormity in the battery voltage, and the enable switch is in the ON position, //and the software failure flag is 0 //如果电池电压不存在异常，而且使能开关在ON档位，而且软件失能标志位为0 if(Turn_Off(Voltage)==0) { //Speed closed-loop control to calculate the PWM value of each motor, //PWM represents the actual wheel speed //速度闭环控制计算各电机PWM值，PWM代表车轮实际转速 MOTOR_A.Motor_Pwm=Incremental_PI_A(MOTOR_A.Encoder, MOTOR_A.Target); MOTOR_B.Motor_Pwm=Incremental_PI_B(MOTOR_B.Encoder, MOTOR_B.Target); MOTOR_C.Motor_Pwm=Incremental_PI_C(MOTOR_C.Encoder, MOTOR_C.Target); MOTOR_D.Motor_Pwm=Incremental_PI_D(MOTOR_D.Encoder, MOTOR_D.Target); //Set different PWM control polarity according to different car models //根据不同小车型号设置不同的PWM控制极性 switch(Car_Mode) { case Akm_Car: Set_Pwm(-MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, Servo); break; //Ackermann structure car //阿克曼小车 } } //If Turn_Off(Voltage) returns to 1, the car is not allowed to move, and the PWM value is set to 0 //如果Turn_Off(Voltage)返回值为1，不允许控制小车进行运动，PWM值设置为0 else Set_Pwm(0,0,0,0,0); } //If self-check mode is enabled, the run self-check function is executed //如果开启了自检模式，则执行运行自检函数 else CheckTask(); } } 函数功能：增量式PI控制器 入口参数：编码器测量值(实际速度)，目标速度 返回 值：电机PWM 根据增量式离散PID公式 pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)] e(k)代表本次偏差 e(k-1)代表上一次的偏差 以此类推 pwm代表增量输出 在我们的速度控制闭环系统里面，只使用PI控制 pwm+=Kp[e（k）-e(k-1)]+Ki*e(k) **************************************************************************/ int Incremental_PI_A (float Encoder,float Target) { static float Bias,Pwm,Last_bias; Bias=Target-Encoder; //Calculate the deviation //计算偏差 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; if(Pwm>7200)Pwm=7200; if(Pwm<-7200)Pwm=-7200; Last_bias=Bias; //Save the last deviation //保存上一次偏差 return Pwm; } void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d,int servo) { //Forward and reverse control of motor //电机正反转控制 if(motor_a<0) AIN2=0, AIN1=1; else AIN2=1, AIN1=0; //Motor speed control //电机转速控制 PWMA=abs(motor_a); //Forward and reverse control of motor //电机正反转控制 if(motor_b<0) BIN2=1, BIN1=0; else BIN2=0, BIN1=1; //Motor speed control //电机转速控制 PWMB=abs(motor_b); //Forward and reverse control of motor //电机正反转控制 if(motor_c>0) CIN2=0, CIN1=1; else CIN2=1, CIN1=0; //Motor speed control //电机转速控制 PWMC=abs(motor_c); //Forward and reverse control of motor //电机正反转控制 if(motor_d>0) DIN2=0, DIN1=1; else DIN2=1, DIN1=0; //Motor speed control //电机转速控制 PWMD=abs(motor_d); //Servo control //舵机控制 Servo_PWM =servo; } |
获取4个车轮编码器的值，并计算出速度。
void Get_Velocity_Form_Encoder(void) { //Retrieves the original data of the encoder //获取编码器的原始数据 float Encoder_A_pr,Encoder_B_pr,Encoder_C_pr,Encoder_D_pr; OriginalEncoder.A=Read_Encoder(2); OriginalEncoder.B=Read_Encoder(3); OriginalEncoder.C=Read_Encoder(4); OriginalEncoder.D=Read_Encoder(5); //Decide the encoder numerical polarity according to different car models //根据不同小车型号决定编码器数值极性 switch(Car_Mode) { case Akm_Car: Encoder_A_pr=-OriginalEncoder.A; Encoder_B_pr= OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C; Encoder_D_pr= OriginalEncoder.D; break; } //The encoder converts the raw data to wheel speed in m/s //编码器原始数据转换为车轮速度，单位m/s MOTOR_A.Encoder= Encoder_A_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; MOTOR_B.Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; MOTOR_C.Encoder= Encoder_C_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; MOTOR_D.Encoder= Encoder_D_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; } |
本作品采用知识共享署名 4.0 国际许可协议进行许可。
