#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"

// Orange	- Vertical M2+
// White/orange - Vertical M2-
// Green        - Left M3+
// White/green  - Left M3-
// Blue         - Right M1+
// White/blue   - Right M1-
// Brown        - RS485 B
// Brown/white  - RS485 A

// Motor shield object at default address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();

// Motor objects
Adafruit_DCMotor *leftMotor = AFMS.getMotor(3);
Adafruit_DCMotor *rightMotor = AFMS.getMotor(1);
Adafruit_DCMotor *vertMotor = AFMS.getMotor(2);

void setup() {
	Serial.begin(115200);

	AFMS.begin();	// Start at default frequency
}

int
parseIn(char dir) {
	int tmp;

	while (Serial.read() != dir)
		;
	while (Serial.read() != ':')
		;
	tmp = Serial.parseInt();

	return tmp;
}

void loop() {
	int tmp;

	Serial.println("Parsing L...");
	tmp = parseIn('L');
	if (tmp > 0)
		leftMotor->run(FORWARD);
	else
		leftMotor->run(BACKWARD);
	leftMotor->setSpeed(abs(tmp));

	Serial.println("Parsing R...");
	tmp = parseIn('R');
	if (tmp > 0)
		rightMotor->run(FORWARD);
	else
		rightMotor->run(BACKWARD);
	rightMotor->setSpeed(abs(tmp));

	Serial.println("Parsing V...");
	tmp = parseIn('V');
	if (tmp > 0)
		vertMotor->run(FORWARD);
	else
		vertMotor->run(BACKWARD);
	vertMotor->setSpeed(abs(tmp));

	Serial.println("Done");
}

/*
 * Local variables:
 * mode: c++
 * End:
 */