Appendix

/* ########### ABOUT THE CODE #############
This is the modified, HARD CODED, script that ultimately performed in the demonstration.

It has several features:
  1.) An On/Off switch, for easy handling
  2.) LED lights, to determine the state of the robot. If it is on, the LED is on. If the robot is waiting, LEDs with flash
  3.) Variable speeds when the robot is at varying distance from the wall
  4.) Motor driver support.
  5.) A waving flag at the end. It only waves once per press of the Off/On button.

This code executes flawlessly and meets all requirements of the test, completing it in 1min 00sec 10ms.

The author of this code is not pleased with the HARD CODED nature of this script
He has thus included the equally valid script below this attached.

*/

// ########### CONSTANTS AND VARIABLES HERE #############

#include <Servo.h>

Servo madFlexer;

const int flex = 7;
int flexNumber = 0;

const int trig = 10;
const int echo = 11;

const int motorL1 = 3; // Opposite Terminals - Red and Black wires soldered in wrong sense
const int motorL2 = 6;
const int motorR1 = 5;
const int motorR2 = 9;

long duration;
float distance;
const int Error = 3;


bool onOff = false;
const int onSwitch = 13;
const int offSwitch = 12;

const int LED = 4;


long timeStart;


// ################# FUNCTIONS BELOW ####################

int measureDistance(int trig, int echo){
  digitalWrite(trig, LOW);
  delayMicroseconds(2); // clearing the trigger pin
  digitalWrite(trig, HIGH); // sending the pulse
  delayMicroseconds(10); // trig on high for at least 10 us
  digitalWrite(trig, LOW);
  return (pulseIn(echo, HIGH)/2) * 0.0343; // recieving and calulating distance
}

// To compensate for differnces in power output for each motor, motors L1 and R1 have been scaled to 90% power

void fullSpeed(){
  analogWrite(motorL1, 230);
  analogWrite(motorL2, 255);
  analogWrite(motorR1, 230);
  analogWrite(motorR2, 255);
}

void halfSpeed(){
  analogWrite(motorL1, 144);
  analogWrite(motorL2, 127);
  analogWrite(motorR1, 144);
  analogWrite(motorR2, 127);
}

void quarterSpeed(){
  analogWrite(motorL1, 64);
  analogWrite(motorL2, 64);
  analogWrite(motorR1, 64);
  analogWrite(motorR2, 64);
}

void fullStop(){
  analogWrite(motorL1, 0);
  analogWrite(motorL2, 0);
  analogWrite(motorR1, 0);
  analogWrite(motorR2, 0);
}

void madFlex(int flexState){    // This function waves the flag.
  if (flexState <2){
      for (int waves = 0; waves<= 3; waves += 1){
        madFlexer.write(110);
        delay(250);
        madFlexer.write(80);
        delay(250);
      }
      madFlexer.write(180);
      flexNumber += 1;
  }
  else{
    madFlexer.write(180);
  }
}

// ################# CODE BEGINS BELOW ###################

void setup() {
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT); // sets ultrasound pins to input and output
  pinMode(motorL1, OUTPUT);
  pinMode(motorL2, OUTPUT);
  pinMode(motorR1, OUTPUT);
  pinMode(motorR2, OUTPUT);
  pinMode(onSwitch, INPUT);
  pinMode(offSwitch, INPUT);
  madFlexer.attach(flex);
  pinMode(LED, OUTPUT);
  Serial.begin(9600);
}

void loop() {
  // As we only had 2 momentary switches, we had to make do with this code
  if (digitalRead(onSwitch) == HIGH){   // If the on switch is pressed, it turns on the LED and changes the onOff state
    onOff = true;
    digitalWrite(LED, HIGH);
    timeStart = millis();
  }
  if (digitalRead(offSwitch) == HIGH){  // Similarly, if the off switch is pressed, the opposite happens. It also resets the flex number
    onOff = false;
    digitalWrite(LED, LOW);
    flexNumber = 0;
  }

  distance = measureDistance(trig, echo);
  Serial.println("Distance:");
  Serial.println(distance);
  Serial.println(onOff);
  Serial.println("Time: ");
  Serial.println(millis() - timeStart);


  if (onOff) {    // If the on has been pushed, execute the following.

    if (not flexNumber){    // The variable flexNumber ensures this segment is only executed ONCE
      fullSpeed();
      flexNumber += 1;
      delay(3000);    // This makes the robot charge forward for 3 seconds, stopping somewhere in the middle of the course
      fullStop();
      for (int i = 0; i <= 112; i += 1){    // Each iteration takes 0.5 seconds. This is a 56 second countdown timer.
        digitalWrite(LED, LOW);
        delay(250);
        digitalWrite(LED, HIGH);
        delay(250);
        Serial.println("Waiting...");
        }
    }

    // This leaves the robot with 60 - 56 - 3 = 1 second to complete the remainder of the course.

    if (distance <= (15 + Error)){    // As the robot stops after rolling down a slope, a 10cm braking distance is factored
      fullStop();
      if (millis() - timeStart >= 59900) {
        madFlex(flexNumber);
      }
    }

    if (distance > (15 + Error)){     // Reduce speed as approaching wall
      halfSpeed();
    }

    if (distance >= (20 + Error)){
      fullSpeed();
    }
  }

  else{
    fullStop();
  }
}

