System checks

commands/systemchecks/BasicCheck.java

@@ -0,0 +1,75 @@
+package org.usfirst.frc4904.standard.commands.systemchecks;
+
+
+import java.util.HashMap;
+import java.util.Map;
+import java.util.stream.Stream;
+import java.util.Arrays;
+import org.usfirst.frc4904.standard.LogKitten;
+import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus;
+import edu.wpi.first.wpilibj.command.Command;
+
+public abstract class BasicCheck extends Command implements Check {
+    protected HashMap<String, StatusMessage> statuses;
+    protected final String[] systemNames;
+    protected static final double DEFAULT_TIMEOUT = 5;
+
+    public BasicCheck(String checkName, double timeout, String... systemNames) {
+        super(checkName, timeout);
+        this.systemNames = systemNames;
+        initStatuses();
+    }
+
+    public BasicCheck(double timeout, String... systemNames) {
+        this("Check", timeout, systemNames);
+    }
+
+    public BasicCheck(String... systemNames) {
+        this(DEFAULT_TIMEOUT, systemNames);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return isTimedOut();
+    }
+
+    @Override
+    public void end() {
+        outputStatuses();
+    }
+
+    public void setStatus(String name, SystemStatus status, Exception... exceptions) {
+        statuses.put(name, new StatusMessage(status, exceptions));
+    }
+
+    public void initStatus(String name) {
+        setStatus(name, SystemStatus.PASS, new Exception("NO ERROR"));
+    }
+
+    public void initStatuses() {
+        for (String name : systemNames) {
+            initStatus(name);
+        }
+    }
+
+    public void updateStatus(String key, StatusMessage.SystemStatus status, Exception... exceptions) {
+        setStatus(key, status, Stream.concat(Arrays.stream(getStatusMessage(key).exceptions), Arrays.stream(exceptions))
+            .toArray(Exception[]::new));
+    }
+
+    public StatusMessage getStatusMessage(String key) {
+        return statuses.get(key);
+    }
+
+    public void outputStatuses() {
+        LogKitten.wtf(getName() + " Statuses:");
+        for (Map.Entry<String, StatusMessage> entry : statuses.entrySet()) {
+            String name = entry.getKey();
+            StatusMessage message = entry.getValue();
+            // TODO: Change Logkitten level if FAIL vs PASS
+            LogKitten.wtf("Subsystem: " + name + ", Status: "
+                + (message.status == StatusMessage.SystemStatus.PASS ? "PASS" : "FAIL") + ", ERROR: " + message.exceptions);
+            // TODO:check if the logging of exceptions above works
+        }
+    }
+}

commands/systemchecks/Check.java

@@ -0,0 +1,26 @@
+package org.usfirst.frc4904.standard.commands.systemchecks;
+
+
+import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus;
+
+public interface Check {
+    public void setStatus(String key, StatusMessage.SystemStatus status, Exception... exceptions);
+
+    default void initStatus(String name) {
+        setStatus(name, SystemStatus.PASS, new Exception("NO ERROR"));
+    }
+
+    public void initStatuses();
+
+    public void updateStatus(String key, StatusMessage.SystemStatus status, Exception... exceptions);
+
+    default void setStatusFail(String key, Exception... exceptions) {
+        updateStatus(key, SystemStatus.FAIL, exceptions);
+    }
+
+    default void setStatusPass(String key, Exception... exceptions) {
+        updateStatus(key, SystemStatus.PASS, exceptions);
+    }
+
+    public void outputStatuses();
+}

commands/systemchecks/CheckGroup.java

@@ -0,0 +1,17 @@
+package org.usfirst.frc4904.standard.commands.systemchecks;
+
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class CheckGroup extends CommandGroup {
+    public CheckGroup(String name, BasicCheck... checks) {
+        for (BasicCheck check : checks) {
+            if (check instanceof SubsystemCheck) { //TODO: Think of better logic for this
+                addSequential(check);
+            }
+            else {
+                addParallel(check);
+            }
+        }
+    }
+}

commands/systemchecks/CompressorCheck.java

