@Override public void doControl() { long startTime = System.nanoTime(); sensorReader.read(); humanoidReferenceFrames.updateFrames(); if (firstControlTick) { stateEstimator.initialize(); automatedDiagnosticAnalysisController.initialize(); firstControlTick = false; } else { stateEstimator.doControl(); automatedDiagnosticAnalysisController.doControl(); } outputWriter.writeAfterController(0); long endTime = System.nanoTime(); controllerTime.set(TimeTools.nanoSecondstoSeconds(endTime - startTime)); averageControllerTime.update(); }
@Override public void doControl() { long startTime = System.nanoTime(); sensorReader.read(); humanoidReferenceFrames.updateFrames(); if (firstControlTick) { stateEstimator.initialize(); automatedDiagnosticAnalysisController.initialize(); firstControlTick = false; } else { stateEstimator.doControl(); automatedDiagnosticAnalysisController.doControl(); } outputWriter.writeAfterController(0); long endTime = System.nanoTime(); controllerTime.set(TimeTools.nanoSecondstoSeconds(endTime - startTime)); averageControllerTime.update(); }
@Override public void doControl() { long startTime = System.nanoTime(); lowLevelOutputWriter.writeBefore(startTime); sensorReader.read(); humanoidReferenceFrames.updateFrames(); if (firstControlTick) { stateEstimator.initialize(); forceSensorStateUpdater.initialize(); automatedDiagnosticAnalysisController.initialize(); firstControlTick = false; } else { stateEstimator.doControl(); forceSensorStateUpdater.updateForceSensorState(); automatedDiagnosticAnalysisController.doControl(); } lowLevelOutputWriter.writeAfter(); long endTime = System.nanoTime(); controllerTime.set(Conversions.nanosecondsToSeconds(endTime - startTime)); averageControllerTime.update(); }
diagnosticController.doControl(); sensorReader.writeCommandsToRobot();