- #include "control.h"
- #include "filter.h"
- #include "Stm32_Beep.h"
- /**************************************************************************
- 作者：平衡小车之家
- 我的淘宝小店：http://shop114407458.taobao.com/
- **************************************************************************/
- int Balance_Pwm=0,Velocity_Pwm=0,Turn_Pwm=0;
- u8 Flag_Target;
- /**************************************************************************
- 函数功能：所有的控制代码都在这里面
- 5ms定时中断由MPU6050的INT引脚触发
- 严格保证采样和数据处理的时间同步
- **************************************************************************/
- int EXTI4_IRQHandler(void)
- {
- if(PAin(4)==0)
- {
- EXTI->PR=1<<4; //清除LINE4上的中断标志位
- gul_softTmr++;
-
- Flag_Target=!Flag_Target;
- if(delay_flag==1)
- {
- if(++delay_50==10)
- {
- delay_50=0,delay_flag=0; //给主函数提供50ms的精准延时
- }
- }
- if(Flag_Target==1) //5ms读取一次陀螺仪和加速度计的值，更高的采样频率可以改善卡尔曼滤波和互补滤波的效果
- {
- Get_Angle(Way_Angle); //===更新姿态
- return 0;
- } //10ms控制一次，为了保证M法测速的时间基准，首先读取编码器数据
- Encoder_Left=-Read_Encoder(2); //===读取编码器的值，因为两个电机的旋转了180度的，所以对其中一个取反，保证输出极性一致
- Encoder_Right=Read_Encoder(4); //===读取编码器的值
- Get_Angle(Way_Angle); //===更新姿态
- //Read_Distane(); //===获取超声波测量距离值
- if(Bi_zhang==0)
- {
- //Led_Flash(0); //===LED闪烁;常规模式 1s改变一次指示灯的状态
- }
- if(Bi_zhang==1)
- {
- Led_Flash(1); //===LED闪烁;避障模式 指示灯常亮
- }
-
- //Voltage=Get_battery_volt(); //===获取电池电压
- Key(); //===扫描按键状态 单击双击可以改变小车运行状态
-
- Balance_Pwm = balance(Angle_Balance,Gyro_Balance); //===平衡PID控制
- Velocity_Pwm = velocity(Encoder_Left,Encoder_Right); //===速度环PID控制 记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点
- Turn_Pwm = turn(Encoder_Left,Encoder_Right,Gyro_Turn); //===转向环PID控制
-
- Moto1=Balance_Pwm-Velocity_Pwm+Turn_Pwm; //===计算左轮电机最终PWM
- Moto2=Balance_Pwm-Velocity_Pwm-Turn_Pwm; //===计算右轮电机最终PWM
- Xianfu_Pwm(); //===PWM限幅
-
- if(Pick_Up(Acceleration_Z,Angle_Balance,Encoder_Left,Encoder_Right))//===检查是否小车被那起
- {
- Flag_Stop=1; //===如果被拿起就关闭电机
- }
- if(Put_Down(Angle_Balance,Encoder_Left,Encoder_Right)) //===检查是否小车被放下
- {
- Flag_Stop=0; //===如果被放下就启动电机
- }
- if(Turn_Off(Angle_Balance,Voltage)==0) //===如果不存在异常
- {
- Set_Pwm(Moto1,Moto2); //===赋值给PWM寄存器
- }
- }
- return 0;
- }
- /**************************************************************************
- 函数功能：直立PD控制
- 入口参数：角度、角速度
- 返回 值：直立控制PWM
- 作 者：平衡小车之家
- **************************************************************************/
- int balance(float Angle,float Gyro)
- {
- float Bias,kp=480,kd=0.73;
- int balance;
- Bias=Angle-ZHONGZHI; //===求出平衡的角度中值 和机械相关
- balance=kp*Bias+Gyro*kd; //===计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数
- return balance;
- }
- /**************************************************************************
- 函数功能：速度PI控制 修改前进后退速度，请修Target_Velocity，比如，改成60就比较慢了
- 入口参数：左轮编码器、右轮编码器
- 返回 值：速度控制PWM
- 作 者：平衡小车之家
- **************************************************************************/
- int velocity(int encoder_left,int encoder_right)
- {
- static float Velocity,Encoder_Least,Encoder,Movement;
- static float Encoder_Integral,Target_Velocity;
- ///float kp=-120,ki=-0.6;
- float kp=-200,ki=-0.9;
-
- //=============遥控前进后退部分=======================//
- if(Bi_zhang==1&&Flag_sudu==1)
- {
- Target_Velocity=45; //如果进入避障模式,自动进入低速模式
- }
- else
- {
- Target_Velocity=30;//90;
- }
-
- if(1==Flag_Qian)
- {
- Movement=-Target_Velocity/Flag_sudu; //===前进标志位置1
- }
- else if(1==Flag_Hou)
- {
- Movement=Target_Velocity/Flag_sudu; //===后退标志位置1
- }
- else
- {
- Movement=0;
- }
-
- if(Bi_zhang==1 && Distance<500 && Flag_Left!=1 &&Flag_Right!=1) //避障标志位置1且非遥控转弯的时候，进入避障模式
- {
- Movement=-Target_Velocity/Flag_sudu;
- }
-
- //=============速度PI控制器=======================//
- Encoder_Least =(Encoder_Left+Encoder_Right)-0; //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
- Encoder *= 0.8; //===一阶低通滤波器
- Encoder += Encoder_Least*0.2; //===一阶低通滤波器
- Encoder_Integral +=Encoder; //===积分出位移 积分时间：10ms
- Encoder_Integral=Encoder_Integral-Movement; //===接收遥控器数据，控制前进后退
- if(Encoder_Integral>10000) Encoder_Integral=10000; //===积分限幅
- if(Encoder_Integral<-10000) Encoder_Integral=-10000; //===积分限幅
- Velocity=Encoder*kp+Encoder_Integral*ki; //===速度控制
- if(Turn_Off(Angle_Balance,Voltage)==1||Flag_Stop==1)
- {
- Encoder_Integral=0; //===电机关闭后清除积分
- }
- return Velocity;
- }
- /**************************************************************************
- 函数功能：转向控制 修改转向速度，请修改Turn_Amplitude即可
- 入口参数：左轮编码器、右轮编码器、Z轴陀螺仪
- 返回 值：转向控制PWM
- 作 者：平衡小车之家
- **************************************************************************/
- int turn(int encoder_left,int encoder_right,float gyro)//转向控制
- {
- static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count;
- float Turn_Amplitude=88/Flag_sudu,Kp=42,Kd=0;
-
- //=============遥控左右旋转部分=======================//
- if(1==Flag_Left||1==Flag_Right) //这一部分主要是根据旋转前的速度调整速度的起始速度，增加小车的适应性
- {
- if(++Turn_Count==1)
- Encoder_temp=myabs(encoder_left+encoder_right);
- Turn_Convert=50/Encoder_temp;
- if(Turn_Convert<0.6)Turn_Convert=0.6;
- if(Turn_Convert>3)Turn_Convert=3;
- }
- else
- {
- Turn_Convert=0.9;
- Turn_Count=0;
- Encoder_temp=0;
- }
- if(1==Flag_Left) Turn_Target-=Turn_Convert;
- else if(1==Flag_Right) Turn_Target+=Turn_Convert;
- else Turn_Target=0;
-
- if(Turn_Target>Turn_Amplitude) Turn_Target=Turn_Amplitude; //===转向速度限幅
- if(Turn_Target<-Turn_Amplitude) Turn_Target=-Turn_Amplitude;
- if(Flag_Qian==1||Flag_Hou==1) Kd=0.5;
- else Kd=0; //转向的时候取消陀螺仪的纠正 有点模糊PID的思想
- //=============转向PD控制器=======================//
- Turn=-Turn_Target*Kp -gyro*Kd; //===结合Z轴陀螺仪进行PD控制
- return Turn;
- }
- /**************************************************************************
- 函数功能：赋值给PWM寄存器
- 入口参数：左轮PWM、右轮PWM
- 返回 值：无
- **************************************************************************/
- void Set_Pwm(int moto1,int moto2)
- {
- if(moto1<0) AIN2=1, AIN1=0;
- else AIN2=0, AIN1=1;
- PWMA=myabs(moto1);
- if(moto2<0) BIN1=0, BIN2=1;
- else BIN1=1, BIN2=0;
- PWMB=myabs(moto2);
- }
- /**************************************************************************
- 函数功能：限制PWM赋值
- 入口参数：无
- 返回 值：无
- **************************************************************************/
- void Xianfu_Pwm(void)
- {
- int Amplitude=6900; //===PWM满幅是7200 限制在6900
- if(Flag_Qian==1) Moto1-=DIFFERENCE; //DIFFERENCE是一个衡量平衡小车电机和机械安装差异的一个变量。直接作用于输出，让小车具有更好的一致性。
- if(Flag_Hou==1) Moto2+=DIFFERENCE;
- if(Moto1<-Amplitude) Moto1=-Amplitude;
- if(Moto1>Amplitude) Moto1=Amplitude;
- if(Moto2<-Amplitude) Moto2=-Amplitude;
- if(Moto2>Amplitude) Moto2=Amplitude;
-
- }
- /**************************************************************************
- 函数功能：按键修改小车运行状态
- 入口参数：无
- 返回 值：无
- **************************************************************************/
- void Key(void)
- {
- u8 tmp,tmp2;
- tmp=click_N_Double(50);
-
- if(tmp==1)
- Flag_Stop=!Flag_Stop;//单击控制小车的启停
-
- if(tmp==2)
- Flag_Show=!Flag_Show;//双击控制小车的显示状态
-
- tmp2=Long_Press();
- if(tmp2==1)
- Bi_zhang=!Bi_zhang; //长按控制小车是否进入超声波避障模式
- }
- /**************************************************************************
- 函数功能：异常关闭电机
- 入口参数：倾角和电压
- 返回 值：1：异常 0：正常
- **************************************************************************/
- u8 Turn_Off(float angle, int voltage)
- {
- u8 temp;
- //if(angle<-40||angle>40||1==Flag_Stop||voltage<1110) //电池电压低于11.1V关闭电机
- if(angle<-40||angle>40||1==Flag_Stop) //去掉电池电压低于11.1V关闭电机
- { //===倾角大于40度关闭电机
- temp=1; //===Flag_Stop置1关闭电机
- AIN1=0;
- AIN2=0;
- BIN1=0;
- BIN2=0;
- }
- else
- {
- temp=0;
- }
- return temp;
- }
-
- /**************************************************************************
- 函数功能：获取角度 三种算法经过我们的调校，都非常理想
- 入口参数：获取角度的算法 1：DMP 2：卡尔曼 3：互补滤波
- 返回 值：无
- **************************************************************************/
- void Get_Angle(u8 way)
- {
- float Accel_Y,Accel_X,Accel_Z,Gyro_Y,Gyro_Z;
- Temperature=Read_Temperature(); //===读取MPU6050内置温度传感器数据，近似表示主板温度。
- if(way==1) //===DMP的读取在数据采集中断提醒的时候，严格遵循时序要求
- {
- Read_DMP(); //===读取加速度、角速度、倾角
-
- //Angle_Balance=Pitch; //===更新平衡倾角--
- Angle_Balance=Roll; //===更新平衡倾角---
-
- //Gyro_Balance=gyro[1]; //===更新平衡角速度
- Gyro_Balance=gyro[0]; //===更新平衡角速度
-
- Gyro_Turn=gyro[2]; //===更新转向角速度
- Acceleration_Z=accel[2]; //===更新Z轴加速度计
- }
- else
- {
- Gyro_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L); //读取Y轴陀螺仪
- Gyro_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L); //读取Z轴陀螺仪
- Accel_X=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //读取X轴加速度计
- Accel_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //读取Z轴加速度计
- if(Gyro_Y>32768) Gyro_Y-=65536; //数据类型转换 也可通过short强制类型转换
- if(Gyro_Z>32768) Gyro_Z-=65536; //数据类型转换
- if(Accel_X>32768) Accel_X-=65536; //数据类型转换
- if(Accel_Z>32768) Accel_Z-=65536; //数据类型转换
- Gyro_Balance=-Gyro_Y; //更新平衡角速度
- Accel_Y=atan2(Accel_X,Accel_Z)*180/PI; //计算倾角
- Gyro_Y=Gyro_Y/16.4; //陀螺仪量程转换
- if(Way_Angle==2) Kalman_Filter(Accel_Y,-Gyro_Y);//卡尔曼滤波
- else if(Way_Angle==3) Yijielvbo(Accel_Y,-Gyro_Y); //互补滤波
- Angle_Balance=angle; //更新平衡倾角
- Gyro_Turn=Gyro_Z; //更新转向角速度
- Acceleration_Z=Accel_Z; //===更新Z轴加速度计
- }
- }
- /**************************************************************************
- 函数功能：绝对值函数
- 入口参数：int
- 返回 值：unsigned int
- **************************************************************************/
- int myabs(int a)
- {
- int temp;
- if(a<0) temp=-a;
- else temp=a;
- return temp;
- }
- /**************************************************************************
- 函数功能：检测小车是否被拿起
- 入口参数：int
- 返回 值：unsigned int
- **************************************************************************/
- int Pick_Up(float Acceleration,float Angle,int encoder_left,int encoder_right)
- {
- static u16 flag,count0,count1,count2;
- if(flag==0) //第一步
- {
- if(myabs(encoder_left)+myabs(encoder_right)<30) //条件1，小车接近静止
- {
- count0++;
- }
- else
- {
- count0=0;
- }
-
- if(count0>10)
- {
- flag=1,count0=0;
- }
- }
-
- if(flag==1) //进入第二步
- {
- if(++count1>200)
- {
- count1=0,flag=0; //超时不再等待2000ms
- }
- if(Acceleration>26000&&(Angle>(-20+ZHONGZHI))&&(Angle<(20+ZHONGZHI))) //条件2，小车是在0度附近被拿起
- {
- flag=2;
- }
- }
-
- if(flag==2) //第三步
- {
- if(++count2>100) count2=0,flag=0; //超时不再等待1000ms
- if(myabs(encoder_left+encoder_right)>135) //条件3，小车的轮胎因为正反馈达到最大的转速
- {
- flag=0;
- return 1; //检测到小车被拿起
- }
- }
- return 0;
- }
- /**************************************************************************
- 函数功能：检测小车是否被放下
- 入口参数：int
- 返回 值：unsigned int
- **************************************************************************/
- int Put_Down(float Angle,int encoder_left,int encoder_right)
- {
- static u16 flag,count;
- if(Flag_Stop==0) //防止误检
- return 0;
- if(flag==0)
- {
- if(Angle>(-10+ZHONGZHI)&&Angle<(10+ZHONGZHI)&&encoder_left==0&&encoder_right==0) //条件1，小车是在0度附近的
- flag=1;
- }
- if(flag==1)
- {
- if(++count>50) //超时不再等待 500ms
- {
- count=0;flag=0;
- }
- //if(encoder_left>3&&encoder_right>3&&encoder_left<60&&encoder_right<60) //条件2，小车的轮胎在未上电的时候被人为转动
- if(encoder_left>=0&&encoder_right>=0&&encoder_left<60&&encoder_right<60) //条件2，小车的轮胎在未上电的时候被人为转动
- {
- flag=0;
- flag=0;
- return 1; //检测到小车被放下
- }
- }
- return 0;
- }
楼主
