/**
* Create a message provider for all the possible message provider classes like the inputs,
* outputs, system information queries
*/
private void initMessageProviders() {
System.out.println("Creating Message Providers:");
// @miguelduarte42 SystemInfoMessageProvider takes ~20 seconds and it
// is not currently used, so I just removed it
// messageProviders.add(new SystemInfoMessageProvider());
// System.out.println("\tSystemInfoMessageProvider");
messageProviders.add(new SystemStatusMessageProvider(this));
System.out.println("\tSystemStatusMessageProvider");
for (ControllerInput i : ioManager.getInputs()) {
if (i instanceof MessageProvider) {
messageProviders.add((MessageProvider) i);
System.out.println("\t" + i.getClass().getSimpleName());
}
}
for (ControllerOutput o : ioManager.getOutputs()) {
if (o instanceof MessageProvider) {
messageProviders.add((MessageProvider) o);
System.out.println("\t" + o.getClass().getSimpleName());
}
}
messageProviders.add(new EntityMessageProvider(this));
System.out.println("\tEntityMessageProvider");
messageProviders.add(new EntitiesMessageProvider(this));
System.out.println("\tEntitiesMessageProvider");
messageProviders.add(new BehaviorMessageProvider(this));
System.out.println("\tBehaviorMessageProvider");
messageProviders.add(new NeuralActivationsMessageProvider(this));
System.out.println("\tNeuralActivationsMessageProvider");
messageProviders.add(new LogMessageProvider(this));
System.out.println("\tLogMessageProvider");
}
@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) {
}
}
private double updateCompass() {
try {
double orientation = ioManager.getCompassModule().getHeadingInDegrees();
return (orientation % 360.0);
} catch (Exception e) {
return -1;
}
}
@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 shutdown() {
log(LogCodex.encodeLog(LogType.MESSAGE, "Shutting down Controller..."));
run = false;
if (logger != null) logger.stopLogging();
ioManager.shutdown();
System.out.println("# Finished Controller cleanup!");
}
// Setters
@Override
public void setLed(int index, LedState state) {
if (ioManager.getDebugLeds() != null) {
if (index >= 0 && index < ioManager.getDebugLeds().getNumberOfOutputs()) {
switch (state) {
case ON:
ioManager.getDebugLeds().removeBlinkLed(index);
ioManager.getDebugLeds().setValue(index, 1);
break;
case OFF:
ioManager.getDebugLeds().removeBlinkLed(index);
ioManager.getDebugLeds().setValue(index, 0);
break;
case BLINKING:
ioManager.getDebugLeds().addBlinkLed(index);
break;
}
} else {
if (logger != null)
logger.logError(
"Invalid led index: "
+ index
+ ", must be >= 0 and < "
+ ioManager.getDebugLeds().getNumberOfOutputs());
}
}
}
private void configureArguments(CIArguments args) {
if (args.getArgumentIsDefined("dronetype"))
setProperty("dronetype", args.getArgumentAsString("dronetype"));
if (args.getFlagIsTrue("filelogger")) this.startLogger();
if (args.getArgumentIsDefined("compassoffset") && ioManager.getCompassModule() != null)
ioManager.getCompassModule().setOffset(args.getArgumentAsInt("compassoffset"));
if (args.getArgumentIsDefined("changewaypoint"))
setProperty("changewaypoint", args.getArgumentAsString("changewaypoint"));
if (args.getArgumentIsDefined("avoiddrones"))
setProperty("avoiddrones", args.getArgumentAsString("avoiddrones"));
if (args.getArgumentIsDefined("avoidobstacles"))
setProperty("avoidobstacles", args.getArgumentAsString("avoidobstacles"));
if (args.getArgumentIsDefined("kalmanfilter"))
setProperty("kalmanfilter", args.getArgumentAsString("kalmanfilter"));
}
private LatLon updateGPS() {
try {
GPSData gpsData = ioManager.getGpsModule().getReadings();
currentGPSOrientation = gpsData.getOrientation();
if (gpsData.getLatitudeDecimal() == 0 || gpsData.getLongitudeDecimal() == 0) return null;
else return new LatLon(gpsData.getLatitudeDecimal(), gpsData.getLongitudeDecimal());
} catch (Exception e) {
currentOrientation = -1;
return null;
}
}
@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++;
}
}
// Init's
private void initIO(CIArguments args) {
ioManager = new IOManager(this, args);
initMessages += ioManager.getInitMessages();
}
