/**
* Copyright (C) BFH www.bfh.ch 2011
* Code written by: Patrick Dobler, Marc Folly
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
package ch.bfh.ti.kybernetik.engine.controller.roboter;
import java.util.ArrayList;
import java.util.Collection;
import java.util.Collections;
import java.util.LinkedList;
import java.util.List;
import javax.vecmath.Point2d;
import javax.vecmath.Vector2d;
import org.apache.commons.collections.BufferUtils;
import org.apache.commons.collections.buffer.CircularFifoBuffer;
import ch.bfh.ti.kybernetik.engine.controller.Simulator;
import ch.bfh.ti.kybernetik.engine.model.Motor;
import ch.bfh.ti.kybernetik.engine.model.Roboter;
import ch.bfh.ti.kybernetik.engine.model.RoboterMove;
import ch.bfh.ti.kybernetik.lego.ki.RoboterKI;
/**
* The {@link Simulator} default {@link RoboterController} implementation
*
*/
class DefaultRoboterControllerImpl implements RoboterController {
private static final int ROBOTER_MOVE_HISTORY_SIZE = 500;
private static volatile double DELTA_T = 0.004;
private final RoboterConstruction roboterConstruction;
private final Roboter roboter;
private RoboterKI roboterKI;
@SuppressWarnings("unchecked")
private Collection<RoboterMove> roboterMoveStates = BufferUtils.synchronizedBuffer(new CircularFifoBuffer(ROBOTER_MOVE_HISTORY_SIZE));
public DefaultRoboterControllerImpl(Roboter roboter, RoboterKI roboterKI, RoboterConstruction roboterConstruction) {
this.roboter = roboter;
this.roboterConstruction = roboterConstruction;
this.roboterKI = roboterKI;
}
private RoboterMove calculateNextRoboterMove(RoboterConstruction roboterConstruction) {
Point2d leftSpeedPoint = getRoboterMotorSpeedPoint(roboter.getLeftMotor());
Point2d rightSpeedPoint = getRoboterMotorSpeedPoint(roboter.getRightMotor());
double newRoboterPointX = (leftSpeedPoint.getX() + rightSpeedPoint.getX()) / 2;
double newRoboterPointY = (leftSpeedPoint.getY() + rightSpeedPoint.getY()) / 2;
Point2d newRoboterPoint = new Point2d(newRoboterPointX, newRoboterPointY);
Vector2d topVektor = new Vector2d(rightSpeedPoint.getX() - leftSpeedPoint.getX(), rightSpeedPoint.getY() - leftSpeedPoint.getY());
topVektor.normalize();
Vector2d newDirectionVector = new Vector2d(topVektor.getY(), -topVektor.getX());
RoboterMove rmove = new RoboterMove(roboter, newRoboterPoint, newDirectionVector);
return rmove;
}
private Point2d getRoboterMotorSpeedPoint(Motor motor) {
Point2d motorPoint = roboterConstruction.getRoboterElementPoint(motor);
double x = motorPoint.getX() + DELTA_T * motor.getVelocity() * roboter.getDirection().getX();
double y = motorPoint.getY() + DELTA_T * motor.getVelocity() * roboter.getDirection().getY();
return new Point2d(x, y);
}
private RoboterMove calculateNextRoboterMove() {
return calculateNextRoboterMove(roboterConstruction);
}
@Override
public RoboterMove calculateNewRoboterPosition(double leftLightSensor, double rightLightSensor) {
// START TODO
roboterKI.setLightMin(0);
roboterKI.setLightMax((int) roboter.getLeftLightSensor().getMaxLightIntensity());
roboterKI.calculateNewRoboterMotorSpeed((int) leftLightSensor, (int) rightLightSensor);
double newLeftMotorSpeed = roboterKI.getMotorSpeedLeft();
double newRightMotorSpeed = roboterKI.getMotorSpeedRight();
roboter.getLeftMotor().setVelocity(newLeftMotorSpeed);
roboter.getRightMotor().setVelocity(newRightMotorSpeed);
// END TODO
final RoboterMove currentRoboterMoveState = calculateNextRoboterMove();
this.roboterMoveStates.add(currentRoboterMoveState);
roboter.setX(currentRoboterMoveState.getNewRoboterPoint().getX());
roboter.setY(currentRoboterMoveState.getNewRoboterPoint().getY());
roboter.setDirection(currentRoboterMoveState.getNewDirectionVector());
return currentRoboterMoveState;
}
@Override
public List<RoboterMove> calculateNextEstimatedRoboterMoves(int count) {
Roboter iterationRoboter = new Roboter(this.roboter);
List<RoboterMove> estimatedRoboterMoveStates = new LinkedList<RoboterMove>();
for (int i = 0; i < count; i++) {
DefaultRoboterConstructionImpl constructionImpl = new DefaultRoboterConstructionImpl(iterationRoboter);
RoboterMove roboterMoveState = calculateNextRoboterMove(constructionImpl);
estimatedRoboterMoveStates.add(roboterMoveState);
// Set new roboter data
iterationRoboter.setX(roboterMoveState.getNewRoboterPoint().getX());
iterationRoboter.setY(roboterMoveState.getNewRoboterPoint().getY());
iterationRoboter.setDirection(roboterMoveState.getNewDirectionVector());
}
return estimatedRoboterMoveStates;
}
@Override
public Roboter getRoboter() {
return roboter;
}
@Override
public List<RoboterMove> getRoboterMoveHistory() {
return Collections.unmodifiableList(new ArrayList<RoboterMove>(roboterMoveStates));
}
@Override
public RoboterConstruction getRoboterConstruction() {
return roboterConstruction;
}
@Override
public void setDeltaT(double deltaT) {
DELTA_T = deltaT;
}
@Override
public double getDeltaT() {
return DELTA_T;
}
@Override
public RoboterKI getRoboterKI() {
return roboterKI;
}
@Override
public void setRoboterKI(RoboterKI roboterKI) {
this.roboterKI = roboterKI;
}
}