diff --git a/commands/systemchecks/BaseCheck.java b/commands/systemchecks/BaseCheck.java new file mode 100644 index 00000000..671b73a0 --- /dev/null +++ b/commands/systemchecks/BaseCheck.java @@ -0,0 +1,144 @@ +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; + +/** + * BaseCheck is the base class for system checks. + * Creates a hashmap between systems and their respective statuses + * that can be updated depending on the results of system checks. + */ +public abstract class BaseCheck extends Command implements Check { + protected static final double DEFAULT_TIMEOUT = 5; + protected HashMap statuses; + protected final String[] systemNames; + + /** + * @param checkName + * The name of the command + * @param timeout + * Duration of the command + * @param systemNames + * Names of the systems being checked + */ + public BaseCheck(String checkName, double timeout, String... systemNames) { + super(checkName, timeout); + this.systemNames = systemNames; + initStatuses(); + } + + /** + * @param timeout + * Duration of the command + * @param systemNames + * Names of the systems being checked + */ + public BaseCheck(double timeout, String... systemNames) { + this("Check", timeout, systemNames); + } + + /** + * @param systemNames + * Names of the systems being checked + */ + public BaseCheck(String... systemNames) { + this(DEFAULT_TIMEOUT, systemNames); + } + + @Override + public boolean isFinished() { + return isTimedOut(); + } + + @Override + public void end() { + outputStatuses(); + } + + /** + * Sets status of a system given: + * + * @param key + * name of the system + * @param status + * pass or fail status + * @param exceptions + * any exception that the class causes + * + * Status should be SystemStatus.FAIL if exceptions given. + */ + public void setStatus(String key, SystemStatus status, Exception... exceptions) { + statuses.put(key, new StatusMessage(status, exceptions)); + } + + /** + * Initializes a name-status pair to have a PASS status and no exceptions + * + * @param key + * name of the system + */ + public void initStatus(String key) { + setStatus(key, SystemStatus.PASS); + } + + /** + * Initializes all systems in systemNames using initStatus + */ + public void initStatuses() { + for (String name : systemNames) { + initStatus(name); + } + } + + /** + * Updates the status of a name-status pair by adding on exceptions + * + * @param name + * name of the system + * @param status + * pass or fail status + * @param exceptions + * any exception that the class causes + */ + 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)); + } + + /** + * Gets the StatusMessage (SystemStatus + exceptions) of a system given its name + * + * @param key + * name of system + * @return StatusMessage of system + */ + public StatusMessage getStatusMessage(String key) { + return statuses.get(key); + } + + /** + * Logs the statuses of all systems with variable severity + * depending on the status of the system + */ + public void outputStatuses() { + LogKitten.d(getName() + " Statuses:"); + for (Map.Entry entry : statuses.entrySet()) { + String name = entry.getKey(); + StatusMessage message = entry.getValue(); + if (message.status == SystemStatus.PASS) { + LogKitten.d("Subsystem: " + name + ", Status: PASS"); + } else { + LogKitten.wtf("Subsystem: " + name + ", Status: FAIL"); + for (Exception e : message.exceptions) { + LogKitten.wtf(e); + } + } + } + } +} \ No newline at end of file diff --git a/commands/systemchecks/BatteryCheck.java b/commands/systemchecks/BatteryCheck.java new file mode 100644 index 00000000..e0829329 --- /dev/null +++ b/commands/systemchecks/BatteryCheck.java @@ -0,0 +1,58 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + + +import edu.wpi.first.hal.can.CANStatus; +import edu.wpi.first.wpilibj.RobotController; + +/** + * Systemcheck of battery status + */ +public class BatteryCheck extends BaseCheck { + protected final double MIN_VOLTAGE = 9.0; + protected CANStatus status; + protected static final String systemName = "BATTERY"; + + /** + * Systemcheck of battery status + * + * @param name + * name of check + * @param timeout + * duration of check + */ + public BatteryCheck(String name, double timeout) { + super(name, timeout, systemName); + } + + /** + * Systemcheck of battery status + * + * @param timeout + * duration of check + */ + public BatteryCheck(double timeout) { + super("RobotControllerCheck", timeout); + } + + /** + * Systemcheck of battery status + * + * @param name + * name of check + * @param timeout + * duration of check + */ + public BatteryCheck(String name) { + super(name, DEFAULT_TIMEOUT); + } + + public void execute() { + status = RobotController.getCANStatus(); + if (RobotController.getBatteryVoltage() < MIN_VOLTAGE) { + updateStatusFail(systemName, new Exception("BATTERY VOLTAGE LESS THAN MIN VOLTAGE REQUIREMENT")); + } + if (RobotController.isBrownedOut()) { + updateStatusFail(systemName, new Exception("BROWNED OUT")); + } + } +} \ No newline at end of file diff --git a/commands/systemchecks/CANCheck.java b/commands/systemchecks/CANCheck.java new file mode 100644 index 00000000..948bbe53 --- /dev/null +++ b/commands/systemchecks/CANCheck.java @@ -0,0 +1,54 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + + +import edu.wpi.first.hal.can.CANStatus; +import edu.wpi.first.wpilibj.RobotController; + +/** + * System check of CAN + */ +public class CANCheck extends BaseCheck { + protected final int MAX_ERROR_COUNT = 127; + protected final int MAX_OFF_COUNT = 0; + protected static final String systemName = "CAN"; + protected CANStatus status; + + /** + * @param name + * name of check + * @param timeout + * duration of check + */ + public CANCheck(String name, double timeout) { + super(name, timeout, systemName); + } + + /** + * @param timeout + * duration of check + */ + public CANCheck(double timeout) { + super("CANCheck", timeout); + } + + /** + * @param name + * name of check + */ + public CANCheck(String name) { + super(name, DEFAULT_TIMEOUT); + } + + public void execute() { + status = RobotController.getCANStatus(); + if (status.receiveErrorCount > MAX_ERROR_COUNT) { + updateStatusFail(systemName, new Exception("TOO MANY RECEIVE ERRORS")); + } + if (status.transmitErrorCount > MAX_ERROR_COUNT) { + updateStatusFail(systemName, new Exception("TOO MANY TRANSMISSION ERRORS")); + } + if (status.busOffCount > MAX_OFF_COUNT) { + updateStatusFail(systemName, new Exception("TOO MANY CANBUS OFF OCCURANCES")); + } + } +} \ No newline at end of file diff --git a/commands/systemchecks/Check.java b/commands/systemchecks/Check.java new file mode 100644 index 00000000..79a83d42 --- /dev/null +++ b/commands/systemchecks/Check.java @@ -0,0 +1,72 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + + +import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; + +public interface Check { + /** + * Sets the status of a system, overriding previous exceptions + * + * @param key + * name of system + * @param status + * PASS or FAIL + * @param exceptions + * exceptions caused by the system + */ + public void setStatus(String key, SystemStatus status, Exception... exceptions); + + /** + * Initializes the status of system to PASS + * + * @param key + * name of system + */ + default void initStatus(String key) { + setStatus(key, SystemStatus.PASS); + } + + /** + * Initializes statuses of all systems + */ + public void initStatuses(); + + /** + * Sets the status of a system, preserving past exceptions + * + * @param key + * name of system + * @param status + * PASS or FAIL + * @param exceptions + * exceptions caused by the system + */ + public void updateStatus(String key, SystemStatus status, Exception... exceptions); + + /** + * Sets the status of a system to FAIL + * + * @param key + * name of system + * @param exceptions + * exceptions caused by the system + */ + default void updateStatusFail(String key, Exception... exceptions) { + updateStatus(key, SystemStatus.FAIL, exceptions); + } + + /** + * Sets the status of a system to PASS + * + * @param key + * name of system + */ + default void setStatusPass(String key) { + setStatus(key, SystemStatus.PASS); + } + + /** + * Outputs the statuses of all systems of check + */ + public void outputStatuses(); +} \ No newline at end of file diff --git a/commands/systemchecks/CheckGroup.java b/commands/systemchecks/CheckGroup.java new file mode 100644 index 00000000..075c3269 --- /dev/null +++ b/commands/systemchecks/CheckGroup.java @@ -0,0 +1,28 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.CommandGroup; + +/** + * Runs a series of robot system checks + */ +public class CheckGroup extends CommandGroup { + /** + * @param name + * name of commandgroup + * @param checks + * systemchecks to run + */ + public CheckGroup(String name, Check... checks) { + for (Check check : checks) { + if (check instanceof Command) { + if (check instanceof SubsystemCheck) { // TODO: Think of better logic for this + addSequential((SubsystemCheck) check); + } else { + addParallel((Command) check); + } + } + } + } +} \ No newline at end of file diff --git a/commands/systemchecks/CompressorCheck.java b/commands/systemchecks/CompressorCheck.java new file mode 100644 index 00000000..f81f1e3e --- /dev/null +++ b/commands/systemchecks/CompressorCheck.java @@ -0,0 +1,67 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + + +import edu.wpi.first.wpilibj.Compressor; + +/** + * System check on compressors + */ +public class CompressorCheck extends SystemCheck { + protected final Compressor[] compressors; + + /** + * @param name + * name of check + * @param timeout + * duration of check + * @param compressors + * compressors to check + */ + public CompressorCheck(String name, double timeout, Compressor... compressors) { + super(name, timeout, compressors); + this.compressors = compressors; + } + + /** + * @param name + * name of check + * @param compressors + * compressors to check + */ + public CompressorCheck(String name, Compressor... compressors) { + this(name, DEFAULT_TIMEOUT, compressors); + } + + /** + * @param timeout + * duration of check + * @param compressors + * compressors to check + */ + public CompressorCheck(double timeout, Compressor... compressors) { + this("CompressorCheck", timeout, compressors); + } + + /** + * @param compressors + * compressors to check + */ + public CompressorCheck(Compressor... compressors) { + this("CompressorCheck", compressors); + } + + public void execute() { + for (Compressor compressor : compressors) { + if (compressor.getCompressorNotConnectedFault() || compressor.getCompressorNotConnectedStickyFault() + || !compressor.enabled()) { + updateStatusFail(compressor.getName(), new Exception("COMPRESSOR NOT CONNECTED")); + } else if (compressor.getCompressorShortedFault() || compressor.getCompressorShortedStickyFault()) { + updateStatusFail(compressor.getName(), new Exception("COMPRESSOR SHORTED")); + } else if (compressor.getCompressorCurrentTooHighFault() || compressor.getCompressorCurrentTooHighStickyFault()) { + updateStatusFail(compressor.getName(), new Exception("CURRENT TOO HIGH")); + } else if (compressor.getPressureSwitchValue()) { // TODO: Test if this works as intended + updateStatusFail(compressor.getName(), new Exception("LOW PRESSURE")); + } + } + } +} \ No newline at end of file diff --git a/commands/systemchecks/DrivePIDCheck.java b/commands/systemchecks/DrivePIDCheck.java new file mode 100644 index 00000000..addd20f5 --- /dev/null +++ b/commands/systemchecks/DrivePIDCheck.java @@ -0,0 +1,122 @@ +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; + +/** + * Checks the accuracy of DrivePID by comparing the position of robot to setpoint + */ +public class DrivePIDCheck extends ChassisMoveDistance implements Check { + protected static final double DEFAULT_ERROR_THRESHOLD = 2.0; // TODO: Test this + protected static final String CHECK_NAME = "DrivePIDCheck"; + protected HashMap statuses; + protected final double distance; + protected double position; + protected final double errorThreshold; + + /** + * Checks the accuracy of DrivePID by comparing the position of robot to setpoint + * + * @param chassis + * Chassis being tested + * @param distance + * Distance to drive forward + * @param motionController + * MotionController used for drive PID + * @param errorThreshold + * threshold for error of the command + */ + public DrivePIDCheck(Chassis chassis, double distance, MotionController motionController, double errorThreshold) { + super(chassis, distance, motionController); + this.distance = distance; + this.errorThreshold = errorThreshold; + initStatuses(); + } + + /** + * Checks the accuracy of DrivePID by comparing the position of robot to setpoint + * + * @param chassis + * Chassis being tested + * @param distance + * Distance to drive forward + * @param motionController + * MotionController used for drive PID + */ + public DrivePIDCheck(Chassis chassis, double distance, MotionController motionController) { + this(chassis, distance, motionController, DEFAULT_ERROR_THRESHOLD); + } + + /** + * Check if driven distance within error threshold + */ + public void end() { + try { + position = motionController.getInputSafely(); + } + catch (InvalidSensorException e) { + position = 0; + updateStatusFail(CHECK_NAME, e); + } + if (Math.abs(motionController.getSetpoint() - position) > errorThreshold) { + updateStatusFail(CHECK_NAME, new Exception("DISTANCE DRIVEN NOT WITHIN ERROR THRESHOLD")); + } + chassisMove.cancel(); + motionController.disable(); + motionController.reset(); + runOnce = false; + outputStatuses(); + } + + /** + * Initialize status of DrivePIDCheck + */ + public void initStatuses() { + initStatus(CHECK_NAME); + } + + /** + * Set status of check, overriding past exceptions + */ + public void setStatus(String name, SystemStatus status, Exception... exceptions) { + statuses.put(name, new StatusMessage(status, exceptions)); + } + + /** + * Update status of check, saving past 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)); + } + + /** + * Get status and exceptions of system + */ + public StatusMessage getStatusMessage(String key) { + return statuses.get(key); + } + + /** + * Log the status of the check + */ + public void outputStatuses() { + if (getStatusMessage(CHECK_NAME).status == SystemStatus.PASS) { + LogKitten.d(CHECK_NAME + ": PASS"); + } else { + LogKitten.e(CHECK_NAME + ": FAIL "); + for (Exception e : getStatusMessage(CHECK_NAME).exceptions) { + LogKitten.e(e); + } + } + } +} \ No newline at end of file diff --git a/commands/systemchecks/NetworkTablesCheck.java b/commands/systemchecks/NetworkTablesCheck.java new file mode 100644 index 00000000..7c694d30 --- /dev/null +++ b/commands/systemchecks/NetworkTablesCheck.java @@ -0,0 +1,57 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PersistentException; +import edu.wpi.first.networktables.NetworkTableEntry; + +/** + * A check on the persistence of Network Tables + */ +public class NetworkTablesCheck extends BaseCheck { + protected static final String SAVE_FILE = "/home/lvuser/logs/networktables/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; + + /** + * @param timeout + * duration of check + */ + public NetworkTablesCheck(double timeout) { + super("NetworkTableCheck", timeout, CHECK_NAME); + } + + public NetworkTablesCheck() { + this(DEFAULT_TIMEOUT); + } + + /** + * Constructs network tables and tests the persistence of Network table data + */ + 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) { + updateStatusFail(CHECK_NAME, e); + } + if (entries[0] != Double.toString(DEFAULT_ENTRY)) { // TODO: Check formatting of the entries array + updateStatusFail(CHECK_NAME, new Exception("LOADED ARRAY NOT EQUAL TO SAVED ARRAY")); + } + } + + @Override + public boolean isFinished() { + return true; + } +} \ No newline at end of file diff --git a/commands/systemchecks/SolenoidCheck.java b/commands/systemchecks/SolenoidCheck.java new file mode 100644 index 00000000..c5763dcf --- /dev/null +++ b/commands/systemchecks/SolenoidCheck.java @@ -0,0 +1,66 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + + +import edu.wpi.first.wpilibj.DoubleSolenoid; + +/** + * System Check of solenoids + */ +public class SolenoidCheck extends SystemCheck { + protected final DoubleSolenoid[] solenoids; + + /** + * @param name + * name of check + * @param timeout + * duration of check + * @param solenoids + * list of solenoids + */ + public SolenoidCheck(String name, double timeout, DoubleSolenoid... solenoids) { + super(name, timeout, solenoids); + this.solenoids = solenoids; + } + + /** + * @param name + * name of check + * @param solenoids + * list of solenoids + */ + public SolenoidCheck(String name, DoubleSolenoid... solenoids) { + this(name, DEFAULT_TIMEOUT, solenoids); + } + + /** + * @param timeout + * duration of check + * @param solenoids + * list of solenoids + */ + public SolenoidCheck(double timeout, DoubleSolenoid... solenoids) { + this("SolenoidCheck", timeout, solenoids); + } + + /** + * @param solenoids + * list of solenoids + */ + public SolenoidCheck(DoubleSolenoid... solenoids) { + this("SolenoidCheck", solenoids); + } + + /** + * Sets every solenoid to FORWARD and updates status if this fails + */ + public void initialize() { + for (DoubleSolenoid solenoid : solenoids) { + try { + solenoid.set(DoubleSolenoid.Value.kForward); + } + catch (Exception e) { + updateStatusFail(solenoid.getName(), e); + } + } + } +} \ No newline at end of file diff --git a/commands/systemchecks/StatusMessage.java b/commands/systemchecks/StatusMessage.java new file mode 100644 index 00000000..4325bcc5 --- /dev/null +++ b/commands/systemchecks/StatusMessage.java @@ -0,0 +1,24 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + +/** + * Status class that contains SystemStatus and Exceptions + */ +public class StatusMessage { + protected SystemStatus status; + protected Exception[] exceptions; + + /** + * @param status + * status of StatusMessage + * @param exceptions + * exceptions of StatusMessage + */ + public StatusMessage(SystemStatus status, Exception... exceptions) { + this.status = status; + this.exceptions = exceptions; + } + + public enum SystemStatus { + PASS, FAIL; + } +} \ No newline at end of file diff --git a/commands/systemchecks/SubsystemCheck.java b/commands/systemchecks/SubsystemCheck.java new file mode 100644 index 00000000..7a1d3b4f --- /dev/null +++ b/commands/systemchecks/SubsystemCheck.java @@ -0,0 +1,52 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + + +import edu.wpi.first.wpilibj.command.Subsystem; + +/** + * SystemCheck that takes in Subsystems + */ +public abstract class SubsystemCheck extends SystemCheck { + /** + * @param name + * name of check + * @param timeout + * duration of check + * @param subsystems + * subsystems being checked + */ + public SubsystemCheck(String name, double timeout, Subsystem... subsystems) { + super(name, timeout, subsystems); + for (Subsystem system : subsystems) { + requires(system); + } + } + + /** + * @param name + * name of check + * @param subsystems + * subsystems being checked + */ + public SubsystemCheck(String name, Subsystem... systems) { + this(name, DEFAULT_TIMEOUT, systems); + } + + /** + * @param timeout + * duration of check + * @param subsystems + * subsystems being checked + */ + public SubsystemCheck(double timeout, Subsystem... systems) { + this("SubsystemCheck", timeout, systems); + } + + /** + * @param subsystems + * subsystems being checked + */ + public SubsystemCheck(Subsystem... systems) { + this("SubsystemCheck", systems); + } +} \ No newline at end of file diff --git a/commands/systemchecks/SystemCheck.java b/commands/systemchecks/SystemCheck.java new file mode 100644 index 00000000..65bcd491 --- /dev/null +++ b/commands/systemchecks/SystemCheck.java @@ -0,0 +1,63 @@ +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; + +/** + * A BaseCheck that takes in SendableBases as systems + */ +public abstract class SystemCheck extends BaseCheck { + protected HashMap statuses; + + /** + * Constructs a basic check with SendableBases: + * + * @param name + * name of command + * @param timeout + * duration of the command + * @param systems + * 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)); + } + + /** + * Constructs a basic check with SendableBases: + * + * @param timeout + * duration of the command + * @param systems + * SendableBase systems + */ + public SystemCheck(double timeout, SendableBase... systems) { + this("SystemCheck", timeout, systems); + } + + /** + * Constructs a basic check with SendableBases: + * + * @param name + * name of command + * @param systems + * SendableBase systems + */ + public SystemCheck(String name, SendableBase... systems) { + this(name, DEFAULT_TIMEOUT, systems); + } + + /** + * Constructs a basic check with SendableBases: + * + * @param systems + * SendableBase systems + */ + public SystemCheck(SendableBase... systems) { + this("SystemCheck", systems); + } +} \ No newline at end of file diff --git a/commands/systemchecks/TurnPIDCheck.java b/commands/systemchecks/TurnPIDCheck.java new file mode 100644 index 00000000..c9095ba0 --- /dev/null +++ b/commands/systemchecks/TurnPIDCheck.java @@ -0,0 +1,128 @@ +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.ChassisTurn; +import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; +import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; +import org.usfirst.frc4904.standard.custom.sensors.IMU; +import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; +import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; + +/** + * Checks accuracy of TurnPID by comparing angle to setpoint + */ +public class TurnPIDCheck extends ChassisTurn implements Check { + protected HashMap statuses; + protected static final String CHECK_NAME = "TurnPIDCheck"; + protected final double finalAngle; + protected static final double DEFAULT_THRESHOLD = 2.0; // TODO: Test + protected final double errorThreshold; + protected double angle; + + /** + * Checks accuracy of TurnPID by comparing angle to setpoint + * + * @param chassis + * Chassis to test + * @param finalAngle + * angle to turn + * @param imu + * IMU used for turning + * @param motioncontroller + * MotionController for turning PID + * @param errorThreshold + * threshold for error of command + */ + public TurnPIDCheck(Chassis chassis, double finalAngle, IMU imu, MotionController motionController, double errorThreshold) { + super(chassis, finalAngle, imu, motionController); + this.finalAngle = finalAngle; + this.errorThreshold = errorThreshold; + initStatuses(); + } + + /** + * Checks accuracy of TurnPID by comparing angle to setpoint + * + * @param chassis + * Chassis to test + * @param finalAngle + * angle to turn + * @param imu + * IMU used for turning + * @param motioncontroller + * MotionController for turning PID + */ + public TurnPIDCheck(Chassis chassis, double finalAngle, IMU imu, MotionController motionController) { + this(chassis, finalAngle, imu, motionController, DEFAULT_THRESHOLD); + } + + /** + * Check if turned angle within error threshold + */ + @Override + public void end() { + try { + angle = motionController.getInputSafely(); + } + catch (InvalidSensorException e) { + angle = 0; + updateStatusFail(CHECK_NAME, e); + } + if (Math.abs(motionController.getSetpoint() - angle) > errorThreshold) { + updateStatusFail(CHECK_NAME, new Exception("ANGLE TURNED NOT WITHIN ERROR THRESHOLD")); + } + move.cancel(); + motionController.disable(); + motionController.reset(); + runOnce = false; + outputStatuses(); + } + + /** + * Initialize status of check + */ + public void initStatuses() { + initStatus(CHECK_NAME); + } + + /** + * Set status of system, overriding past exceptions + */ + public void setStatus(String name, SystemStatus status, Exception... exceptions) { + statuses.put(name, new StatusMessage(status, exceptions)); + } + + /** + * Update status of system, saving past 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)); + } + + /** + * Get status and exceptions of system + */ + public StatusMessage getStatusMessage(String key) { + return statuses.get(key); + } + + /** + * Log status of system + */ + public void outputStatuses() { + if (getStatusMessage(CHECK_NAME).status == SystemStatus.PASS) { + LogKitten.d(CHECK_NAME + ": PASS"); + } else { + LogKitten.e(CHECK_NAME + ": FAIL "); + for (Exception e : getStatusMessage(CHECK_NAME).exceptions) { + LogKitten.e(e); + } + } + } +} \ No newline at end of file diff --git a/commands/systemchecks/motor/MotorCheck.java b/commands/systemchecks/motor/MotorCheck.java new file mode 100644 index 00000000..3643e296 --- /dev/null +++ b/commands/systemchecks/motor/MotorCheck.java @@ -0,0 +1,150 @@ +package org.usfirst.frc4904.standard.commands.systemchecks.motor; + + +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.revrobotics.CANSparkMax; +import org.usfirst.frc4904.standard.Util; +import org.usfirst.frc4904.standard.commands.systemchecks.SubsystemCheck; +import org.usfirst.frc4904.standard.subsystems.motor.Motor; +import edu.wpi.first.wpilibj.SpeedController; + +/** + * SystemCheck of motor health + */ +public class MotorCheck extends SubsystemCheck { + protected static final double DEFAULT_SPEED = 0.5; + protected static final Util.Range outputCurrentRange = new Util.Range(0.1, 0.3); // TODO: Test this range + protected static final Util.Range voltageRange = new Util.Range(11.5, 14); // TODO: Test this range + protected double speed; + protected final Motor[] motors; + + /** + * SystemCheck of motor health + * + * @param name + * Name of check + * @param timeout + * Duration of check + * @param speed + * Speed of motors + * @param motors + * Motors to test + */ + public MotorCheck(String name, double timeout, double speed, Motor... motors) { + super(name, timeout, motors); + this.motors = motors; + this.speed = speed; + } + + /** + * SystemCheck of motor health + * + * @param name + * Name of check + * @param speed + * Speed of motors + * @param motors + * Motors to test + */ + public MotorCheck(String name, double speed, Motor... motors) { + this(name, DEFAULT_TIMEOUT, speed, motors); + } + + /** + * SystemCheck of motor health + * + * @param timeout + * Duration of check + * @param speed + * Speed of motors + * @param motors + * Motors to test + */ + public MotorCheck(double timeout, double speed, Motor... motors) { + this("MotorCheck", timeout, speed, motors); + } + + /** + * SystemCheck of motor health + * + * @param speed + * Speed of motors + * @param motors + * Motors to test + */ + public MotorCheck(double speed, Motor... motors) { + this(DEFAULT_TIMEOUT, speed, motors); + } + + /** + * SystemCheck of motor health + * + * @param name + * Name of check + * @param motors + * Motors to test + */ + public MotorCheck(String name, Motor... motors) { + this(name, DEFAULT_SPEED, motors); + } + + /** + * SystemCheck of motor health + * + * @param motors + * Motors to test + */ + public MotorCheck(Motor... motors) { + this("MotorCheck", motors); + } + + /** + * Sets motor to aforementioned speed + */ + public void initialize() { + for (Motor motor : motors) { + motor.set(speed); + } + } + + /** + * Checks if setting motors returns errors or if speedcontroller currents are not in acceptable range + */ + public void execute() { + for (Motor motor : motors) { + try { + motor.set(speed); + } + catch (Exception e) { + updateStatusFail(motor.getName(), e); + } + VICheck(motor); + } + } + + /** + * Checks voltage input and output current of a motor's speedcontrollers + */ + public void VICheck(Motor motor) { + for (SpeedController controller : motor.getSpeedControllers()) { + double current; + double voltage; + if (controller instanceof CANSparkMax) { + current = ((CANSparkMax) controller).getOutputCurrent(); + voltage = ((CANSparkMax) controller).getBusVoltage(); + } else if (controller instanceof TalonSRX) { + current = ((TalonSRX) controller).getOutputCurrent(); + voltage = ((TalonSRX) controller).getBusVoltage(); + } else { + current = outputCurrentRange.getMax(); + voltage = voltageRange.getMax(); + } + if (!outputCurrentRange.contains(current)) { + updateStatusFail(motor.getName(), new Exception("MOTOR CURRENT NOT IN ACCEPTABLE RANGE")); + } + if (!voltageRange.contains(voltage)) { + updateStatusFail(motor.getName(), new Exception("MOTOR VOLTAGE NOT IN ACCEPTABLE RANGE")); + } + } + } +} \ No newline at end of file diff --git a/commands/systemchecks/motor/PositionMotorCheck.java b/commands/systemchecks/motor/PositionMotorCheck.java new file mode 100644 index 00000000..e6ded29d --- /dev/null +++ b/commands/systemchecks/motor/PositionMotorCheck.java @@ -0,0 +1,57 @@ +package org.usfirst.frc4904.standard.commands.systemchecks.motor; + + +import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor; + +/** + * Systemcheck of PositionSensorMotors + */ +public class PositionMotorCheck extends SensorMotorCheck { + /** + * Systemcheck of PositionSensorMotors + * + * @param name + * Name of check + * @param position + * Position to set PositionSensorMotors + * @param motors + * PositionSensorMotors to test + */ + public PositionMotorCheck(String name, double position, PositionSensorMotor... motors) { + super(name, position, motors); + } + + /** + * Systemcheck of PositionSensorMotors + * + * @param name + * Name of check + * @param motors + * PositionSensorMotors to test + */ + public PositionMotorCheck(String name, PositionSensorMotor... motors) { + this(name, DEFAULT_POSITION, motors); + } + + /** + * Systemcheck of PositionSensorMotors + * + * @param position + * Position to set PositionSensorMotors + * @param motors + * PositionSensorMotors to test + */ + public PositionMotorCheck(double position, PositionSensorMotor... motors) { + this("PositionMotorCheck", position, motors); + } + + /** + * Systemcheck of PositionSensorMotors + * + * @param motors + * PositionSensorMotors to test + */ + public PositionMotorCheck(PositionSensorMotor... motors) { + this("PositionMotorCheck", motors); + } +} \ No newline at end of file diff --git a/commands/systemchecks/motor/SensorMotorCheck.java b/commands/systemchecks/motor/SensorMotorCheck.java new file mode 100644 index 00000000..138453d3 --- /dev/null +++ b/commands/systemchecks/motor/SensorMotorCheck.java @@ -0,0 +1,195 @@ +package org.usfirst.frc4904.standard.commands.systemchecks.motor; + + +import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; +import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor; +import org.usfirst.frc4904.standard.subsystems.motor.SensorMotor; +import org.usfirst.frc4904.standard.subsystems.motor.VelocitySensorMotor; + +/** + * Systemcheck of SensorMotor + */ +public abstract class SensorMotorCheck extends MotorCheck { + protected static final double DEFAULT_POSITION = 50; + protected static final double POSITION_THRESHOLD = 2.0; // TODO: TEST THIS + protected static final double VELOCITY_THRESHOLD = 2.0; // TODO: TEST THIS + protected final double timeout; + protected SensorMotor[] motors; + protected double position; + + /** + * Systemcheck of SensorMotor + * + * @param name + * Name of check + * @param timeout + * Duration of check + * @param speed + * Speed to set VelocitySensorMotors + * @param position + * Position to set PositionSensorMotors + * @param motors + * SensorMotors to test + */ + public SensorMotorCheck(String name, double timeout, double speed, double position, SensorMotor... motors) { + super(name, timeout, motors); + this.motors = motors; + this.timeout = timeout; + this.speed = speed; + this.position = position; + } + + /** + * Systemcheck of SensorMotor + * + * @param name + * Name of check + * @param speed + * Speed to set VelocitySensorMotors + * @param position + * Position to set PositionSensorMotors + * @param motors + * SensorMotors to test + */ + public SensorMotorCheck(String name, double speed, double position, SensorMotor... motors) { + this(name, DEFAULT_TIMEOUT, speed, position, motors); + } + + /** + * Systemcheck of SensorMotor + * + * @param timeout + * Duration of check + * @param speed + * Speed to set VelocitySensorMotors + * @param position + * Position to set PositionSensorMotors + * @param motors + * SensorMotors to test + */ + public SensorMotorCheck(double timeout, double speed, double position, SensorMotor... motors) { + this("SensorMotorCheck", timeout, speed, position, motors); + } + + /** + * Systemcheck of SensorMotor + * + * @param speed + * Speed to set VelocitySensorMotors + * @param position + * Position to set PositionSensorMotors + * @param motors + * SensorMotors to test + */ + public SensorMotorCheck(double speed, double position, SensorMotor... motors) { + this(DEFAULT_TIMEOUT, speed, position, motors); + } + + /** + * Systemcheck of SensorMotor + * + * @param name + * Name of check + * @param position + * Position to set PositionSensorMotors + * @param motors + * SensorMotors to test + */ + public SensorMotorCheck(String name, double position, SensorMotor... motors) { + this(name, DEFAULT_TIMEOUT, DEFAULT_SPEED, position, motors); + } + + /** + * Systemcheck of SensorMotor + * + * @param position + * Position to set PositionSensorMotors + * @param motors + * SensorMotors to test + */ + public SensorMotorCheck(double position, SensorMotor... motors) { + this("SensorMotorCheck", position, motors); + } + + /** + * Systemcheck of SensorMotor + * + * @param name + * Name of check + * @param motors + * SensorMotors to test + */ + public SensorMotorCheck(String name, SensorMotor... motors) { + this(name, DEFAULT_POSITION, motors); + } + + /** + * Systemcheck of SensorMotor + * + * @param motors + * SensorMotors to test + */ + public SensorMotorCheck(SensorMotor... motors) { + this("SensorMotorCheck", motors); + } + + /** + * Sets SensorMotors depending on type + */ + @Override + public void initialize() { + for (SensorMotor motor : motors) { + if (motor instanceof VelocitySensorMotor) { + ((VelocitySensorMotor) motor).set(speed); + } else if (motor instanceof PositionSensorMotor) { + ((PositionSensorMotor) motor).setPosition(position); + } + } + } + + /** + * Checks precision of sensor motor + * Checks voltage input and current output of motors + */ + @Override + public void execute() { + for (SensorMotor motor : motors) { + double input; + try { + input = motor.getMotionController().getInputSafely(); + } + catch (InvalidSensorException e) { + input = 0; + updateStatusFail(motor.getName(), e); + } + if (motor instanceof VelocitySensorMotor) { + ((VelocitySensorMotor) motor).set(speed); + VICheck(motor); + if (((VelocitySensorMotor) motor).getMotionController().onTarget() && input - speed > VELOCITY_THRESHOLD) { + updateStatusFail(motor.getName(), new Exception("SPEED NOT WITHIN REQUIRED THRESHOLD")); + } + } else if (motor instanceof PositionSensorMotor) { + if (!motor.getMotionController().onTarget()) { + VICheck(motor); + } + if (((PositionSensorMotor) motor).getMotionController().onTarget() && input - position > POSITION_THRESHOLD) { + updateStatusFail(motor.getName(), new Exception("POSITION NOT WITHIN THRESHOLD")); + } + } + } + } + + /** + * Checks if motors are onTarget by end of command + */ + @Override + public void end() { + super.end(); + for (SensorMotor motor : motors) { + if (!motor.getMotionController().onTarget()) { + updateStatusFail(motor.getName(), + new Exception(String.format("MOTIONCONTROLLER NOT ON TARGET IN %f", timeout))); + } + } + } +} \ No newline at end of file diff --git a/commands/systemchecks/motor/ServoCheck.java b/commands/systemchecks/motor/ServoCheck.java new file mode 100644 index 00000000..88623fca --- /dev/null +++ b/commands/systemchecks/motor/ServoCheck.java @@ -0,0 +1,78 @@ +package org.usfirst.frc4904.standard.commands.systemchecks.motor; + + +import org.usfirst.frc4904.standard.commands.systemchecks.SubsystemCheck; +import org.usfirst.frc4904.standard.subsystems.motor.ServoSubsystem; + +/** + * Systemcheck on ServoSubsystems + */ +public class ServoCheck extends SubsystemCheck { + protected static final double DEFAULT_ANGLE = 50; + protected final double angle; + protected final ServoSubsystem[] servos; + + /** + * Systemcheck on ServoSubsystems + * + * @param name + * Name of check + * @param angle + * Angle to set servos + * @param servos + * ServoSubsystems to test + */ + public ServoCheck(String name, double angle, ServoSubsystem... servos) { + super(name, servos); + this.servos = servos; + this.angle = angle; + } + + /** + * Systemcheck on ServoSubsystems + * + * @param name + * Name of check + * @param servos + * ServoSubsystems to test + */ + public ServoCheck(String name, ServoSubsystem... servos) { + this(name, DEFAULT_ANGLE, servos); + } + + /** + * Systemcheck on ServoSubsystems + * + * @param angle + * Angle to set servos + * @param servos + * ServoSubsystems to test + */ + public ServoCheck(double angle, ServoSubsystem... servos) { + this("ServoCheck", angle, servos); + } + + /** + * Systemcheck on ServoSubsystems + * + * @param servos + * ServoSubsystems to test + */ + public ServoCheck(ServoSubsystem... servos) { + this("ServoCheck", servos); + } + + /** + * Tests setting of servo angle + */ + public void initialize() { + for (ServoSubsystem servo : servos) { + try { + servo.setAngle(angle); + } + catch (Exception e) { + updateStatusFail(servo.getName(), e); + } + } + } +} \ No newline at end of file diff --git a/commands/systemchecks/motor/VelocityMotorCheck.java b/commands/systemchecks/motor/VelocityMotorCheck.java new file mode 100644 index 00000000..d9c3d9d6 --- /dev/null +++ b/commands/systemchecks/motor/VelocityMotorCheck.java @@ -0,0 +1,73 @@ +package org.usfirst.frc4904.standard.commands.systemchecks.motor; + + +import org.usfirst.frc4904.standard.subsystems.motor.VelocitySensorMotor; + +/** + * Systemcheck of VelocitySensorMotors + */ +public class VelocityMotorCheck extends SensorMotorCheck { + /** + * Systemcheck of VelocitySensorMotors + * + * @param name + * Name of check + * @param timeout + * Duration of check + * @param speed + * Speed to set VelocitySensorMotors + * @param motors + * VelocitySensorMotors to test + */ + public VelocityMotorCheck(String name, double timeout, double speed, VelocitySensorMotor... motors) { + super(name, timeout, speed, 0, motors); + } + + /** + * Systemcheck of VelocitySensorMotors + * + * @param name + * Name of check + * @param speed + * Speed to set VelocitySensorMotors + * @param motors + * VelocitySensorMotors to test + */ + public VelocityMotorCheck(String name, double speed, VelocitySensorMotor... motors) { + super(name, speed, 0, motors); + } + + /** + * Systemcheck of VelocitySensorMotors + * + * @param name + * Name of check + * @param motors + * VelocitySensorMotors to test + */ + public VelocityMotorCheck(String name, VelocitySensorMotor... motors) { + this(name, DEFAULT_SPEED, motors); + } + + /** + * Systemcheck of VelocitySensorMotors + * + * @param speed + * Speed to set VelocitySensorMotors + * @param motors + * VelocitySensorMotors to test + */ + public VelocityMotorCheck(double speed, VelocitySensorMotor... motors) { + this("VelocityMotorCheck", speed, motors); + } + + /** + * Systemcheck of VelocitySensorMotors + * + * @param motors + * VelocitySensorMotors to test + */ + public VelocityMotorCheck(VelocitySensorMotor... motors) { + this("VelocityMotorCheck", motors); + } +} \ No newline at end of file diff --git a/subsystems/motor/Motor.java b/subsystems/motor/Motor.java index 6befacc5..ccb097ba 100644 --- a/subsystems/motor/Motor.java +++ b/subsystems/motor/Motor.java @@ -19,22 +19,22 @@ public class Motor extends Subsystem implements SpeedController { protected final SpeedModifier speedModifier; protected boolean isInverted; protected double lastSpeed; - + /** * A class that wraps around a variable number of SpeedController objects to give them Subsystem functionality. * Can also modify their speed with a SpeedModifier for things like scaling or brownout protection. * * @param name - * The name for the motor + * The name for the motor * @param isInverted - * Inverts the direction of all of the SpeedControllers. - * This does not override the individual inversions of the motors. + * Inverts the direction of all of the SpeedControllers. + * This does not override the individual inversions of the motors. * @param speedModifier - * A SpeedModifier changes the input to every motor based on some factor. - * The default is an IdentityModifier, which does not affect anything. + * A SpeedModifier changes the input to every motor based on some factor. + * The default is an IdentityModifier, which does not affect anything. * @param motors - * The SpeedControllers in this subsystem. - * Can be a single SpeedController or multiple SpeedControllers. + * The SpeedControllers in this subsystem. + * Can be a single SpeedController or multiple SpeedControllers. */ public Motor(String name, boolean isInverted, SpeedModifier speedModifier, SpeedController... motors) { super(name); @@ -48,134 +48,134 @@ public Motor(String name, boolean isInverted, SpeedModifier speedModifier, Speed } setInverted(isInverted); } - + /** * A class that wraps around a variable number of SpeedController objects to give them Subsystem functionality. * Can also modify their speed with a SpeedModifier for things like scaling or brownout protection. * * @param name - * The name for the motor + * The name for the motor * @param isInverted - * Inverts the direction of all of the SpeedControllers. - * This does not override the individual inversions of the motors. + * Inverts the direction of all of the SpeedControllers. + * This does not override the individual inversions of the motors. * @param motors - * The SpeedControllers in this subsystem. - * Can be a single SpeedController or multiple SpeedControllers. + * The SpeedControllers in this subsystem. + * Can be a single SpeedController or multiple SpeedControllers. */ public Motor(String name, boolean isInverted, SpeedController... motors) { this(name, isInverted, new IdentityModifier(), motors); } - + /** * A class that wraps around a variable number of SpeedController objects to give them Subsystem functionality. * Can also modify their speed with a SpeedModifier for things like scaling or brownout protection. * * @param name - * The name for the motor. + * The name for the motor. * @param speedModifier - * A SpeedModifier changes the input to every motor based on some factor. - * The default is an IdentityModifier, which does not affect anything. - * Can also regulate set speed to prevent brownouts (if you use AccelerationCap). + * A SpeedModifier changes the input to every motor based on some factor. + * The default is an IdentityModifier, which does not affect anything. + * Can also regulate set speed to prevent brownouts (if you use AccelerationCap). * @param motors - * The SpeedControllers in this subsystem. - * Can be a single SpeedController or multiple SpeedControllers. + * The SpeedControllers in this subsystem. + * Can be a single SpeedController or multiple SpeedControllers. */ public Motor(String name, SpeedModifier speedModifier, SpeedController... motors) { this(name, false, speedModifier, motors); } - + /** * A class that wraps around a variable number of SpeedController objects to give them Subsystem functionality. * Can also modify their speed with a SpeedModifier for things like scaling or brownout protection. * * @param name - * The name for the motor. + * The name for the motor. * @param motors - * The SpeedControllers in this subsystem. - * Can be a single SpeedController or multiple SpeedControllers. + * The SpeedControllers in this subsystem. + * Can be a single SpeedController or multiple SpeedControllers. */ public Motor(String name, SpeedController... motors) { this(name, false, new IdentityModifier(), motors); } - + /** * A class that wraps around a variable number of SpeedController objects to give them Subsystem functionality. * Can also modify their speed with a SpeedModifier for things like scaling or brownout protection. * * @param isInverted - * Inverts the direction of all of the SpeedControllers. - * This does not override the individual inversions of the motors. + * Inverts the direction of all of the SpeedControllers. + * This does not override the individual inversions of the motors. * @param speedModifier - * A SpeedModifier changes the input to every motor based on some factor. - * The default is an IdentityModifier, which does not affect anything. - * Can also regulate set speed to prevent brownouts (if you use AccelerationCap). + * A SpeedModifier changes the input to every motor based on some factor. + * The default is an IdentityModifier, which does not affect anything. + * Can also regulate set speed to prevent brownouts (if you use AccelerationCap). * @param motors - * The SpeedControllers in this subsystem. - * Can be a single SpeedController or multiple SpeedControllers. + * The SpeedControllers in this subsystem. + * Can be a single SpeedController or multiple SpeedControllers. */ public Motor(boolean isInverted, SpeedModifier speedModifier, SpeedController... motors) { this("Motor", isInverted, speedModifier, motors); } - + /** * A class that wraps around a variable number of SpeedController objects to give them Subsystem functionality. * Can also modify their speed with a SpeedModifier for things like scaling or brownout protection. * * @param name - * The name for the motor. + * The name for the motor. * @param isInverted - * Inverts the direction of all of the SpeedControllers. - * This does not override the individual inversions of the motors. + * Inverts the direction of all of the SpeedControllers. + * This does not override the individual inversions of the motors. * @param speedModifier - * A SpeedModifier changes the input to every motor based on some factor. - * The default is an IdentityModifier, which does not affect anything. - * Can also regulate set speed to prevent brownouts (if you use AccelerationCap). + * A SpeedModifier changes the input to every motor based on some factor. + * The default is an IdentityModifier, which does not affect anything. + * Can also regulate set speed to prevent brownouts (if you use AccelerationCap). * @param motors - * The SpeedControllers in this subsystem. - * Can be a single SpeedController or multiple SpeedControllers. + * The SpeedControllers in this subsystem. + * Can be a single SpeedController or multiple SpeedControllers. */ public Motor(boolean isInverted, SpeedController... motors) { this("Motor", isInverted, motors); } - + /** * A class that wraps around a variable number of SpeedController objects to give them Subsystem functionality. * Can also modify their speed with a SpeedModifier for things like scaling or brownout protection. * * @param speedModifier - * A SpeedModifier changes the input to every motor based on some factor. - * The default is an IdentityModifier, which does not affect anything. - * Can also regulate set speed to prevent brownouts (if you use AccelerationCap). + * A SpeedModifier changes the input to every motor based on some factor. + * The default is an IdentityModifier, which does not affect anything. + * Can also regulate set speed to prevent brownouts (if you use AccelerationCap). * @param motors - * The SpeedControllers in this subsystem. - * Can be a single SpeedController or multiple SpeedControllers. + * The SpeedControllers in this subsystem. + * Can be a single SpeedController or multiple SpeedControllers. */ public Motor(SpeedModifier speedModifier, SpeedController... motors) { this("Motor", speedModifier, motors); } - + /** * A class that wraps around a variable number of SpeedController objects to give them Subsystem functionality. * Can also modify their speed with a SpeedModifier for things like scaling or brownout protection. * * @param motors - * The SpeedControllers in this subsystem. - * Can be a single SpeedController or multiple SpeedControllers. + * The SpeedControllers in this subsystem. + * Can be a single SpeedController or multiple SpeedControllers. */ public Motor(SpeedController... motors) { this("Motor", motors); } - + @Override protected void initDefaultCommand() { setDefaultCommand(new MotorIdle(this)); } - + /** * Get a set value from a PIDController. * * @param speed - * The speed returned by the PID loop. + * The speed returned by the PID loop. */ @Override public void pidWrite(double speed) { @@ -185,7 +185,7 @@ public void pidWrite(double speed) { motor.pidWrite(newSpeed); } } - + /** * Disables the motor. * This function uses the underlying SpeedController's disable implementation. @@ -196,7 +196,17 @@ public void disable() { motor.disable(); } } - + + /** + * Returns speedcontrollers of motor + * + * @return motors + * Speedcontrollers of motor + */ + public SpeedController[] getSpeedControllers() { + return motors; + } + /** * Stops the motor. * This function uses the underlying SpeedController's stopMotor implementation. @@ -209,7 +219,7 @@ public void stopMotor() { motor.stopMotor(); } } - + /** * Get the most recently set speed. * @@ -219,12 +229,12 @@ public void stopMotor() { public double get() { return lastSpeed; } - + /** * Set the motor speed. Passes through SpeedModifier. * * @param speed - * The speed to set. Value should be between -1.0 and 1.0. + * The speed to set. Value should be between -1.0 and 1.0. */ @Override public void set(double speed) { @@ -235,7 +245,7 @@ public void set(double speed) { motor.set(newSpeed); } } - + /** * Get whether this entire motor is inverted. * @@ -246,14 +256,14 @@ public void set(double speed) { public boolean getInverted() { return isInverted; } - + /** * Sets the direction inversion of all motor substituents. * This respects the original inversion state of each SpeedController when constructed, * and will only invert SpeedControllers if this.getInverted() != the input. * * @param isInverted - * The state of inversion, true is inverted. + * The state of inversion, true is inverted. */ @Override public void setInverted(boolean isInverted) { @@ -264,21 +274,22 @@ public void setInverted(boolean isInverted) { } this.isInverted = isInverted; } - + protected class UnsynchronizedSpeedControllerRuntimeException extends RuntimeException { private static final long serialVersionUID = 8688590919561059584L; - + public UnsynchronizedSpeedControllerRuntimeException() { super(getName() + "'s SpeedControllers report different speeds"); } } - + @Deprecated protected class StrangeCANSpeedControllerModeRuntimeException extends RuntimeException { private static final long serialVersionUID = -539917227288371271L; - + public StrangeCANSpeedControllerModeRuntimeException() { - super("One of " + getName() + "'s SpeedControllers is a CANSpeedController with a non-zero mode. This might mess up it's .get(), so Motor cannot verify safety."); + super("One of " + getName() + + "'s SpeedControllers is a CANSpeedController with a non-zero mode. This might mess up it's .get(), so Motor cannot verify safety."); } } } diff --git a/subsystems/motor/PositionSensorMotor.java b/subsystems/motor/PositionSensorMotor.java index 6a513174..b733641e 100644 --- a/subsystems/motor/PositionSensorMotor.java +++ b/subsystems/motor/PositionSensorMotor.java @@ -1,15 +1,39 @@ package org.usfirst.frc4904.standard.subsystems.motor; +import org.usfirst.frc4904.standard.Util; import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; import edu.wpi.first.wpilibj.SpeedController; public class PositionSensorMotor extends SensorMotor { + protected Util.Range positionRange; + public PositionSensorMotor(String name, boolean isInverted, SpeedModifier speedModifier, MotionController motionController, SpeedController... motors) { super(name, isInverted, speedModifier, motionController, motors); + this.positionRange = null; + } + + public PositionSensorMotor(String name, Util.Range positionRange, boolean isInverted, SpeedModifier speedModifier, + MotionController motionController, + SpeedController... motors) { + super(name, isInverted, speedModifier, motionController, motors); + this.positionRange = positionRange; + } + + public PositionSensorMotor(String name, Util.Range positionRange, SpeedModifier speedModifier, + MotionController motionController, + SpeedController... motors) { + super(name, false, speedModifier, motionController, motors); + this.positionRange = positionRange; + } + + public PositionSensorMotor(String name, Util.Range positionRange, MotionController motionController, + SpeedController... motors) { + super(name, false, new IdentityModifier(), motionController, motors); + this.positionRange = positionRange; } public PositionSensorMotor(String name, boolean isInverted, MotionController motionController, SpeedController... motors) { @@ -41,8 +65,8 @@ public PositionSensorMotor(SpeedModifier speedModifier, MotionController motionC public PositionSensorMotor(MotionController motionController, SpeedController... motors) { this("PositionSensorMotor", motionController, motors); } - + public void setPosition(double position) { - motionController.setSetpoint(position); + motionController.setSetpoint(positionRange != null ? positionRange.limitValue(position) : position); } } diff --git a/subsystems/motor/SensorMotor.java b/subsystems/motor/SensorMotor.java index f1117e40..fe44869a 100644 --- a/subsystems/motor/SensorMotor.java +++ b/subsystems/motor/SensorMotor.java @@ -47,6 +47,9 @@ public SensorMotor(MotionController motionController, SpeedController... motors) this("SensorMotor", motionController, motors); } + public MotionController getMotionController() { + return motionController; + } public void reset() throws InvalidSensorException { motionController.reset(); }