// StandardRobot.java
//
// A Java representation of the NXT hardware
//
// Simon Parsons
// 17th October 2013
//  
// Based on Davide Grossi's StandardRobot code.

public class StandardRobot{
    // A data member for each element of the hardware we want to control
    private TouchSensor leftBump, rightBump;
    private UltrasonicSensor uSense;
    private ColorHTSensor cSense;
    private NXTRegulatedMotor leftMotor, rightMotor;
	
    public StandardRobot(){
	// Setup sensors
	leftBump = new TouchSensor(SensorPort.S2);
	rightBump = new TouchSensor(SensorPort.S1);
	uSense = new UltrasonicSensor(SensorPort.S3);
	cSense = new ColorHTSensor(SensorPort.S4);
	// Setup motors
	leftMotor = Motor.C;
	rightMotor = Motor.B;
    }
    
    // Accessor functions for the hardware elements
    public boolean isLeftBumpPressed(){
	return leftBump.isPressed();
    }
    
    public boolean isRightBumpPressed(){
	return rightBump.isPressed();
    }
    
    public float getUSenseRange(){
	return uSense.getRange();
    }
    
    public int getCSenseColor(){
		return cSense.getColorID();
    }
    
    // We can go straight ahead
    public void startMotors(){
	leftMotor.forward();
	rightMotor.forward();
    }
	
    // We can go straight back
    public void reverseMotors(){
	leftMotor.backward();
	rightMotor.backward();
	}
    
    // We can turn in place, clockwise or anticlockwise
    public void turnMotors(boolean clockwise){
	if(clockwise){
	    leftMotor.forward();
	    rightMotor.backward();
	}
	else{
	    leftMotor.backward();
	    rightMotor.forward();	    
	}
    }

    // We can turn off the motors
    public void stopMotors(){
	leftMotor.stop();
	rightMotor.stop();
    }
}