@@ -0,0 +1,41 @@
+package org.usfirst.frc4904.standard.commands.systemchecks;
+
+
+import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus;
+import edu.wpi.first.wpilibj.Compressor;
+
+public class CompressorCheck extends SystemCheck {
+    protected final Compressor[] compressors;
+
+    public CompressorCheck(String name, double timeout, Compressor... compressors) {
+        super("CompressorCheck", timeout, compressors);
+        this.compressors = compressors;
+    }
+
+    public CompressorCheck(String name, Compressor... compressors) {
+        this(name, DEFAULT_TIMEOUT, compressors);
+    }
+
+    public CompressorCheck(double timeout, Compressor... compressors) {
+        this("CompressorCheck", timeout, compressors);
+    }
+
+    public CompressorCheck(Compressor... compressors) {
+        this("CompressorCheck", compressors);
+    }
+
+    public void execute() {
+        for (Compressor compressor : compressors) {
+            if (compressor.getCompressorNotConnectedFault() || compressor.getCompressorNotConnectedStickyFault()
+                || !compressor.enabled()) {
+                updateStatus(compressor.getName(), SystemStatus.FAIL, new Exception("COMPRESSOR NOT CONNECTED"));
+            } else if (compressor.getCompressorShortedFault() || compressor.getCompressorShortedStickyFault()) {
+                updateStatus(compressor.getName(), SystemStatus.FAIL, new Exception("COMPRESSOR SHORTED"));
+            } else if (compressor.getCompressorCurrentTooHighFault() || compressor.getCompressorCurrentTooHighStickyFault()) {
+                updateStatus(compressor.getName(), SystemStatus.FAIL, new Exception("CURRENT TOO HIGH"));
+            } else if (compressor.getPressureSwitchValue()) { // TODO: Check if this works as intended
+                updateStatus(compressor.getName(), SystemStatus.FAIL, new Exception("LOW PRESSURE"));
+            }
+        }
+    }
+}

commands/systemchecks/DrivePIDCheck.java

@@ -0,0 +1,65 @@
+package org.usfirst.frc4904.standard.commands.systemchecks;
+
+
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.stream.Stream;
+import org.usfirst.frc4904.standard.LogKitten;
+import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance;
+import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus;
+import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController;
+import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException;
+import org.usfirst.frc4904.standard.subsystems.chassis.Chassis;
+
+public class DrivePIDCheck extends ChassisMoveDistance implements Check {
+    protected HashMap<String, StatusMessage> statuses;
+    protected static final String CHECK_NAME = "DrivePIDCheck";
+    protected final double distance;
+    protected static final double ERROR_THRESHOLD = 2.0; // TODO: CHANGE THIS
+    protected double position;
+
+    public DrivePIDCheck(Chassis chassis, double distance, MotionController motionController) {
+        super(chassis, distance, motionController);
+        this.distance = distance;
+        initStatuses();
+    }
+
+    public void end() {
+        try {
+            position = motionController.getInputSafely();
+        }
+        catch (InvalidSensorException e) {
+            position = 0;
+            updateStatus(CHECK_NAME, SystemStatus.FAIL, e);
+        }
+        if (Math.abs(motionController.getSetpoint() - position) > ERROR_THRESHOLD) {
+            updateStatus(CHECK_NAME, SystemStatus.FAIL, new Exception("DISTANCE DRIVEN NOT WITHIN ERROR THRESHOLD"));
+        }
+        chassisMove.cancel();
+        motionController.disable();
+        motionController.reset();
+        runOnce = false;
+    }
+
+    public void initStatuses() {
+        initStatus(CHECK_NAME);
+    }
+
+    public void setStatus(String name, SystemStatus status, Exception... exceptions) {
+        statuses.put(name, new StatusMessage(status, exceptions));
+    }
+
+    public void updateStatus(String key, SystemStatus status, Exception... exceptions) {
+        setStatus(key, status,
+            Stream.concat(Arrays.stream(getStatusMessage(key).exceptions), Arrays.stream(exceptions))
+                .toArray(Exception[]::new));
+    }
+
+    public StatusMessage getStatusMessage(String key) {
+        return statuses.get(key);
+    }
+
+    public void outputStatuses() {
+        LogKitten.wtf(CHECK_NAME + ": " + getStatusMessage(CHECK_NAME).exceptions); // TODO: change logkitten level
+    }
+}

commands/systemchecks/NetworkTablesCheck.java

