 
 	Serial.begin(BAUD_RATE);
 
-	////////////////Ultrasonic test////////////////
+	robot.setUltrasonic(US_TRIG, US_ECHO, FRONT);	//Set up Ultrasonic sensor
 
-	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)

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();

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

