whitelineproc

gistfile1.txt

package procedure;

import model.Color;
import model.Direction;
import model.DrawRequest;
import model.DrawType;
import model.Position;
import model.RobotActivity;
import model.RobotState;
import model.Rotation;
import model.WhiteLineRequest;
import controller.InputData;
import robot.IControllable;

public class WhiteLineProcedure extends Procedure<WhiteLineRequest>{

private boolean whiteLineDetected;
private boolean whiteLineOnRigthSideReached;
private Position rightLinePose;
private DrawRequest savedDrawRequest;

public WhiteLineProcedure(IControllable robot, WhiteLineRequest request) {
super(robot, request);
setWhiteLineDetected(false);
setWhiteLineOnRigthSideReached(false);
this.rightLinePose = null;
this.savedDrawRequest = null;
}

private boolean isWhiteLineDetected() {
return whiteLineDetected;
}

private void setWhiteLineDetected(boolean whiteLineDetected) {
this.whiteLineDetected = whiteLineDetected;
}

private boolean isWhiteLineOnRigthSideReached() {
return whiteLineOnRigthSideReached;
}

private void setWhiteLineOnRigthSideReached(boolean whiteLineOnRigthSideReached) {
this.whiteLineOnRigthSideReached = whiteLineOnRigthSideReached;
}

private Position getRightLinePose() {
return rightLinePose;
}

private void setRightLinePose(Position rightLinePose) {
this.rightLinePose = rightLinePose;
}

@Override
protected IProcedure evaluateSecure(InputData inputData,
RobotState robotState) {
if((robotState.getColor().equals(Color.BLACK) || robotState.getColor().equals(Color.BROWN)) && isWhiteLineDetected() == false && robotState.getActivity().equals(RobotActivity.IDLE)){
getRobot().drive(getRequest().getShortDriveDistance(), Direction.FORWARD);
System.out.println("Diving short forward");
return null;
}
else if(robotState.getColor().equals(Color.WHITE) && isWhiteLineDetected() == false){
savedDrawRequest = new DrawRequest(robotState.getLightSensorPose(), DrawType.SCANNED_WHITE_SPOT);
setWhiteLineDetected(true);
getRobot().stop();
getRobot().drive(getRequest().getLongDriveDistance(), Direction.FORWARD);
try {
Thread.sleep(1000);
} catch (InterruptedException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
System.out.println("driving long forward");
}
else if((robotState.getColor().equals(Color.BLACK) || robotState.getColor().equals(Color.BROWN)) && isWhiteLineDetected() == true && isWhiteLineOnRigthSideReached() == false && robotState.getActivity().equals(RobotActivity.IDLE)) {
getRobot().rotate(getRequest().getShortTurnAngle(), Rotation.CLOCKWISE);
System.out.println("turning short clockwise");
}
else if(robotState.getColor().equals(Color.WHITE) && isWhiteLineDetected() == true && isWhiteLineOnRigthSideReached() == false) {
this.savedDrawRequest = new DrawRequest(robotState.getLightSensorPose(), DrawType.SCANNED_WHITE_SPOT);
getRobot().stop();
setWhiteLineOnRigthSideReached(true);
setRightLinePose(robotState.getPose());
getRobot().rotate(getRequest().getLongTurnAngle(), Rotation.COUNTERCLOCKWISE);
System.out.println("turning long counterclockwise");
}
else if((robotState.getColor().equals(Color.BLACK) || robotState.getColor().equals(Color.BROWN)) && isWhiteLineDetected() == true && isWhiteLineOnRigthSideReached() == true && robotState.getActivity().equals(RobotActivity.IDLE)) {
getRobot().rotate(getRequest().getShortTurnAngle(), Rotation.COUNTERCLOCKWISE);
System.out.println("turning short counterclockwise");
}
else if(robotState.getColor().equals(Color.WHITE) && isWhiteLineDetected() == true && isWhiteLineOnRigthSideReached() == true) {
this.savedDrawRequest = new DrawRequest(robotState.getLightSensorPose(), DrawType.SCANNED_WHITE_SPOT);
getRobot().stop();
double angleToRotate = Math.abs(getRightLinePose().getAngleDifference(robotState.getPose()))/2d;
getRobot().rotate(angleToRotate, Rotation.CLOCKWISE);
System.out.println("turning into position");
terminate();
return null;
}
return null;
}

@Override
public DrawRequest getDrawRequest() {
DrawRequest request = this.savedDrawRequest;
this.savedDrawRequest = null;
return request;
}

@Override
public void terminate() {
super.terminate();
getRobot().stop();
}

}