/** * 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(); }