Arustocrats Breakfast Kart - Arduino by Blue Martin

ABK_code.ino

// CODE TO DRIVE A VERY SIMPLE AUTONOMOUS KART USING A KID'S CART WITH FRONT CASTERS, 
// THREE ULTRASONIC MODULE SENSORS (HC-SR04), TWO 24V 250W BRUSHED MOTORS, 
// TWO VICTOR 883 ESCs, AND AN ARUDINO UNO
// Author: Blue Martin

#include <Servo.h>

//choose I/O pins to assign each sensor to
const int LeftTriggerPin = 2;
const int LeftEchoPin = 3;
const int CenterTriggerPin = 4;
const int CenterEchoPin = 5;
const int RightTriggerPin = 6;
const int RightEchoPin = 7;

int cmL, cmC, cmR, SensorResult;
long duration; // must be "long" otherwise readings will be negative and incorrect
const int startLimit = 30; //when the cart starts moving
const int endLimit = 60; //when the cart stops moving

//Victor 883 and 24V 250W motor: PWM 0-86 is CCW (left:forward, right:rev),
//87-93 is neutral, 94-180+ is CW (left:rev, right:forward). May vary.
Servo VictorLeft;
Servo VictorRight;

//motor speeds, may vary!
const int FastForwardL = 82; //CCW
const int SlowForwardL = 86; //CCW
const int SlowReverseL = 94; //CW
const int neutral = 90;
const int FastForwardR = 97; //CW
const int SlowForwardR = 93; //CW
const int SlowReverseR = 83; //CCW needs to faster because of drag! (86-3)

void setup() {

    delay(2000);

    Serial.begin(9600);

    VictorLeft.attach(10);
    VictorRight.attach(9);

}

void loop(){

    cmL = SensorRead(LeftTriggerPin, LeftEchoPin);
    delay(2);
    cmC = SensorRead(CenterTriggerPin, CenterEchoPin);
    delay(2);
    cmR = SensorRead(RightTriggerPin, RightEchoPin);
    delay(2);

    //1: if left only <= threshold ... left motor slow, right motor stops
    if (cmL <= startLimit && cmC > startLimit && cmR > startLimit)
    {
        while (cmL <= endLimit)
        {
            VictorLeft.write(SlowForwardL);
            VictorRight.write(neutral);

            cmL = SensorRead(LeftTriggerPin, LeftEchoPin);
            delay(2);
            cmC = SensorRead(CenterTriggerPin, CenterEchoPin);
            delay(2);
            cmR = SensorRead(RightTriggerPin, RightEchoPin);
            delay(2);
        }
    }

    //2: if right only <= threshold ... left motor stops, right motor slow
    else if (cmL > startLimit && cmC > startLimit && cmR <= startLimit)
    {
        while (cmR <= endLimit)
        {
            VictorLeft.write(neutral);
            VictorRight.write(SlowForwardR);

            cmL = SensorRead(LeftTriggerPin, LeftEchoPin);
            delay(2);
            cmC = SensorRead(CenterTriggerPin, CenterEchoPin);
            delay(2);
            cmR = SensorRead(RightTriggerPin, RightEchoPin);
            delay(2);
        }
    }

    //3: if only center <= threshold ... stop both motors, reverse
    else if (cmL > startLimit && cmC <= startLimit && cmR > startLimit)
    {
        VictorLeft.write(neutral);
        VictorRight.write(neutral);
        delay(500);

        VictorLeft.write(SlowReverseL);
        VictorRight.write(SlowReverseR);
        delay(2000);

        VictorLeft.write(neutral);
        VictorRight.write(neutral);
        delay(500);

        cmL = SensorRead(LeftTriggerPin, LeftEchoPin);
        delay(2);
        cmC = SensorRead(CenterTriggerPin, CenterEchoPin);
        delay(2);
        cmR = SensorRead(RightTriggerPin, RightEchoPin);
        delay(2);
    }

    //4: if all three > threshold ... both motors goes fast!
    else
    {
        VictorLeft.write(FastForwardL);
        VictorRight.write(FastForwardR);

        cmL = SensorRead(LeftTriggerPin, LeftEchoPin);
        delay(2);
        cmC = SensorRead(CenterTriggerPin, CenterEchoPin);
        delay(2);
        cmR = SensorRead(RightTriggerPin, RightEchoPin);
        delay(2);
    }

    /*
    // print sensor readings for testing only (Ctrl + Shift + M)
    Serial.print("cmL:");
    Serial.println(cmL);
    Serial.print("cmC:");
    Serial.println(cmC);
    Serial.print("cmR:");
    Serial.println(cmR);
    Serial.print('\n');

    delay(500);
    */

}

int SensorRead(int x, int y) //Ultrasonic sensor read and return
// x = trigger pin, y = echo pin
{
    pinMode(x, OUTPUT);
    digitalWrite(x, LOW);
    delayMicroseconds(2);
    digitalWrite(x, HIGH);
    delayMicroseconds(5);
    digitalWrite(x, LOW);
    pinMode(y, INPUT);
    duration = pulseIn(y, HIGH);

    SensorResult = microsecondsToCentimeters(duration);
    return SensorResult;
}

long microsecondsToCentimeters(long microseconds) //convert microsec to cm
{
    return microseconds/29/2;
}

Victor_ESC_calibrate.ino

// Calbrate two Victor ESCs with two motors
// Author: Blue Martin

#include <Servo.h>

//Victor 883: PWM 0-86 is CCW, 87-93 is neutral, 94-180+ is CW
Servo VictorLeft;
Servo VictorRight;

void setup() {

    Serial.begin(9600);

    VictorLeft.attach(10); // attach left motor to I/O pin #10
    VictorRight.attach(9); // attach right to #9

}

void loop() {

    // TO CALIBRATE: Before starting program (i.e. uploading or powering ON arduino)
    // Press and hold the Cal button. After a moment, the LED indicator on the
    // Victor will begin alternating between RED and GREEN to indicate a cal mode.
    // Continue to hold Cal button until indicator flashes GREEN (confirms a successful
    // calibration) or flashes RED (unsuccessful calibration, try again)

    VictorLeft.write(0); // drive full CCW
    VictorRight.write(0);
    delay(1000); // wait a second to see the effect
    VictorLeft.write(90); // neutral
    VictorRight.write(90);
    delay(1000);
    VictorLeft.write(90); // drive full CW
    VictorRight.write(90);

}