Commands Sketch

This sketch waits for data to arrive on the serial port. Once data arrives it is read in as a stream until either a newline or a carriage return character is received. The data read in is then checked to see if it matches a command. If it does the command is executed, if not the data is ignored. The sketch then waits for more data to arrive on the serial port.

This makes the robot operate manually based upon the commands it receives.

/*
 * Reads commands from the serial ports.
 *
 * Commands are;
 *    "led_on" turns the LED on.
 *    "led_off" turns the LED off.
 *    "look_left" turn ping sensor 90 degress left
 *    "look_right" turns ping sensor 90 degree right
 *    "look_forward" turn ping sensor forward.
 *    "forward" drives forward continously
 *    "reverse" drives in reverse for 1 second.
 *    "turn_left"
 *    "turn_right"
 *    "halt"
 * All other text is ignored.
 */
 
#include 
Servo servo; // Define our Servo

// Input/Output Pin configuration.
int ledPin     = 13;
int rightL3    = 12;
int rightL4    = 11;
int rightSpeed = 10; // pwm (potential problem when using servo library!)
int leftL1     =  8;
int leftL2     =  7;
int leftSpeed  =  6; // pwm
int pingPin    =  4;
int servoPin   =  3; // pwm
int echoPin    =  2;

// Speed.
int spd = 255;

// 90 Degree turn time.
int tt = 1400;

void setup()
{ 
  // Configure LED pin.
  pinMode(ledPin, OUTPUT);
  
  // Configure the servo pin.
  servo.attach(servoPin);
  
  // Initialize serial communications.
  Serial.begin(9600);
}

String getCommand()
{
  boolean done = false;
  String command = "";
  char character;

  while (!done)
  {    
    if (Serial.available())
    {
      character = Serial.read();
      if ((character == '\n') || (character == '\r'))
      {
        done = true;
      }
      else
      {  
        command.concat(character);
      }
    }
  }

  return command;
}

void loop()
{
  String command = getCommand();
  
  // debug.
  if (command != "")
  {
    Serial.println(command);
  }
  
  if (command == "led_on")
  {
    digitalWrite(ledPin, HIGH);
  }
  
  if (command == "led_off")
  {
    digitalWrite(ledPin, LOW);
  }
  
  if (command == "look_left")
  {
    lookLeft(90);
  }
  
  if (command == "look_right")
  {
    lookRight(90);
  }
  
  if (command == "look_forward")
  {
    lookForward();
  }
  
  if (command == "forward")
  {
    forward(255);
  }
  
  if (command == "reverse")
  {
    reverse(255, 1000);
  }
  
  if (command == "turn_left")
  {
    turnLeft(255, tt);
  }
  
  if (command == "turn_right")
  {
    turnRight(255, tt);
  }
  
  if (command == "halt")
  {
    halt();
  }
  
}

long lookLeft(int angle)
{
  int curAngle = servo.read();
  Serial.print("Current angle: ");
  Serial.print(curAngle);
  Serial.println();
  
  int newAngle = (90 + angle);
  Serial.print("New angle: ");
  Serial.print(newAngle);
  Serial.println();
  
  int degDiff = abs(curAngle - newAngle);
  Serial.print("Degrees to move: ");
  Serial.print(degDiff);
  Serial.println();
  
  servo.write(newAngle);  // Turn servo left
  delay(500 / 90 * degDiff); // Delay to ensure servo has time to turn
}

long lookRight(int angle)
{
  int curAngle = servo.read();
  Serial.print("Current angle: ");
  Serial.print(curAngle);
  Serial.println();
  
  int newAngle = (90 - angle);
  Serial.print("New angle: ");
  Serial.print(newAngle);
  Serial.println();
  
  int degDiff = abs(curAngle - newAngle);
  Serial.print("Degrees to move: ");
  Serial.print(degDiff);
  Serial.println();
  
  servo.write(newAngle);  // Turn Servo right
  delay(500 / 90 * degDiff); // Delay to ensure servo has time to turn
}

long lookForward()
{
  int curAngle = servo.read();
  Serial.print("Current angle: ");
  Serial.print(curAngle);
  Serial.println();
  
  int newAngle = 90;
  Serial.print("New angle: ");
  Serial.print(newAngle);
  Serial.println();
  
  int degDiff = abs(curAngle - newAngle);
  Serial.print("Degrees to move: ");
  Serial.print(degDiff);
  Serial.println();
  
  servo.write(newAngle); // Turn Servo to Center 90 degrees
  delay(500 / 90 * degDiff); // Delay to ensure servo has time to turn
  
}

void forward(int velocity)
{
  // Left motor forward at specified speed.
  digitalWrite(leftL1, LOW);
  digitalWrite(leftL2, HIGH);
  analogWrite(leftSpeed, velocity);
  
  // Right motor forward at specified speed.
  digitalWrite(rightL3, LOW);
  digitalWrite(rightL4, HIGH);
  analogWrite(rightSpeed, velocity);
}

void reverse(int velocity, int time)
{
  // Left motor reverse at specified speed.
  digitalWrite(leftL1, HIGH);
  digitalWrite(leftL2, LOW);
  analogWrite(leftSpeed, velocity);
  
  // Right motor reverse at specified speed.
  digitalWrite(rightL3, HIGH);
  digitalWrite(rightL4, LOW);
  analogWrite(rightSpeed, velocity);
  
  // Delay for specified time.
  delay(time);
  
  // Stop both motors.
  halt;  
}

void turnLeft(int velocity, int time)
{
  Serial.println("Turning Left...");
  
  // Left motor reverse at specified speed.
  digitalWrite(leftL1, HIGH);
  digitalWrite(leftL2, LOW);
  analogWrite(leftSpeed, velocity);
  
  // Right motor forward at specified speed.
  digitalWrite(rightL3, LOW);
  digitalWrite(rightL4, HIGH);
  analogWrite(rightSpeed, velocity);
  
  // Delay for specified time.
  delay(time);
  
  // Stop both motors.
  halt;
}

void turnRight(int velocity, int time)
{
  Serial.println("Turning right...");
  
  // Left motor forward at specified speed.
  digitalWrite(leftL1, LOW);
  digitalWrite(leftL2, HIGH);
  analogWrite(leftSpeed, velocity);
  
  // Right motor reverse at specified speed.
  digitalWrite(rightL3, HIGH);
  digitalWrite(rightL4, LOW);
  analogWrite(rightSpeed, velocity);
  
  // Delay for specified time.
  delay(time);
  
  // Stop both motors.
  halt;
}

void halt()
{
  // Stop both motors.
  digitalWrite(leftL1, LOW);
  digitalWrite(leftL2, LOW);
  digitalWrite(rightL3, LOW);
  digitalWrite(rightL4, LOW);
  analogWrite(leftSpeed, 255);  // Breaking
  analogWrite(rightSpeed, 255);    // on both wheels.
  
  // pint reading to display
  Serial.println("Stopped motors");
}