Untitled
unknown
c_cpp
a year ago
3.4 kB
11
Indexable
#include <Servo.h> Servo s; int m1p1 = 2; int m1p2 = 3; int m2p1 = 5; int m2p2 = 4; int tp = 6; int ep = 7; float duration,distance; void setup(){ pinMode(tp, OUTPUT); pinMode(ep, INPUT); Serial.begin(9600); s.attach(8); pinMode(m1p1,OUTPUT); pinMode(m1p2,OUTPUT); pinMode(m2p1,OUTPUT); pinMode(m2p2,OUTPUT); } void loop(){ int ls; int cs; int rs; for(int i=0;i<=180;){ s.write(i); delay(500); digitalWrite(tp,LOW); delayMicroseconds(2); digitalWrite(tp,HIGH); delayMicroseconds(10); digitalWrite(tp,LOW); duration = pulseIn(ep,HIGH); distance = (duration*.0343)/2; //Serial.print("Distance: "); //Serial.println(distance); if(i==0){rs = distance;} if(i==90){cs = distance;} if(i==180){ls = distance;} i+=90; } if(ls<=10){ls=1;} if(ls>10){ls=0;} if(cs<=10){cs=1;} if(cs>10){cs=0;} if(rs<=10){rs=1;} if(rs>10){rs=0;} Serial.print("LS: "); Serial.println(ls); Serial.print("CS: "); Serial.println(cs); Serial.print("RS: "); Serial.println(rs); Serial.println(" "); if(ls==0 && cs==0 && rs==0){ digitalWrite(m1p1,HIGH); digitalWrite(m1p2,LOW); digitalWrite(m2p1,HIGH); digitalWrite(m2p2,LOW); delay(1000); digitalWrite(m1p1,LOW); digitalWrite(m1p2,LOW); digitalWrite(m2p1,LOW); digitalWrite(m2p2,LOW); } if(ls==0 && cs==0 && rs==1){ digitalWrite(m1p1,LOW); digitalWrite(m1p2,HIGH); digitalWrite(m2p1,HIGH); digitalWrite(m2p2,LOW); delay(1000); digitalWrite(m1p1,LOW); digitalWrite(m1p2,LOW); digitalWrite(m2p1,LOW); digitalWrite(m2p2,LOW); } if(ls==0 && cs==1 && rs==0){ digitalWrite(m1p1,LOW); digitalWrite(m1p2,HIGH); digitalWrite(m2p1,HIGH); digitalWrite(m2p2,LOW); delay(1000); digitalWrite(m1p1,LOW); digitalWrite(m1p2,LOW); digitalWrite(m2p1,LOW); digitalWrite(m2p2,LOW); } if(ls==0 && cs==1 && rs==1){ digitalWrite(m1p1,LOW); digitalWrite(m1p2,HIGH); digitalWrite(m2p1,HIGH); digitalWrite(m2p2,LOW); delay(1000); digitalWrite(m1p1,LOW); digitalWrite(m1p2,LOW); digitalWrite(m2p1,LOW); digitalWrite(m2p2,LOW); } if(ls==1 && cs==0 && rs==0){ digitalWrite(m1p1,HIGH); digitalWrite(m1p2,LOW); digitalWrite(m2p1,HIGH); digitalWrite(m2p2,LOW); delay(1000); digitalWrite(m1p1,LOW); digitalWrite(m1p2,LOW); digitalWrite(m2p1,LOW); digitalWrite(m2p2,LOW); } if(ls==1 && cs==0 && rs==1){ digitalWrite(m1p1,HIGH); digitalWrite(m1p2,LOW); digitalWrite(m2p1,HIGH); digitalWrite(m2p2,LOW); delay(1000); digitalWrite(m1p1,LOW); digitalWrite(m1p2,LOW); digitalWrite(m2p1,LOW); digitalWrite(m2p2,LOW); } if(ls==1 && cs==1 && rs==0){ digitalWrite(m1p1,HIGH); digitalWrite(m1p2,LOW); digitalWrite(m2p1,LOW); digitalWrite(m2p2,HIGH); delay(1000); digitalWrite(m1p1,LOW); digitalWrite(m1p2,LOW); digitalWrite(m2p1,LOW); digitalWrite(m2p2,LOW); } if(ls==1 && cs==1 && rs==1){ digitalWrite(m1p1,HIGH); digitalWrite(m1p2,LOW); digitalWrite(m2p1,LOW); digitalWrite(m2p2,HIGH); delay(1000); digitalWrite(m1p1,LOW); digitalWrite(m1p2,LOW); digitalWrite(m2p1,LOW); digitalWrite(m2p2,LOW); } }
Editor is loading...
Leave a Comment