 const uint8_t trigPin = 9;
 const uint8_t echoPin = 10;
 
+const uint8_t ledPin = 11;
+
 const uint8_t motorPin1_1 = 4;
 const uint8_t motorPin1_2 = 5;
-const uint8_t motorPin1_speed = 3;
-const uint8_t motorPin1_A = A7;
 
 const uint8_t motorPin2_1 = 7;
 const uint8_t motorPin2_2 = 8;
-const uint8_t motorPin2_speed = 6;
-const uint8_t motorPin2_A = A6;
 
-/*
-const uint8_t motorPin3_1 = 10;
-const uint8_t motorPin3_2 = 11;
-const uint8_t motorPin3_speed = 9;
-const uint8_t motorPin3_A = A5;
-*/
+
+const uint8_t motorEnablePin = 3;
 
 const uint8_t rangeLimit = 15;
 

    pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
    pinMode(echoPin, INPUT); // Sets the echoPin as an Input

    // Motor
    pinMode(motorPin1_1, OUTPUT);
    pinMode(motorPin1_2, OUTPUT);
    pinMode(motorPin1_speed, OUTPUT);
    pinMode(motorPin1_A, INPUT);

    pinMode(motorPin2_1, OUTPUT);
    pinMode(motorPin2_2, OUTPUT);
    pinMode(motorPin2_speed, OUTPUT);
    pinMode(motorPin2_A, INPUT);

    /*
    pinMode(motorPin3_1, OUTPUT);
    pinMode(motorPin3_2, OUTPUT);
    pinMode(motorPin3_speed, OUTPUT);
    pinMode(motorPin3_A, INPUT);
    */

    Serial.begin(9600); // Starts the serial communication
}

    if(spinCompleted && range > 0 && range > rangeLimit)
    {
        // drive
        SetMotor(motorPin1_1, motorPin1_2, 255);
        SetMotor(motorPin2_1, motorPin2_2, 255);
    }
    else
    {
        spinCompleted = false;

        // rotate
        SetMotor(motorPin1_1, motorPin1_2, 255);
        SetMotor(motorPin2_1, motorPin2_2, 0);

        if(range > (rangeLimit * 2))
        {
