drive traxxas

/*
Use pin 2 and 4 for sound sensors trig/ echo pins.
ESC is attached to pin 5 (any PWM pin will work - but here we use 5)
Pin 13 has led on arduino board- so use it to signal stop
*/

#include <Servo.h>


Servo esc;

int count = 3;
int diff = 0;
int intv = 100;

int done = 0;

const int trigPin = 2;
const int echoPin = 4;

void setup() {
  pinMode(13, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  esc.attach(5);
  delay(10);  
  esc.write(90);  
  delay(1000);
}

void stopEngine()
{
    digitalWrite(13, HIGH);
    esc.write(90);
}

void applyBrake2()
{
    digitalWrite(13, HIGH);
    esc.write(0);  
}

void forward()
{
  digitalWrite(13, LOW);
  esc.write(99);
}

void applyBrake()
{
  digitalWrite(13, HIGH);
  esc.write(254);
}

void loop() { 
  if(!done)
  {
    int dist = getFrontDistance();
    
    if(dist < 90)
    {
      applyBrake();
      done = 1;
    }
    else if(dist < 120)
    {
      stopEngine();
    }
    else
    {
      forward();
    }    
  }
  
  delay(1)  
}


long getFrontDistance()
{
  long duration;
  
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
    
  duration = pulseIn(echoPin, HIGH);
  return (duration / 29 / 2);
}