diff --git a/src/main/java/xbot/common/command/BaseMaintainerCommand.java b/src/main/java/xbot/common/command/BaseMaintainerCommand.java index f6b83bcb..baf0b52b 100644 --- a/src/main/java/xbot/common/command/BaseMaintainerCommand.java +++ b/src/main/java/xbot/common/command/BaseMaintainerCommand.java @@ -6,6 +6,7 @@ import xbot.common.logic.TimeStableValidator; import xbot.common.properties.BooleanProperty; import xbot.common.properties.DoubleProperty; +import xbot.common.properties.Property; import xbot.common.properties.PropertyFactory; import xbot.common.properties.StringProperty; @@ -32,8 +33,10 @@ public BaseMaintainerCommand(BaseSetpointSubsystem subsystemToMaintain, Property pf.setPrefix(this); errorToleranceProp = pf.createPersistentProperty("Error Tolerance", defaultErrorTolerance); - errorWithinToleranceProp = pf.createEphemeralProperty("Error Within Tolerance", false); errorTimeStableWindowProp = pf.createPersistentProperty("Error Time Stable Window", defaultTimeStableWindow); + + pf.setDefaultLevel(Property.PropertyLevel.Debug); + errorWithinToleranceProp = pf.createEphemeralProperty("Error Within Tolerance", false); errorIsTimeStableProp = pf.createEphemeralProperty("Error Is Time Stable", false); currentModeProp = pf.createEphemeralProperty("Current Mode", "Not Yet Run"); subsystemReportsReadyProp = pf.createEphemeralProperty("Subsystem Ready", false); diff --git a/src/main/java/xbot/common/command/BaseRobot.java b/src/main/java/xbot/common/command/BaseRobot.java index a44f9516..fa805f9a 100644 --- a/src/main/java/xbot/common/command/BaseRobot.java +++ b/src/main/java/xbot/common/command/BaseRobot.java @@ -298,12 +298,16 @@ else if(timeSinceLastLog >= frequencyReportInterval.get()) { @Override public void simulationInit() { + /* webots = injectorComponent.webotsClient(); webots.initialize(); + + */ } @Override public void simulationPeriodic() { + /* // find all simulatable motors List motors = new ArrayList(); @@ -319,5 +323,6 @@ public void simulationPeriodic() { JSONObject response = webots.sendMotors(motors); simulationPayloadDistributor.distributeSimulationPayload(response); + */ } } diff --git a/src/main/java/xbot/common/controls/actuators/XCANSparkMax.java b/src/main/java/xbot/common/controls/actuators/XCANSparkMax.java index 9e097549..2ff3099b 100644 --- a/src/main/java/xbot/common/controls/actuators/XCANSparkMax.java +++ b/src/main/java/xbot/common/controls/actuators/XCANSparkMax.java @@ -21,18 +21,19 @@ public abstract class XCANSparkMax { protected int deviceId; protected String prefix = ""; PropertyFactory propertyFactory; + protected boolean usesPropertySystem = true; - final DoubleProperty kPprop; - final DoubleProperty kIprop; - final DoubleProperty kDprop; - final DoubleProperty kIzProp; - final DoubleProperty kFFprop; - final DoubleProperty kMaxOutputProp; - final DoubleProperty kMinOutoutProp; + DoubleProperty kPprop; + DoubleProperty kIprop; + DoubleProperty kDprop; + DoubleProperty kIzProp; + DoubleProperty kFFprop; + DoubleProperty kMaxOutputProp; + DoubleProperty kMinOutoutProp; - final DoubleProperty percentProp; - final DoubleProperty voltageProp; - final DoubleProperty currentProp; + DoubleProperty percentProp; + DoubleProperty voltageProp; + DoubleProperty currentProp; protected final String policeTicket; @@ -48,6 +49,10 @@ public abstract XCANSparkMax create( public XCANSparkMax create(DeviceInfo deviceInfo, String owningSystemPrefix, String name) { return create(deviceInfo, owningSystemPrefix, name, new XCANSparkMaxPIDProperties()); } + + public XCANSparkMax createWithoutProperties(DeviceInfo deviceInfo, String owningSystemPrefix, String name) { + return create(deviceInfo, owningSystemPrefix, name, null); + } } protected XCANSparkMax( @@ -64,18 +69,22 @@ protected XCANSparkMax( prefix = pf.getPrefix(); policeTicket = police.registerDevice(DeviceType.CAN, deviceId, this); - kPprop = pf.createPersistentProperty("kP", defaultPIDProperties.p); - kIprop = pf.createPersistentProperty("kI", defaultPIDProperties.i); - kDprop = pf.createPersistentProperty("kD", defaultPIDProperties.d); - kMaxOutputProp = pf.createPersistentProperty("kMaxOutput", defaultPIDProperties.maxOutput); - kMinOutoutProp = pf.createPersistentProperty("kMinOutput", defaultPIDProperties.minOutput); - - pf.setDefaultLevel(Property.PropertyLevel.Debug); - percentProp = pf.createEphemeralProperty("Percent", 0); - voltageProp = pf.createEphemeralProperty("Voltage", 0); - currentProp = pf.createEphemeralProperty("Current", 0); - kIzProp = pf.createPersistentProperty("kIzone", defaultPIDProperties.iZone); - kFFprop = pf.createPersistentProperty("kFeedForward", defaultPIDProperties.feedForward); + if (defaultPIDProperties == null) { + usesPropertySystem = false; + } else { + kPprop = pf.createPersistentProperty("kP", defaultPIDProperties.p); + kIprop = pf.createPersistentProperty("kI", defaultPIDProperties.i); + kDprop = pf.createPersistentProperty("kD", defaultPIDProperties.d); + + pf.setDefaultLevel(Property.PropertyLevel.Debug); + percentProp = pf.createEphemeralProperty("Percent", 0); + voltageProp = pf.createEphemeralProperty("Voltage", 0); + currentProp = pf.createEphemeralProperty("Current", 0); + kIzProp = pf.createPersistentProperty("kIzone", defaultPIDProperties.iZone); + kFFprop = pf.createPersistentProperty("kFeedForward", defaultPIDProperties.feedForward); + kMaxOutputProp = pf.createPersistentProperty("kMaxOutput", defaultPIDProperties.maxOutput); + kMinOutoutProp = pf.createPersistentProperty("kMinOutput", defaultPIDProperties.minOutput); + } } /// @@ -87,30 +96,34 @@ public String getPrefix() { } private void setAllProperties() { - setP(kPprop.get()); - setI(kIprop.get()); - setD(kDprop.get()); - setIZone(kIzProp.get()); - setFF(kFFprop.get()); - setOutputRange(kMinOutoutProp.get(), kMaxOutputProp.get()); + if (usesPropertySystem) { + setP(kPprop.get()); + setI(kIprop.get()); + setD(kDprop.get()); + setIZone(kIzProp.get()); + setFF(kFFprop.get()); + setOutputRange(kMinOutoutProp.get(), kMaxOutputProp.get()); + } } public void periodic() { - if (firstPeriodicCall) { - setAllProperties(); - firstPeriodicCall = false; + if (usesPropertySystem) { + if (firstPeriodicCall) { + setAllProperties(); + firstPeriodicCall = false; + } + kPprop.hasChangedSinceLastCheck((value) -> setP(value)); + kIprop.hasChangedSinceLastCheck((value) -> setI(value)); + kDprop.hasChangedSinceLastCheck((value) -> setD(value)); + kIzProp.hasChangedSinceLastCheck((value) -> setIZone(value)); + kFFprop.hasChangedSinceLastCheck((value) -> setFF(value)); + kMaxOutputProp.hasChangedSinceLastCheck((value) -> setOutputRange(kMinOutoutProp.get(), value)); + kMinOutoutProp.hasChangedSinceLastCheck((value) -> setOutputRange(value, kMaxOutputProp.get())); + + percentProp.set(getAppliedOutput()); + voltageProp.set(getAppliedOutput() * getBusVoltage()); + currentProp.set(getOutputCurrent()); } - kPprop.hasChangedSinceLastCheck((value) -> setP(value)); - kIprop.hasChangedSinceLastCheck((value) -> setI(value)); - kDprop.hasChangedSinceLastCheck((value) -> setD(value)); - kIzProp.hasChangedSinceLastCheck((value) -> setIZone(value)); - kFFprop.hasChangedSinceLastCheck((value) -> setFF(value)); - kMaxOutputProp.hasChangedSinceLastCheck((value) -> setOutputRange(kMinOutoutProp.get(), value)); - kMinOutoutProp.hasChangedSinceLastCheck((value) -> setOutputRange(value, kMaxOutputProp.get())); - - percentProp.set(getAppliedOutput()); - voltageProp.set(getAppliedOutput() * getBusVoltage()); - currentProp.set(getOutputCurrent()); } /**** Speed Controller Interface ****/ diff --git a/src/main/java/xbot/common/logic/HumanVsMachineDecider.java b/src/main/java/xbot/common/logic/HumanVsMachineDecider.java index 34843607..071aac83 100644 --- a/src/main/java/xbot/common/logic/HumanVsMachineDecider.java +++ b/src/main/java/xbot/common/logic/HumanVsMachineDecider.java @@ -5,6 +5,7 @@ import dagger.assisted.AssistedInject; import xbot.common.controls.sensors.XTimer; import xbot.common.properties.DoubleProperty; +import xbot.common.properties.Property; import xbot.common.properties.PropertyFactory; public class HumanVsMachineDecider { @@ -30,11 +31,10 @@ public abstract static class HumanVsMachineDeciderFactory { public HumanVsMachineDecider(@Assisted("prefix") String prefix, PropertyFactory propMan) { propMan.setPrefix(prefix); propMan.appendPrefix("Decider"); + propMan.setDefaultLevel(Property.PropertyLevel.Debug); deadbandProp = propMan.createPersistentProperty("Deadband", 0.1); coastTimeProp = propMan.createPersistentProperty("Coast Time", 0.3); reset(); - - } public void reset() { diff --git a/src/main/java/xbot/common/math/PIDManager.java b/src/main/java/xbot/common/math/PIDManager.java index cce375de..020b958d 100644 --- a/src/main/java/xbot/common/math/PIDManager.java +++ b/src/main/java/xbot/common/math/PIDManager.java @@ -109,9 +109,9 @@ public PIDManager( super(functionName, propMan, assertionManager, defaultP, defaultI, defaultD, defaultF, errorThreshold, derivativeThreshold, timeThreshold, iZone); + propMan.setDefaultLevel(Property.PropertyLevel.Debug); maxOutput = propMan.createPersistentProperty("Max Output", defaultMaxOutput); minOutput = propMan.createPersistentProperty("Min Output", defaultMinOutput); - propMan.setDefaultLevel(Property.PropertyLevel.Debug); isEnabled = propMan.createPersistentProperty("Is Enabled", true); offTargetReasonProp = propMan.createEphemeralProperty("OffTargetReason", ""); propMan.setDefaultLevel(Property.PropertyLevel.Important); diff --git a/src/main/java/xbot/common/properties/XPropertyManager.java b/src/main/java/xbot/common/properties/XPropertyManager.java index bdd285f3..58f6a828 100644 --- a/src/main/java/xbot/common/properties/XPropertyManager.java +++ b/src/main/java/xbot/common/properties/XPropertyManager.java @@ -34,7 +34,7 @@ public XPropertyManager( this.properties = new ArrayList(); this.permanentStore = permanentStore; this.randomAccessStore = randomAccessStore; - this.inMemoryRandomAccessStore = inMemoryRandomAccessStore; + this.inMemoryRandomAccessStore = new TableProxy(); } /**