public void controlState() {
final Mat image = camera.getLastFrame();
final ComputerVisionSummary summaryOfImage = ComputerVisionSummary.produceFullSummary(image);
final double wallThresholdDistance = 10;
// if wall is close, and we are not currently avoiding walls, then avoid walls
if ((summaryOfImage.getDistanceToBlueWall() <= wallThresholdDistance)
&& !(currentStateController.getStateMachineType() == StateMachineType.AVOID_WALLS
&& !currentStateController.isDone())) {
avoidWalls();
}
// else if see ball, and not currently collecting one, then collect ball
else if ((summaryOfImage.isGreenBall() || summaryOfImage.isRedBall())
&& !(currentStateController.getStateMachineType() == StateMachineType.COLLECT_GROUND_BALLS
&& !currentStateController.isDone())) {
collectGroundBalls();
}
// if see reactor and have green balls then score
// if see interface wall and have red balls then score over wall
// if see energy silo then collect ball
}
private void avoidWalls() {
currentStateController.stop();
final AvoidWallStateController avoidWallController = new AvoidWallStateController(robotModel);
currentStateController = avoidWallController;
Thread avoidWallThread =
new Thread(
new Runnable() {
public void run() {
avoidWallController.controlState();
}
});
avoidWallThread.start();
}
private void collectGroundBalls() {
currentStateController.stop();
final BallCollectionStateController ballCollectionController =
new BallCollectionStateController(robotModel, camera);
currentStateController = ballCollectionController;
Thread ballCollectionThread =
new Thread(
new Runnable() {
public void run() {
while (!ballCollectionController.isDone()) {
ballCollectionController.controlState();
}
}
});
ballCollectionThread.start();
}
