循跡車
#include <Servo.h>
#define Trig 3
#define Echo 2
#define R1 7
#define R2 8
#define L1 12
#define L2 11
int temp = 30;
double d;
int w,x,y,z;
int L,R;
void setup() {
Serial.begin(9600);
pinMode(Trig,OUTPUT);
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
pinMode(9,OUTPUT);
pinMode(10,OUTPUT);
pinMode(L1,INPUT);
pinMode(L2,INPUT);
pinMode(R1,INPUT);
pinMode(R2,INPUT);
}
void loop(){
track();
if(w==0&&x==0&&y==0&&z==0){
forward();
}
if(w==1&&x==1&&y==1&&z==1){
stand();
}
if(w==0&&x==1&&y==0&&z==0){
L=1;
do{
do{
x=digitalRead(L2); //碰觸黑線
left2();
}while(x==1);
do{
x=digitalRead(L2); //超過
left2();
}while(x==0);
do{
x=digitalRead(L2); //碰觸黑線
left2();
}while(x==1);
L=0;
}while(L==1);
}
}
void track(){
w=digitalRead(L1);
x=digitalRead(L2);
y=digitalRead(R2);
z=digitalRead(R1);
}
void dist(){
digitalWrite(Trig, LOW);
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
double t = pulseIn(Echo, HIGH);
d = (331+0.6*temp)*t/20000 ;
Serial.println(d);
}
void forward(){
analogWrite(5,255);
analogWrite(6,0);
analogWrite(9,0);
analogWrite(10,255);
}
void stand(){
analogWrite(5,0);
analogWrite(6,0);
analogWrite(9,0);
analogWrite(10,0);
}
void left2(){
analogWrite(5,0);
analogWrite(6,0);
analogWrite(9,128);
analogWrite(10,0);
}
void left1(){
analogWrite(5,255);
analogWrite(6,0);
analogWrite(9,128);
analogWrite(10,0);
}
void right1(){
analogWrite(5,0);
analogWrite(6,128);
analogWrite(9,0);
analogWrite(10,255);
}
void right2(){
analogWrite(5,0);
analogWrite(6,0);
analogWrite(9,0);
analogWrite(10,255);
}
void back(){
analogWrite(5,255);
analogWrite(6,0);
analogWrite(9,0);
analogWrite(10,255);
}