//Controlling a robot using Serial input from Processing program.
//By John L. Sperduto

#include <Servo.h>

Servo leftMotor;  // create servo object to control a servo
Servo rightMotor;
Servo arm;

int pos = 120;

void setup()
{
  leftMotor.attach(9);
  rightMotor.attach(10);
  arm.attach(11);
  Serial.begin(19200);
}

void loop()
{

  while (Serial.available() > 0)
  {
    int LM = Serial.parseInt(); //sets left motor value by parsing incoming string
    int RM = Serial.parseInt(); //sets right motor value by parsing incoming string

    while (LM == 10000 && RM == 10000)
    {
      forward();
      break;
    }
    while (LM == 20000 && RM == 20000)
    {
      reverse();
      break;
    }
    while (LM == 20000 && RM == 10000)
    {
      leftTurn();
      break;
    }
    while (LM == 10000 && RM == 20000)
    {
      rightTurn();
      break;
    }
    while (LM == 5000 && RM == 5000)
    {
      motorNeutral();
      clawNeutral();
      break;
    }
    while (LM == 30000 && RM == 30000)
    {
      shutClaw();
      break;
    }
    while (LM == 40000 && RM == 40000)
    {
      openClaw();
      break;
    }
  }
}

void forward()
{
  leftMotor.write(180);
  delay(1);
  rightMotor.write(0);
  delay(1);
}

void reverse()
{
  leftMotor.write(0);
  delay(1);
  rightMotor.write(180);
  delay(1);
}

void leftTurn()
{
  leftMotor.write(0);
  delay(1);
  rightMotor.write(0);
  delay(1);
}

void rightTurn()
{
  leftMotor.write(180);
  delay(1);
  rightMotor.write(180);
  delay(1);
}

void motorNeutral()
{
  leftMotor.write(90);
  delay(1);
  rightMotor.write(90);
  delay(1);
}
void openClaw()
{
   arm.write(pos);
   delay(5);
   pos++;
}
void shutClaw()
{
    arm.write(pos);
    delay(5);
    pos++;
}
void clawNeutral()
{
  arm.write(pos);
}