[开发进程]实验十、另类走8字
背景：
上文通过对左右编码器采样出来的数据进行积分（distance =
distance + (float)Encoder_Left*0.005;）得到了小车前进的距离。
其中distance是自己定义的小车前进距离。
那么，同样的道理，可以通过对左右编码器之差进行积分turning = turning + (float)(Encoder_Left-Encoder_Right)*0.005;，得到小车转弯的大概的角度。
笔者试验室的地板是方块的，60cm*60cm，那么为什么不可以走方形的八字呢。这种八字就是数码管的8。
原理：
本试验的原理很简单，要使车子走数码管8字，只需要使小车按照这个路线图走就可以了。
只是调试过程比较痛苦……
实验代码
这个实验基于<Mini Balance V2.5 标准版源码卡尔曼版本>
修改了两个源文件。分别是show.c和MiniBalance.c
以下是这两个修改后的文件的代码：
//show.c
#include "show.h"
#include "MiniBalance.h"
/**************************************************************************
作者：平衡小车之家
淘宝店铺：http://shop114407458.taobao.com/
**************************************************************************/
unsigned char i; //计数变量
unsigned char Send_Count; //串口需要发送的数据个数
/**************************************************************************
函数功能：OLED显示
入口参数：无
返回 值：无
作 者：平衡小车之家
**************************************************************************/
static u32 Count;
void oled_show(void)
{
int disSHow;
int turSHow;
Count=0;
OLED_Display_On(); //显示屏打开
//turSHow = turning*1.087613;
turSHow = turning;
disSHow = -distance*2.176542;
if(ThreeSecFlag==1)
{
if(disSHow<300)
{ Flag_Qian=1,Flag_Hou=0,Flag_Left=0,Flag_Right=0;}
else if(turSHow<85)
{ Flag_Qian=0,Flag_Hou=0,Flag_Left=1,Flag_Right=0;}
else if(disSHow<900)
{ Flag_Qian=1,Flag_Hou=0,Flag_Left=0,Flag_Right=0;}
else if(turSHow<175)
{ Flag_Qian=0,Flag_Hou=0,Flag_Left=1,Flag_Right=0;}
else if(disSHow<1300)
{ Flag_Qian=1,Flag_Hou=0,Flag_Left=0,Flag_Right=0;}
else if(turSHow<274)
{ Flag_Qian=0,Flag_Hou=0,Flag_Left=0,Flag_Right=1;}
else if(disSHow<2000)
{ Flag_Qian=1,Flag_Hou=0,Flag_Left=0,Flag_Right=0;}
else if(turSHow<373)
{ Flag_Qian=0,Flag_Hou=0,Flag_Left=0,Flag_Right=1;}
else if(disSHow<2700)
{ Flag_Qian=1,Flag_Hou=0,Flag_Left=0,Flag_Right=0;}
else if(turSHow<472)
{ Flag_Qian=0,Flag_Hou=0,Flag_Left=0,Flag_Right=1;}
else if(disSHow<3300)
{ Flag_Qian=1,Flag_Hou=0,Flag_Left=0,Flag_Right=0;}
else if(turSHow<540)
{ Flag_Qian=0,Flag_Hou=0,Flag_Left=0,Flag_Right=1;}
else if(disSHow<3900)
{ Flag_Qian=1,Flag_Hou=0,Flag_Left=0,Flag_Right=0;}
else if(turSHow<625)
{ Flag_Qian=0,Flag_Hou=0,Flag_Left=1,Flag_Right=0;}
else if(disSHow<4400)
{ Flag_Qian=1,Flag_Hou=0,Flag_Left=0,Flag_Right=0;}
else if(turSHow<715)
{ Flag_Qian=0,Flag_Hou=0,Flag_Left=1,Flag_Right=0;}
else
Flag_Qian=0,Flag_Hou=0,Flag_Left=0,Flag_Right=0;
}
if(disSHow<0)
{
OLED_ShowString(0,0,"Dist -");
OLED_ShowNumber(45,0, (-disSHow),5,12);
}
else
{
OLED_ShowString(0,0,"Dist ");
OLED_ShowNumber(45,0, disSHow,5,12);
}
if(turSHow<0)
{
OLED_ShowString(0,10,"Turn -");
OLED_ShowNumber(45,10, (-turSHow),5,12);
}
else
{
OLED_ShowString(0,10,"Turn ");
OLED_ShowNumber(45,10, turSHow,5,12);
}
//=============显示温度=======================//
/*
OLED_ShowString(00,10,"Wendu");
OLED_ShowNumber(45,10,Temperature/10,2,12);
OLED_ShowNumber(68,10,Temperature%10,1,12);
OLED_ShowString(58,10,".");
OLED_ShowString(80,10,"`C");
*/
//=============显示编码器1=======================//
OLED_ShowString(00,20,"Enco1");
if( Encoder_Left<0) OLED_ShowString(45,20,"-"),
OLED_ShowNumber(65,20,-Encoder_Left,5,12);
else OLED_ShowString(45,20,"+"),
OLED_ShowNumber(65,20, Encoder_Left,5,12);
//=============显示编码器2=======================//
OLED_ShowString(00,30,"Enco2");
if(Encoder_Right<0) OLED_ShowString(45,30,"-"),
OLED_ShowNumber(65,30,-Encoder_Right,5,12);
else OLED_ShowString(45,30,"+"),
OLED_ShowNumber(65,30,Encoder_Right,5,12);
//=============显示电压=======================//
OLED_ShowString(00,40,"Volta");
OLED_ShowString(58,40,".");
OLED_ShowString(80,40,"V");
OLED_ShowNumber(45,40,Voltage/100,2,12);
OLED_ShowNumber(68,40,Voltage%100,2,12);
if(Voltage%100<10) OLED_ShowNumber(62,40,0,2,12);
//=============显示角度=======================//
OLED_ShowString(0,50,"Angle");
if(Angle_Balance<0) OLED_ShowNumber(45,50,Angle_Balance+360,3,12);
else OLED_ShowNumber(45,50,Angle_Balance,3,12);
//=============刷新=======================//
OLED_Refresh_Gram();
}
/**************************************************************************
函数功能：虚拟示波器往上位机发送数据 关闭显示屏
入口参数：无
返回 值：无
作 者：平衡小车之家
**************************************************************************/
void DataScope(void)
{
if(++Count==1)
{
OLED_Clear();
OLED_Display_Off();
}
DataScope_Get_Channel_Data( Angle_Balance, 1 );
DataScope_Get_Channel_Data( Encoder_Right, 2 );
DataScope_Get_Channel_Data( Encoder_Left, 3 );
DataScope_Get_Channel_Data( Voltage , 4 );
DataScope_Get_Channel_Data(0, 5 ); //用您要显示的数据替换0就行了
DataScope_Get_Channel_Data(0 , 6 );//用您要显示的数据替换0就行了
DataScope_Get_Channel_Data(0, 7 );
DataScope_Get_Channel_Data( 0, 8 );
DataScope_Get_Channel_Data(0, 9 );
DataScope_Get_Channel_Data( 0 , 10);
Send_Count = DataScope_Data_Generate(10);
for( i = 0 ; i < Send_Count; i++)
{
while((USART1->SR&0X40)==0);
USART1->DR = DataScope_OutPut_Buffer[i];
}
delay_ms(50); //20HZ
}
//MiniBalance.c
#include "MiniBalance.h"
#include "math.h"
#include "led.h"
#include "mpu6050.h"
#define PI 3.14159265
/**************************************************************************
作者：平衡小车之家
淘宝店铺：http://shop114407458.taobao.com/
**************************************************************************/
/**************************************************************************
函数功能：5MS定时中断函数 5MS控制周期
入口参数：无
返回 值：无
作 者：平衡小车之家
**************************************************************************/
int Balance_Pwm,Velocity_Pwm,Turn_Pwm;
int FiveMSCount=0;
int ThreeSecFlag=0;
void TIM1_UP_TIM16_IRQHandler(void)
{
if(TIM1->SR&0X0001)//5ms定时中断
{
FiveMSCount++;
if(FiveMSCount==600)
ThreeSecFlag=1;
if(ThreeSecFlag==1)
{
distance = distance + (float)Encoder_Left*0.005;
if((Encoder_Left-Encoder_Right)>0)
turning = turning + (float)(Encoder_Left-Encoder_Right)*0.005;
else
turning = turning + (float)(Encoder_Right-Encoder_Left)*0.005;
}
TIM1->SR&=~(1<<0); //===清除定时器1中断标志位
readEncoder(); //===读取编码器的值
Led_Flash(400); //===LED闪烁;
Get_battery_volt(); //===获取电池电压
key(100); //===扫描按键状态
Get_Angle(Way_Angle); //===更新姿态
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(Turn_Off(Angle_Balance,Voltage)==0) //===如果不存在异常
Set_Pwm(Moto1,Moto2); //===赋值给PWM寄存器
}
}
/**************************************************************************
函数功能：直立PD控制
入口参数：角度、角速度
返回 值：直立控制PWM
作 者：平衡小车之家
**************************************************************************/
int balance(float Angle,float Gyro)
{
float Bias;
int balance;
Bias=Angle+0; //===求出平衡的角度中值 和机械相关 +0意味着身重中心在0度附近 如果身重中心在5度附近 那就应该减去5
balance=35*Bias+Gyro*0.125;//===计算平衡控制的电机PWM PD控制
return balance;
}
/**************************************************************************
函数功能：速度PI控制
入口参数：左轮编码器、右轮编码器
返回 值：速度控制PWM
作 者：平衡小车之家
**************************************************************************/
int velocity(int encoder_left,int encoder_right)
{
static int Velocity,Encoder_Least,Encoder,Movement;
static int Encoder_Integral;
//=============遥控前进后退部分=======================//
if(1==Flag_Qian) Movement=-700; //===如果前进标志位置1 位移为负
else if(1==Flag_Hou) Movement=700; //===如果后退标志位置1 位移为正
else Movement=0;
//=============速度PI控制器======================//
Encoder_Least =Encoder_Left+Encoder_Right; //===获取最新速度偏差
Encoder *= 0.8; //===一阶低通滤波器
Encoder += Encoder_Least*0.2; //===一阶低通滤波器
if(Turn_Off(Angle_Balance,Voltage)==0) //为了防止积分影响用户体验，只有电机开启的时候才开始积分
{
Encoder_Integral +=Encoder; //===积分出位移 积分时间：5ms
Encoder_Integral=Encoder_Integral-Movement; //===接收遥控器数据，控制前进后退
}
if(Encoder_Integral>360000) Encoder_Integral=360000; //===积分限幅
if(Encoder_Integral<-360000) Encoder_Integral=-360000; //===积分限幅
Velocity=Encoder*4+Encoder_Integral/140; //===速度PI控制器
if(Turn_Off(Angle_Balance,Voltage)==1) Encoder_Integral=0; //===电机关闭后清除积分
return Velocity;
}
/**************************************************************************
函数功能：转向PD控制
入口参数：左轮编码器、右轮编码器、Z轴陀螺仪
返回 值：转向控制PWM
作 者：平衡小车之家
**************************************************************************/
int turn(int encoder_left,int encoder_right,float gyro)//转向控制
{
static int Turn_Target,Turn,Encoder_temp,Turn_Convert=3,Turn_Count;
//int Turn_Bias,Turn_Amplitude=1500/Way_Angle+800; //===Way_Angle为滤波方法，当是1时，即由DMP获取姿态，Turn_Amplitude取大，卡尔曼和互补是，取小，因为这两种滤波算法效果稍差。
int Turn_Bias,Turn_Amplitude=700;
static long Turn_Bias_Integral;
//=============遥控左右旋转部分=======================//
if(1==Flag_Left||1==Flag_Right) //这一部分主要是根据旋转前的速度调整速度的起始速度，增加小车的适应性
{
if(++Turn_Count==1)
Encoder_temp=myabs(encoder_left+encoder_right);
Turn_Convert=2000/Encoder_temp;
if(Turn_Convert<3)Turn_Convert=3;
if(Turn_Convert>10)Turn_Convert=10;
}
else
{
Turn_Convert=3;
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;
//=============转向PD控制器=======================//
Turn_Bias=Encoder_Left-Encoder_Right; //===计算转向速度偏差
if(Turn_Off(Angle_Balance,Voltage)==0) //为了防止积分影响用户体验，只有电机开启的时候才开始积分
{
Turn_Bias_Integral+=Turn_Bias; //转向速度偏差积分得到转向偏差
Turn_Bias_Integral-=Turn_Target; //获取遥控器数据
}
if(Turn_Bias_Integral>1800) Turn_Bias_Integral=1800; //===积分限幅
if(Turn_Bias_Integral<-1800) Turn_Bias_Integral=-1800; //===积分限幅
Turn=Turn_Bias_Integral*2+gyro/12; //===结合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);
}
/**************************************************************************
函数功能：读取编码器的数据并进行数据类型转换
入口参数：无
返回 值：无
作 者：平衡小车之家
**************************************************************************/
void readEncoder(void)
{
u16 Encoder_L,Encoder_R; //===左右编码器的脉冲计数
Encoder_R = TIM4 -> CNT; //===获取正交解码1数据
TIM4 -> CNT=0; //===计数器清零
Encoder_L= TIM2 -> CNT; //===获取正交解码2数据
TIM2 -> CNT=0; //===计数器清零
if(Encoder_L>32768) Encoder_Left=Encoder_L-65000; else Encoder_Left=Encoder_L; //===数据类型转换
Encoder_Left=-Encoder_Left;
if(Encoder_R>32768) Encoder_Right=Encoder_R-65000; else Encoder_Right=Encoder_R;//===数据类型转换
}
/**************************************************************************
函数功能：限制PWM赋值
入口参数：无
返回 值：无
作 者：平衡小车之家
**************************************************************************/
void Xianfu_Pwm(void)
{
int Amplitude=3500; //===PWM满幅是3600 限制在3500
if(Moto1<-Amplitude) Moto1=-Amplitude;
if(Moto1>Amplitude) Moto1=Amplitude;
if(Moto2<-Amplitude) Moto2=-Amplitude;
if(Moto2>Amplitude) Moto2=Amplitude;
}
/**************************************************************************
函数功能：异常关闭电机
入口参数：倾角和电压
返回 值：1：异常 0：正常
作 者：平衡小车之家
**************************************************************************/
u8 Turn_Off(float angle, int voltage)
{
u8 temp;
if(angle<-40||angle>40||1==Flag_Stop||Voltage<1110)//===电压低于11.1V 关闭电机
{ //===倾角大于40度关闭电机
temp=1; //===Flag_Stop置1关闭电机
AIN1=0; //===可自行增加主板温度过高时关闭电机
AIN2=0;
BIN1=0;
BIN2=0;
}
else
temp=0;
return temp;
}
/**************************************************************************
函数功能：获取角度
入口参数：获取角度的算法 1：无 2：卡尔曼 3：互补滤波
返回 值：无
作 者：平衡小车之家
**************************************************************************/
void Get_Angle(u8 way)
{
float Accel_Y,Accel_X,Accel_Z,Gyro_Y,Gyro_Z;
if(way==1) //DMP没有涉及到严格的时序问题，在主函数读取
{
}
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; //数据类型转换
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; //更新转向角速度
}
}
而后在MiniBalance.h中放入extern int ThreeSecFlag;即可。
实验视频在审核，估计得在下一楼再 放出。。
STM32
MCU
通讯及无线技术
物联网技术
电子DIY
板卡试用
基础知识
软件与操作系统
我爱生活
小e食堂