####################    ORIGINAL SCRIPT BELOW    #######################


/* ########### ABOUT THE CODE #############
This piece of code is the original.

It has several features:
  1.) An On/Off switch, for easy handling
  2.) LED lights, to determine the state of the robot. If it is on, the LED is on. If the robot is waiting, LEDs with flash
  3.) Variable speeds when the robot is at varying distance from the wall
  4.) Motor driver support.
  5.) A waving flag at the end. It only waves once per press of the Off/On button.

This code executes flawlessly and meets all requirements of the test.

However, it was not clear to an outsider as to whether the robot has completed the course early or if it just waiting as it waits near the wall.
Hence a modified, HARD CODED, version of this script, included above, was designed to stop and wait in the middle of the course.

*/

// ########### CONSTANTS AND VARIABLES HERE #############

#include <Servo.h>

Servo madFlexer;

const int flex = 7;
int flexNumber = 0;

const int trig = 10; // trig on high for at least 10 us
const int echo = 11;

const int motorL1 = 3; // Opposite Terminals
const int motorL2 = 6;
const int motorR1 = 5;
const int motorR2 = 9;

long duration;
float distance;
const int Error = 2;


bool onOff = false;
const int onSwitch = 13;
const int offSwitch = 12;

const int LED = 4;

long timeStart;


// ################# FUNCTIONS BELOW ####################

int measureDistance(int trig, int echo){
  digitalWrite(trig, LOW);
  delayMicroseconds(2); // clearing the trigger pin
  digitalWrite(trig, HIGH); // sending the pulse
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  return (pulseIn(echo, HIGH)/2) * 0.0343; // recieving and calulating distance
}

// To compensate for differnces in power output for each motor, motors L1 and R1 have been scaled to 90% power

void fullSpeed(){
  analogWrite(motorL1, 230);
  analogWrite(motorL2, 255);
  analogWrite(motorR1, 230);
  analogWrite(motorR2, 255);
}

void halfSpeed(){
  analogWrite(motorL1, 144);
  analogWrite(motorL2, 127);
  analogWrite(motorR1, 144);
  analogWrite(motorR2, 127);
}

void quarterSpeed(){
  analogWrite(motorL1, 64);
  analogWrite(motorL2, 64);
  analogWrite(motorR1, 64);
  analogWrite(motorR2, 64);
}

void fullStop(){
  analogWrite(motorL1, 0);
  analogWrite(motorL2, 0);
  analogWrite(motorR1, 0);
  analogWrite(motorR2, 0);
}

void madFlex(int flexState){
  if (not flexState){
      for (int waves = 0; waves<= 3; waves += 1){
        madFlexer.write(80);
        delay(250);
        madFlexer.write(110);
        delay(250);
      }
      madFlexer.write(180);
      flexNumber += 1;
  }
  else{
    madFlexer.write(0);
  }
}

// ################# CODE BEGINS BELOW ###################

void setup() {
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT); // sets ultrasound pins to input and output
  pinMode(motorL1, OUTPUT);
  pinMode(motorL2, OUTPUT);
  pinMode(motorR1, OUTPUT);
  pinMode(motorR2, OUTPUT);
  pinMode(onSwitch, INPUT);
  pinMode(offSwitch, INPUT);
  pinMode(LED, OUTPUT);

  madFlexer.attach(7);

  Serial.begin(9600);
}

void loop() {

  if (digitalRead(onSwitch) == HIGH){
    onOff = true;
    digitalWrite(LED, HIGH);
    timeStart = millis();
  }
  if (digitalRead(offSwitch) == HIGH){
    onOff = false;
    digitalWrite(LED, LOW);
    flexNumber = 0;
    madFlexer.write(180);
  }

  distance = measureDistance(trig, echo);

  Serial.println("Distance: ");
  Serial.println(distance);
  Serial.println(onOff);
  Serial.println("Time: ");
  Serial.println(millis() - timeStart);


  if (onOff) {
    if (distance <= (5 + Error)){
      fullStop();
      madFlex(flexNumber);    // MAD FLEX HERE
    }

    if (distance > (5 + Error)){    // It travels at half speed if it is 5< x <15 cm from the wall
      halfSpeed();
    }

    if (distance <= (10+ Error) and (millis() - timeStart) <= 58000){ // This leaves 2 secs to clear 5cm
      fullStop();
      digitalWrite(LED, LOW);
      delay(250);
      digitalWrite(LED, HIGH);
      delay(250);
      Serial.println("Waiting...");
    }

    if (distance >= (15 + Error)){
      fullSpeed();
    }
  }

  else{
    fullStop();
  }
}