Drive Traxxas Slash 4x4 using arduino

drive.ino

#include <Servo.h>

Servo myservo;
Servo stearing;

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

// for power servo
int min = 83;
int max = 88;

void setup() {
    Serial.begin(9600);

    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT);

    myservo.attach(5);
    stearing.attach(9);
    stearing.write(90);
    myservo.write(90);
    delay(1000);
}

void forward() {  
    for (int pos = min; pos <= max; pos += 1) {
        myservo.write(pos);
        delay(45);
    }

    state = 1;
}

void applyBrake() {
    myservo.write(0);
    state = 9;
    delay(100);
}

void stopEngine() {

    for (int pos = max; pos <= min; pos -= 1) {
        myservo.write(pos);
        delay(1);
    }
    
    state = 2;
}

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

void turnLeft() {
    stearing.write(30);
}

void turnRight() {
    stearing.write(120);
}

void goStraight() {
    stearing.write(90);
}


void loop() {

    char buffer[10];


    if (state) {
        int dist = getFrontDistance();


        if (dist < 30 && state == 1) {
            applyBrake();
            sprintf(buffer, "%d", dist);
            Serial.println(buffer);
        }
        else if (state && state != 1) {
            //forward();
        }
    }

    delay(1);

    int cmd;

    if (Serial.available() > 0) {

        // read the incoming byte:
        cmd = Serial.read();

        if (cmd == 'B') {
            applyBrake();
            state = 0;
            Serial.println("B");
        }
        if (cmd == 'F') {
            forward();
            Serial.println("F");
        }

        if (cmd == 'L') {
            turnLeft();
            Serial.println("L");
        }

        if (cmd == 'R') {
            turnRight();
            Serial.println("R");
        }

        if (cmd == 'M') {
            goStraight();
            Serial.println("M");
        }
    }
}