 char last_command = 'S';
 
 bool autonomous = false;		//Stores if the robot is on autonomous mode
-
-////Ultrasonic test////
-
-#define US_VCC		A2
-#define US_TRIG		A4
-#define US_ECHO		A3
-#define US_GND		A5
-
-////////////////////////
 
 //Functions
 void getCommand();


	Serial.begin(BAUD_RATE);

	////////////////Ultrasonic test////////////////

	pinMode(US_VCC, OUTPUT);
	pinMode(US_GND, OUTPUT);
	digitalWrite(US_VCC, HIGH);
	digitalWrite(US_GND, LOW);

	robot.setUltrasonic(US_TRIG, US_ECHO, FRONT);

	///////////////////////////////////////////////
}

void loop()
{
	bool read_sensors = true;

	if(Serial.available() > 0)

#define R_MOTOR_2		6
#define R_MOTOR_MAX_PWM	255

#define L_ENABLE		8
#define L_MOTOR_1		9
#define L_MOTOR_2		10
#define L_MOTOR_MAX_PWM	255

#define COMMAND_DELAY	500

#define US_TRIG			13		//Ultrasonic Trigger
#define US_ECHO			12		//Ultrasonic Echo

Robot robot(R_ENABLE,R_MOTOR_1,R_MOTOR_2,L_ENABLE,L_MOTOR_1,L_MOTOR_2);

unsigned long timer = 0;		//Stores the time when the last command was received

