Untitled

#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);
  }
}