@@ -0,0 +1,49 @@
+package org.usfirst.frc4904.standard.commands.systemchecks;
+
+
+import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.PersistentException;
+import edu.wpi.first.networktables.NetworkTableEntry;
+
+public class NetworkTablesCheck extends BasicCheck {
+    protected static final String SAVE_PATH = "/home/lvuser/logs/networktables";
+    protected static final String SAVE_FILE = SAVE_PATH + "testTable.log";
+    protected static final String CHECK_NAME = "NetworkTables";
+    protected static final double DEFAULT_ENTRY = 4.904;
+    protected static NetworkTableInstance inst;
+    protected static NetworkTable table;
+    protected static NetworkTableEntry entry;
+    protected static String[] entries;
+
+    public NetworkTablesCheck(double timeout) {
+        super("NetworkTableCheck", timeout, CHECK_NAME);
+    }
+
+    public NetworkTablesCheck() {
+        this(DEFAULT_TIMEOUT);
+    }
+
+    public void initialize() {
+        inst = NetworkTableInstance.getDefault();
+        table = inst.getTable("table");
+        entry = table.getEntry("entry");
+        entry.setDefaultDouble(DEFAULT_ENTRY);
+        try {
+            table.saveEntries(SAVE_FILE);
+            entries = table.loadEntries(SAVE_FILE);
+        }
+        catch (PersistentException e) {
+            updateStatus(CHECK_NAME, SystemStatus.FAIL, e);
+        }
+        if (entries[0] != Double.toString(DEFAULT_ENTRY)) { // TODO: Check formatting of the entries array
+            updateStatus(CHECK_NAME, SystemStatus.FAIL, new Exception("LOADED ARRAY NOT EQUAL TO SAVED ARRAY"));
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}

commands/systemchecks/SolenoidCheck.java

@@ -0,0 +1,29 @@
+package org.usfirst.frc4904.standard.commands.systemchecks;
+
+
+import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+
+public class SolenoidCheck extends SystemCheck {
+    protected final DoubleSolenoid[] solenoids;
+
+    public SolenoidCheck(String name, DoubleSolenoid... solenoids) {
+        super(name, solenoids);
+        this.solenoids = solenoids;
+    }
+
+    public SolenoidCheck(DoubleSolenoid... solenoids) {
+        this("SolenoidCheck", solenoids);
+    }
+
+    public void initialize() {
+        for (DoubleSolenoid solenoid : solenoids) {
+            try {
+                solenoid.set(DoubleSolenoid.Value.kForward);
+            }
+            catch (Exception e) {
+                updateStatus(solenoid.getName(), SystemStatus.FAIL, e);
+            }
+        }
+    }
+}

commands/systemchecks/StatusMessage.java

@@ -0,0 +1,16 @@
+package org.usfirst.frc4904.standard.commands.systemchecks;
+
+
+public class StatusMessage {
+    protected SystemStatus status;
+    protected Exception[] exceptions;
+
+    public StatusMessage(SystemStatus status, Exception... exceptions) {
+        this.status = status;
+        this.exceptions = exceptions;
+    }
+
+    public enum SystemStatus {
+        PASS, FAIL;
+    }
+}

commands/systemchecks/SubsystemCheck.java

@@ -0,0 +1,25 @@
+package org.usfirst.frc4904.standard.commands.systemchecks;
+
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+public abstract class SubsystemCheck extends SystemCheck {
+    public SubsystemCheck(String name, double timeout, Subsystem... subsystems) {
+        super(name, timeout, subsystems);
+        for (Subsystem system : subsystems) {
+            requires(system);
+        }
+    }
+
+    public SubsystemCheck(String name, Subsystem... systems) {
+        this(name, DEFAULT_TIMEOUT, systems);
+    }
+
+    public SubsystemCheck(double timeout, Subsystem... systems) {
+        this("SubsystemCheck", timeout, systems);
+    }
+
+    public SubsystemCheck(Subsystem... systems) {
+        this("SubsystemCheck", systems);
+    }
+}

commands/systemchecks/SystemCheck.java

@@ -0,0 +1,30 @@
+package org.usfirst.frc4904.standard.commands.systemchecks;
+
+
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.stream.Collectors;
+import edu.wpi.first.wpilibj.SendableBase;
+
+public abstract class SystemCheck extends BasicCheck {
+    protected HashMap<String, StatusMessage> statuses;
+    protected SendableBase[] systems;
+
+    public SystemCheck(String name, double timeout, SendableBase... systems) {
+        super(name, timeout, Arrays.asList(systems).stream().map(system -> system.getName()).collect(Collectors.toList())
+            .toArray(String[]::new));
+        this.systems = systems;
+    }
+
+    public SystemCheck(double timeout, SendableBase... systems) {
+        this("SystemCheck", timeout, systems);
+    }
+
+    public SystemCheck(String name, SendableBase... systems) {
+        this(name, DEFAULT_TIMEOUT, systems);
+    }
+
+    public SystemCheck(SendableBase... systems) {
+        this("SystemCheck", systems);
+    }
+}