首页 › Arduino › 新手制作丑陋避障小车一枚

kewei_9900 发表于 2014-7-24 21:00:49

新手制作丑陋避障小车一枚

初学arduino，做了一台避障小车。其上安装了20格的码盘用于直行，安装了超声和红外探头用于避障。
整车就这模样
带码盘的齿轮箱
丑陋的排线
手工焊的红外对管模块
我是胶带党
码盘检测小车两轮的速度，然后用PI算法（没必要用D）控制小车走直线。
代码如下：const int TrigPin = 8;
const int EchoPin = 7;//设置超声波端口
const int Kleft = 3;
const int Kright = 2;//设置码盘端口
const int mlf=10;
const int mlb=11;
const int mrf=5;
const int mrb=6;//设置电机接口
const int IFleft=19;
const int IFright=18;
int count_left;
int count_right;//定义码盘计数器
long old_time;
long time;//定义计时器
int PID=0;
int I=0;//PID控制用变量
float cm; //超声测距结果
void setup()
{
pinMode(TrigPin, OUTPUT);
pinMode(EchoPin, INPUT);//设置超声波
pinMode(mlf,OUTPUT);
pinMode(mlb,OUTPUT);
pinMode(mrf,OUTPUT);
pinMode(mrb,OUTPUT);//设定电机
pinMode(Kleft,INPUT);
pinMode(Kright,INPUT);//码盘
attachInterrupt(0, Code_right, FALLING);
attachInterrupt(1, Code_left, FALLING);//中断设置
}
void advance(){
analogWrite(mlf,120+PID);
digitalWrite(mlb,LOW);
analogWrite(mrf,120);
digitalWrite(mrb,LOW);
}//定义前进函数
void back(){
analogWrite(mlb,120+PID);
digitalWrite(mlf,LOW);
analogWrite(mrb,120);
digitalWrite(mrf,LOW);
}//定义后退函数
void left(){
digitalWrite(mlf,LOW);
analogWrite(mlb,120);
analogWrite(mrf,120);
digitalWrite(mrb,LOW);
}//定义左转函数
void right(){
analogWrite(mlf,120);
digitalWrite(mlb,LOW);
digitalWrite(mrf,LOW);
analogWrite(mrb,120);
}//定义右转函数
void Stop(){
digitalWrite(mlf,LOW);
digitalWrite(mlb,LOW);
digitalWrite(mrf,LOW);
digitalWrite(mrb,LOW);
}//定义停止函数
void loop()
{
old_time=millis();
while(millis()-old_time<500){
advance();
}//前进并计数
detachInterrupt(0); // 关闭外部中断0
detachInterrupt(1); // 关闭外部中断1
I+=count_right-count_left;
PID=(count_right-count_left)*4+I;//PI算法
if(count_right<10||count_right<10){
    old_time=millis();
   while(millis()-old_time<700){
   back();
   }
   while(millis()-old_time<1100){
   Stop();
   }
   while(millis()-old_time<1600){
   left();
   }
}//防卡死算法
count_left=0;
count_right=0;//码盘计数归零
//超声测距算法(需改进)
digitalWrite(TrigPin, LOW); //低高低电平发一个短时间脉冲去TrigPin
delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);
delayMicroseconds(10);
digitalWrite(TrigPin, LOW);
cm = pulseIn(EchoPin, HIGH) / 58.0; //将回波时间换算成cm
cm = (int(cm * 100.0)) / 100.0; //保留两位小数
if(cm<30){
   old_time=millis();
   while(millis()-old_time<500){
   right();
   }
}//超声避障算法
if(analogRead(IFleft)<700){
    old_time=millis();
   while(millis()-old_time<500){
   right();
   }
}
if(analogRead(IFright)<700){
   old_time=millis();
   while(millis()-old_time<500){
   left();
   }
}//红外避障算法
attachInterrupt(0, Code_right,FALLING); // 重新开放外部中断0
attachInterrupt(1, Code_left,FALLING); // 重新开放外部中断1
}
//右侧电机码盘计数中断函数
void Code_right()
{
count_right += 1; //右侧编码器码盘计数加一
}
//左侧电机码盘计数中断函数
void Code_left()
{
count_left += 1; // 编码器码盘计数加一   
}
这个算法问题还是很大的，一是红外避障部分没有排除环境红外光；二是使用了原始的超声测距算法；三是程序运行周期太长，反应迟钝。我想知道论坛里还有比我这更烂的小车么。

