LEGO+ardunio的避障车 DF创客社区 - Powered by Discuz! Archiver

高三师 发表于 2017-7-27 09:31:39

LEGO+ardunio的避障车

控制部分：ardunio uno R3

L298N红板
SR04
MG90S舵机
370马达
线材若干

车体部分：
LEGO 科技系列小颗粒，采用螺杆+3级减速
尼龙牛眼轮
乐高电池盒给ardunio和L298N供电
L298N 5V输出给舵机供电（ardunio上端口不够用）

代码：
分远距离采样和近距离采样，当距离小于15cm时，切换近距离采样，可实现小障碍物躲避

#include <Servo.h>

#define SERVO 6// PIN 6 为舵机控制
Servo myservo;

#define Trig 9//PIN 9 为SR04触发端
#define Echo 10//PIN 10 为SR04回波端
float cm;
int Degree;

#define EN1 3 // PIN 3 为EN1 输出
#define EN2 13 // PIN 13 为EN2 输出
#define IN1 7 // PIN 8 为IN1 输出
#define IN2 8 // PIN 7 为IN2 输出
#define IN3 11 // PIN 11 为IN3 输出
#define IN4 12 // PIN 12 为IN4 输出

/************************ setup ************************/
void setup() {
Serial.begin (9600);
myservo.attach(SERVO);//初始化舵机

pinMode(Trig,OUTPUT); //初始化SR04各IO
pinMode(Echo,INPUT);

pinMode(EN1,OUTPUT); //初始化各IO,模式为OUTPUT 输出模式
pinMode(EN2,OUTPUT);
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
pinMode(IN3,OUTPUT);
pinMode(IN4,OUTPUT);
} //end setup

/************************ loop ************************/
void loop() {
int i,j;
boolean flag=false;

Motor_Ctr(255,HIGH,LOW,255,HIGH,LOW); //前进
/*远距离测距（0-170cm），如果距障碍物距离小于15cm则跳到近距离测距（0-17cm）*/
for(j=0;j<3;j++){                      //舵机逆时针转，3个角度采样
   Servo_Ctr(30*j+64,60);             //Servo周期应大于5次Filter周期
   if(Filter(10)<15){                //SR04等待回波时间10ms
      switch(j){
         case 0:                   //60度有障碍物，左转
               Motor_Ctr(255,LOW,HIGH,255,HIGH,LOW);
               delay(100);
               break;
         case 1:                   //90度有障碍物，后退
               Motor_Ctr(255,LOW,HIGH,255,LOW,HIGH);
               delay(200);
               break;
         case 2:                   //120度有障碍物，右转
               Motor_Ctr(255,HIGH,LOW,255,LOW,HIGH);
               delay(100);
               break;
      } //end switch
   flag=!flag;                   //跳转到近距离测距
   goto Near_Detection;
   } //end if
} //end for
for(j=3;j>0;j--){                      //舵机顺时针转，3个角度采样
   Servo_Ctr(30*(j-1)+64,60);       //Servo周期应大于5次Filter周期
   if(Filter(10)<15){                //SR04等待回波时间10ms
      switch(j){
         case 1:                   //60度有障碍物，左转
               Motor_Ctr(255,LOW,HIGH,255,HIGH,LOW);
               delay(100);
               break;
         case 2:                   //90度有障碍物，后退
               Motor_Ctr(255,LOW,HIGH,255,LOW,HIGH);
               delay(200);
               break;
         case 3:                   //120度有障碍物，右转
               Motor_Ctr(255,HIGH,LOW,255,LOW,HIGH);
               delay(100);
               break;
      } //end switch
   flag=!flag;                   //flag=true,跳转到近距离测距
   goto Near_Detection;
   } //end if
} //end for
/*近距离测距（0-17cm），如果距障碍物距离大于15cm则跳出循环*/
Near_Detection:
while(flag==true){
   for(i=0;i<5;i++){             //舵机逆时针转，5个角度采样
         Servo_Ctr(45*i+4,10);
         if(Filter(1)<15){          //SR04等待回波时间1ms
            switch(i){
               case 0:             //0度有障碍物，大角度左转
                     Motor_Ctr(255,LOW,HIGH,255,HIGH,LOW);
                     delay(60);
                     break;
               case 1:             //45度有障碍物，小角度左转
                     Motor_Ctr(255,LOW,HIGH,255,HIGH,LOW);
                     delay(30);
                     break;
               case 2:             //90度有障碍物，后退
                     Motor_Ctr(255,LOW,HIGH,255,LOW,HIGH);
                     delay(100);
                     break;
               case 3:             //135度有障碍物，小角度右转
                     Motor_Ctr(255,HIGH,LOW,255,LOW,HIGH);
                     delay(30);
                     break;
               case 4:             //180度有障碍物，大角度右转
                     Motor_Ctr(255,HIGH,LOW,255,LOW,HIGH);
                     delay(60);
                     break;
            } //end switch
         } //end if
         else{                      //距离大于15cm，跳出循环
            break;
         } //end else
   } //end for

   for(i=5;i>0;i--){             //舵机顺时针转，5个角度采样
         Servo_Ctr(45*(i-1)+4,10);
         if(Filter(1)<15){          //SR04等待回波时间1ms
            switch(i){
               case 1:             //0度有障碍物，大角度左转
                     Motor_Ctr(255,LOW,HIGH,255,HIGH,LOW);
                     delay(60);
                     break;
               case 2:             //45度有障碍物，小角度左转
                     Motor_Ctr(255,LOW,HIGH,255,HIGH,LOW);
                     delay(30);
                     break;
               case 3:             //90度有障碍物，后退
                     Motor_Ctr(255,LOW,HIGH,255,LOW,HIGH);
                     delay(100);
                     break;
               case 4:             //135度有障碍物，小角度右转
                     Motor_Ctr(255,HIGH,LOW,255,LOW,HIGH);
                     delay(30);
                     break;
               case 5:             //180度有障碍物，大角度右转
                     Motor_Ctr(255,HIGH,LOW,255,LOW,HIGH);
                     delay(60);
                     break;
            } //end switch
         } //end if
         else{                      //距离大于15cm，跳出循环
            break;
         } //end else
   } //end for
   break;
} //end while
} //end loop

