 /********************************************************************
   Code to allow the MRK-1 sumo robot to be operated using simple Logo commands
+  
 
   This code was built off of the Lesson 8, Competitive Sumo Robot
   http://github.com/SumoRobotLeague/MRK-1/

/************************/
// LED pin
#define led 7

// IR Sensor pins
#define leftSensor  A1


	// Motor setup
	motor.setupRight(rightMotorSpeed, rightMotorDirection);
  Serial.println("Setup Right Motor");
	motor.setupLeft(leftMotorSpeed, leftMotorDirection);
  Serial.println("Setup Left Motor");
	Serial.println("Robot powered up. Awaiting Logo commands via serial communication");
}

void loop() {
   serialEvent(); //call the function
  // print the string when a newline arrives:
  if (stringComplete) {
    
    //Serial.println(inputString);
    executeCommand(inputString);
    
    // clear the string:
    inputString = "";
    stringComplete = false;
  }
}

//captures the command sent via serial
void serialEvent() {
  //Serial.println("Starting aggregation of serial characters");
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the inputString:
   
    // if the incoming character is a newline, set a flag
    // so the main loop can do something about it:
    if (inChar == '\n' || inChar == ' ') {
      stringComplete = true;
    } else {
      inputString += inChar;
      Serial.print("adding character: ");
      Serial.println(inChar);
    }
  }
}

//TO DO - add the ability to parse out numbers following commands
//this version simply matches the command with the function and execute it
void executeCommand(String command){
  //command.toLowerCase();
  Serial.println(command);
  if (command == "forward" || command == "fd"){
    FD(moveTime);
    Serial.println("Moving forward");
  } else {
    if (command == "backward" || command == "bd"){
      BD(moveTime);
      Serial.println("Moving backward");
    } else {
      if (command == "right" || command == "rt"){
        RT(moveTime);
        Serial.println("Right Turn");
      } else {
        if (command == "left" || command == "lt"){
          LT(moveTime);
          Serial.println("Left Turn");
        } else {
          if (command == "toot"){
            toot();
            Serial.println("Toot");
          } else {
            if (command == "light_on"){
              light_on();
              Serial.println("Lights On");
            } else {
              if (command == "light_off"){
                light_off();
                Serial.println("Lights Off");
              } else {
                Serial.print("Unable to execute:_");
                Serial.println(command);
              }
            }
          }
        }
      }
    }
  }
  
}

/***************************************************************/
/**      Commands from Logo's TORTIS project,                 **/