kewei_9900 发表于 2014-7-24 21:02:48

本帖最后由 迷你强 于 2014-7-27 14:36 编辑

注释怎么挂了const int TrigPin = 8;
const int EchoPin = 7;//设置超声波端口
const int Kleft = 3;
const int Kright = 2;//设置码盘端口
const int mlf=10;
const int mlb=11;
const int mrf=5;
const int mrb=6;//设置电机接口
const int IFleft=19;
const int IFright=18;
int count_left;
int count_right;//定义码盘计数器
long old_time;
long time;//定义计时器
int PID=0;
int I=0;//PID控制用变量
float cm; //超声测距结果
void setup()
{
pinMode(TrigPin, OUTPUT);
pinMode(EchoPin, INPUT);//设置超声波
pinMode(mlf,OUTPUT);
pinMode(mlb,OUTPUT);
pinMode(mrf,OUTPUT);
pinMode(mrb,OUTPUT);//设定电机
pinMode(Kleft,INPUT);
pinMode(Kright,INPUT);//码盘
attachInterrupt(0, Code_right, FALLING);
attachInterrupt(1, Code_left, FALLING);//中断设置
}
void advance(){
analogWrite(mlf,120+PID);
digitalWrite(mlb,LOW);
analogWrite(mrf,120);
digitalWrite(mrb,LOW);
}//定义前进函数
void back(){
analogWrite(mlb,120+PID);
digitalWrite(mlf,LOW);
analogWrite(mrb,120);
digitalWrite(mrf,LOW);
}//定义后退函数
void left(){
digitalWrite(mlf,LOW);
analogWrite(mlb,120);
analogWrite(mrf,120);
digitalWrite(mrb,LOW);
}//定义左转函数
void right(){
analogWrite(mlf,120);
digitalWrite(mlb,LOW);
digitalWrite(mrf,LOW);
analogWrite(mrb,120);
}//定义右转函数
void Stop(){
digitalWrite(mlf,LOW);
digitalWrite(mlb,LOW);
digitalWrite(mrf,LOW);
digitalWrite(mrb,LOW);
}//定义停止函数
void loop()
{
old_time=millis();
while(millis()-old_time<500){
advance();
}//前进并计数
detachInterrupt(0); // 关闭外部中断0
detachInterrupt(1); // 关闭外部中断1
I+=count_right-count_left;
PID=(count_right-count_left)*4+I;//PI算法
if(count_right<10||count_right<10){
    old_time=millis();
   while(millis()-old_time<700){
   back();
   }
   while(millis()-old_time<1100){
   Stop();
   }
   while(millis()-old_time<1600){
   left();
   }
}//防卡死算法
count_left=0;
count_right=0;//码盘计数归零
//超声测距算法(需改进)
digitalWrite(TrigPin, LOW); //低高低电平发一个短时间脉冲去TrigPin
delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);
delayMicroseconds(10);
digitalWrite(TrigPin, LOW);
cm = pulseIn(EchoPin, HIGH) / 58.0; //将回波时间换算成cm
cm = (int(cm * 100.0)) / 100.0; //保留两位小数
if(cm<30){
   old_time=millis();
   while(millis()-old_time<500){
   right();
   }
}//超声避障算法
if(analogRead(IFleft)<700){
    old_time=millis();
   while(millis()-old_time<500){
   right();
   }
}
if(analogRead(IFright)<700){
   old_time=millis();
   while(millis()-old_time<500){
   left();
   }
}//红外避障算法
attachInterrupt(0, Code_right,FALLING); // 重新开放外部中断0
attachInterrupt(1, Code_left,FALLING); // 重新开放外部中断1
}
//右侧电机码盘计数中断函数
void Code_right()
{
count_right += 1; //右侧编码器码盘计数加一
}
//左侧电机码盘计数中断函数
void Code_left()
{
count_left += 1; // 编码器码盘计数加一   
}

kewei_9900 发表于 2014-7-25 08:46:45

附上视频http://v.youku.com/v_show/id_XNzQ1NTkzMDM2.html

wetnt 发表于 2014-7-26 23:35:39

真好，真好需要收藏备用！

shh80s 发表于 2014-7-27 15:38:36

朋友有硬件连接图吗？还有你是使用L298驱动还是直接驱动的？

查看完整版本: 新手制作丑陋避障小车一枚