/************************ 马达控制 ************************/
void Motor_Ctr(int Speed1,char Status1,char Status2,int Speed2,char Status3,char Status4) {
analogWrite(EN1,Speed1);
analogWrite(EN2,Speed2);
digitalWrite(IN1,Status1);
digitalWrite(IN2,Status2);
digitalWrite(IN3,Status3);
digitalWrite(IN4,Status4);
} //end Motor_Ctr

/************************ 舵机控制 ************************/
void Servo_Ctr(int Degree,int period){
myservo.write(Degree);             //Degree为舵机角度值,需加4度补偿
Serial.println("Serivo active!");
delay(period);
} //end Servo_Ctr

/************************ 超声波测距 ************************/
float SR04(float wait_time){
float temp;
/*给Trig发送一个低高低的短时间脉冲,触发测距*/
digitalWrite(Trig, LOW);             //给Trig发送一个低电平
delayMicroseconds(2);                //等待 2微妙
digitalWrite(Trig,HIGH);             //给Trig发送一个高电平
delayMicroseconds(10);                //等待 10微妙
digitalWrite(Trig, LOW);             //给Trig发送一个低电平

temp = float(pulseIn(Echo, HIGH));    //存储回波等待时间
cm = (temp * 17 )/1000;             //把回波时间换算成cm
Serial.print("Echo =");
Serial.print(temp);                //串口输出等待时间的原始数据
Serial.print(" | | Distance = ");
Serial.print(cm);                   //串口输出距离换算成cm的结果
Serial.println("cm");
delay(wait_time);
return cm;
}
/************************距离值采样滤波************************/
/*SR04采样时会有超大值和负值出现，使用中位值平均滤波法，采样5次，
去掉最大值和最小值后取平均值*/
float Filter(int period){
int filter_N=5;                      //定义采样数量为5
float filter_buf;
int i,j;
float filter_temp,filter_sum=0,filter_avg=0;

for(i=0;i<filter_N;i++){             //从超声波传感器SR04采样
filter_buf=SR04(period);
} //end for

for(j=0;j<filter_N-1;j++){          //使用冒泡法将采样值从小到大排列
for(i=0;i<filter_N-1;i++){
   if(filter_buf>filter_buf){
   filter_temp=filter_buf;
   filter_buf=filter_buf;
   filter_buf=filter_temp;
   } //end if
} //end for
} //end for

for(i=1;i<filter_N-1;i++){          //去除最小最大值
filter_sum+=filter_buf;
} //end for

filter_avg=filter_sum/(filter_N-2);
Serial.print("filter_avg = ");
Serial.println(filter_avg);
return filter_avg;                //返回平均值
} //end Filter

xiaohe9527 发表于 2017-7-29 15:04:30

大神，膜拜

xiaohe9527 发表于 2017-7-29 16:08:19

for(i=0;i<filter_N;i++){ //从超声波传感器SR04采样
filter_buf=SR04(period);
} //end for

超声波采样存在数组中，fliter_buf=SR04(period);可以直接这样写fliter_buf=SR04(period);

xiaohe9527 发表于 2017-7-29 16:32:48

你的冒泡排序好像有点问题的

xiaohe9527 发表于 2017-7-29 16:47:26

#define EN1 3 // PIN 3 为EN1 输出
#define EN2 13 // PIN 13 为EN2 输出
#define IN1 7 // PIN 8 为IN1 输出
#define IN2 8 // PIN 7 为IN2 输出
#define IN3 11 // PIN 11 为IN3 输出
#define IN4 12 // PIN 12 为IN4 输出

用L298控制两路马达，为什么要用6个端口啊，印象中4个端口就够了啊

NodeMaker 发表于 2017-8-4 17:53:36

xiaohe9527 发表于 2017-7-29 16:47
#define EN1 3 // PIN 3 为EN1 输出
#define EN2 13 // PIN 13 为EN2 输出
#define IN1 7 // PI ...

EN1和EN2 是使能引脚（高电平时芯片工作，低电平不工作）

平时用四个脚时，是将使能引脚接高电平，然后在 IN1、2、3、4脚上用PWM控制速度和方向

而楼主是在 IN1、2、3、4脚上接高电平或低电平控制方向，在使能引脚上用PWM控制速度

xiaohe9527 发表于 2017-8-5 06:52:04

NodeMaker 发表于 2017-8-4 17:53
EN1和EN2 是使能引脚（高电平时芯片工作，低电平不工作）

平时用四个脚时，是将使能引脚接高电平，然后 ...

6路控制已经理解了
4路的话，只能高低电平吧，不能PWM了吧？

NNN904 发表于 2017-8-21 22:04:30

6666

页: [1]

DF创客社区 - 分享创造的喜悦's Archiver

LEGO+ardunio的避障车