 	}
 	else if(read_sensors)
 	{
-		Serial.println(robot.getUltrasonicDistance(FRONT, 1));
+		//Not used yet. Must test it
+		//Serial.println(robot.getUltrasonicDistance(FRONT, 1));
 	}
 }
 

#define R_MOTOR_2		6
#define R_MOTOR_MAX_PWM	255

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

#define VCC				12

#define COMMAND_DELAY	500

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

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



	Serial.begin(BAUD_RATE);

	robot.setUltrasonic(US_TRIG, US_ECHO, FRONT);	//Set up Ultrasonic sensor

}

void loop()
{

	bool read_sensors = true;

	if(Serial.available() > 0)
