// Behaviors
@Override
public void startBehavior(CIBehavior b) {
stopActiveBehavior();
activeBehavior = b;
behaviorTimestep = 0;
activeBehavior.start();
String str = "Starting CIBehavior " + activeBehavior.toString();
log(LogCodex.encodeLog(LogType.MESSAGE, str));
}
@Override
public void stopActiveBehavior() {
if (activeBehavior != null) {
activeBehavior.cleanUp();
String str = "Stopping CIBehavior " + activeBehavior.toString();
log(LogCodex.encodeLog(LogType.MESSAGE, str));
activeBehavior = null;
ioManager.setMotorSpeeds(leftSpeed, rightSpeed);
}
}
@Override
public void run() {
while (run) {
updateSensors();
long lastCycleTime = System.currentTimeMillis();
CIBehavior current = activeBehavior;
try {
if (current != null) {
current.step(behaviorTimestep++);
for (CIBehavior b : alwaysActiveBehaviors) b.step(timestep);
if (current.getTerminateBehavior()) {
stopActiveBehavior();
}
}
} catch (Exception e) {
e.printStackTrace();
}
ioManager.setMotorSpeeds(leftSpeed, rightSpeed);
if (broadcastHandler != null) broadcastHandler.update(timestep);
long timeToSleep = CYCLE_TIME - (System.currentTimeMillis() - lastCycleTime);
if (timeToSleep > 0) {
try {
Thread.sleep(timeToSleep);
} catch (InterruptedException e) {
}
}
timestep++;
}
}
@Override
public void reset() {
ioManager.shutdownMotors();
if (activeBehavior != null) {
activeBehavior.cleanUp();
activeBehavior = null;
ioManager.setMotorSpeeds(leftSpeed, rightSpeed);
}
try {
// make sure that the current control step is processed
Thread.sleep(100);
} catch (InterruptedException e) {
}
}