|
10086| 7
|
[项目] 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[filter_N]; 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[i+1]){ filter_temp=filter_buf; filter_buf=filter_buf[i+1]; filter_buf[i+1]=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 |
|
for(i=0;i<filter_N;i++){ //从超声波传感器SR04采样 filter_buf=SR04(period); } //end for 超声波采样存在数组中，fliter_buf[i]=SR04(period);可以直接这样写fliter_buf=SR04(period); |
|
#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 6路控制已经理解了 4路的话，只能高低电平吧，不能PWM了吧？ |
© 2013-2024 Comsenz Inc. Powered by Discuz! X3.4 Licensed
置顶卡
变色卡
千斤顶
