/******************** (C) COPYRIGHT 2024 GeekRebot ***************************** * File Name : control.c * Current Version : V1.0 & ST 3.5.0 * Author : zhanli 719901725@qq.com * Date of Issued : 2024.01.06 zhanli: Create * Comments : 平衡车控制代码 ********************************************************************************/ #include "control.h" /**---------------------------------------------------------------------- * Function : EXTI15_10_IRQHandler * Description : 所有的控制代码都在这里面, 5ms定时中断由MPU6050的INT引脚触发 严格保证采样和数据处理的时间同步 * Author : zhanli&719901725@qq.com * Date : 2023/05/20 zhanli *---------------------------------------------------------------------**/ int EXTI15_10_IRQHandler(void) { static u8 Flag_Target; // 控制函数相关变量，提供10ms基准 int Balance_Pwm, Velocity_Pwm; // 平衡环PWM变量，速度环PWM变量 int Motor_Left, Motor_Right; // 电机PWM变量 应是Motor的 向Moto致敬 if (INT == 0) { EXTI->PR = 1 << 12; // 清除中断标志位 Flag_Target = !Flag_Target; IMU_GetAngle(); // 更新姿态 5ms读取一次 Encoder_Left = -Read_Encoder(2); // 读取左轮编码器的值，前进为正，后退为负 Encoder_Right = -Read_Encoder(4); // 读取右轮编码器的值，前进为正，后退为负 // 左轮A相接TIM2_CH1,右轮A相接TIM4_CH2,故这里两个编码器的极性相同 if (Flag_Target == 1) // 实际上10ms控制一次 return 0; // 扫描按键状态 单击可启停电机 Key(); // 平衡PID控制,Gyro_Balance平衡角速度极性：前倾为正，后倾为负 Balance_Pwm = Balance(Angle_Balance, Gyro_Balance); // 速度环PID控制,记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点 Velocity_Pwm = Velocity(Encoder_Left, Encoder_Right); Motor_Left = Balance_Pwm + Velocity_Pwm; // 计算左轮电机最终PWM Motor_Right = Balance_Pwm + Velocity_Pwm; // 计算右轮电机最终PWM Motor_Left = Motor_PWMLimit(Motor_Left, 6900, -6900); Motor_Right = Motor_PWMLimit(Motor_Right, 6900, -6900); // PWM限幅 if (Turn_Off(Angle_Balance) == 0) // 如果不存在异常 Motor_PWMSet(Motor_Left, Motor_Right); // 赋值给PWM寄存器 } return 0; } /**---------------------------------------------------------------------- * Function : Balance * Description : 直立PD控制 * Input : Angle:角度, Gyro:角速度 * Author : zhanli&719901725@qq.com * Date : 2023/05/20 zhanli *---------------------------------------------------------------------**/ int Balance(float Angle, float Gyro) { // 直立环PD参数 float Balance_Kp = 225, Balance_Kd = 1.08; float Angle_bias, Gyro_bias; int balance; // 求出平衡的角度中值 和机械相关 Angle_bias = Middle_angle - Angle; Gyro_bias = 0 - Gyro; // 计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数 balance = -Balance_Kp * Angle_bias - Gyro_bias * Balance_Kd; return balance; } /************************************************************************** Function: Speed PI control Input : encoder_left：Left wheel encoder reading；encoder_right：Right wheel encoder reading Output : Speed control PWM 函数功能：速度控制PWM 入口参数：encoder_left：左轮编码器读数；encoder_right：右轮编码器读数 返回 值：速度控制PWM **************************************************************************/ int Velocity(int encoder_left, int encoder_right) { float Velocity_Kp = 160, Velocity_Ki = 0.8; // 速度环PI参数 static float velocity, Encoder_Least, Encoder_bias; static float Encoder_Integral; // 速度PI控制器 Encoder_Least = 0 - (encoder_left + encoder_right); // 获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零） Encoder_bias *= 0.8; // 一阶低通滤波器 Encoder_bias += Encoder_Least * 0.2; // 一阶低通滤波器 // 相当于上次偏差的0.8 + 本次偏差的0.2，减缓速度差值，减少对直立的干扰 Encoder_Integral += Encoder_bias; // 积分出位移 积分时间：10ms if (Encoder_Integral > 10000) Encoder_Integral = 10000; // 积分限幅 if (Encoder_Integral < -10000) Encoder_Integral = -10000; // 积分限幅 // 速度控制 velocity = -Encoder_bias * Velocity_Kp - Encoder_Integral * Velocity_Ki; // 电机关闭后清除积分 if (Turn_Off(Angle_Balance) == 1 || Flag_Stop == 1) Encoder_Integral = 0; return velocity; } /************************************************************************** Function: Assign to PWM register Input : motor_left：Left wheel PWM；motor_right：Right wheel PWM Output : none 函数功能：赋值给PWM寄存器 入口参数：左轮PWM、右轮PWM 返回 值：无 **************************************************************************/ void Motor_PWMSet(int motor_left, int motor_right) { if (motor_left > 0) BIN1 = 1, BIN2 = 0; // 前进 else BIN1 = 0, BIN2 = 1; // 后退 PWMB = myabs(motor_left); if (motor_right > 0) AIN2 = 1, AIN1 = 0; // 前进 else AIN2 = 0, AIN1 = 1; // 后退 PWMA = myabs(motor_right); } /************************************************************************** Function: PWM limiting range Input : IN：Input max：Maximum value min：Minimum value Output : Output 函数功能：限制PWM赋值 入口参数：IN：输入参数 max：限幅最大值 min：限幅最小值 返回 值：限幅后的值 **************************************************************************/ int Motor_PWMLimit(int IN, int max, int min) { int OUT = IN; if (OUT > max){ OUT = max; } if (OUT < min){ OUT = min; } return OUT; } /************************************************************************** Function: Press the key to modify the car running state Input : none Output : none 函数功能：按键修改小车运行状态 入口参数：无 返回 值：无 **************************************************************************/ void Key(void) { u8 tmp; tmp = click(); if (tmp == 1){ Flag_Stop = !Flag_Stop; // 单击控制小车的启停 } } /************************************************************************** Function: If abnormal, turn off the motor Input : angle：Car inclination；voltage：Voltage Output : 1：abnormal；0：normal 函数功能：异常关闭电机 入口参数：angle：小车倾角 返回 值：1：异常 0：正常 **************************************************************************/ u8 Turn_Off(float angle) { u8 temp; if (angle < -40 || angle > 40 || 1 == Flag_Stop) // 倾角大于40度关闭电机 { // Flag_Stop置1关闭电机 temp = 1; AIN1 = 0; AIN2 = 0; BIN1 = 0; BIN2 = 0; } else temp = 0; return temp; } /************************************************************************** Function: Get angle Input : way：The algorithm of getting angle 1：DMP 2：kalman 3：Complementary filtering Output : none 函数功能：获取角度 入口参数：无 返回 值：无 **************************************************************************/ void IMU_GetAngle(void) { Read_DMP(); // 读取加速度、角速度、倾角 Angle_Balance = Pitch; // 更新平衡倾角,前倾为正，后倾为负 Gyro_Balance = gyro[0]; // 更新平衡角速度,前倾为正，后倾为负 } /************************************************************************** Function: Absolute value function Input : a：Number to be converted Output : unsigned int 函数功能：绝对值函数 入口参数：a：需要计算绝对值的数 返回 值：无符号整型 **************************************************************************/ int myabs(int a) { int temp; if (a < 0) temp = -a; else temp = a; return temp; }