From 6e2ec77209702da3b93a0d651b84b6371b600d06 Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Mon, 21 Jan 2019 10:53:47 -0800 Subject: [PATCH 01/23] init commit --- commands/systemchecks/MotorCheck.java | 37 +++++++++++++++++++++++++ commands/systemchecks/SystemStatus.java | 5 ++++ 2 files changed, 42 insertions(+) create mode 100644 commands/systemchecks/MotorCheck.java create mode 100644 commands/systemchecks/SystemStatus.java diff --git a/commands/systemchecks/MotorCheck.java b/commands/systemchecks/MotorCheck.java new file mode 100644 index 00000000..328bf556 --- /dev/null +++ b/commands/systemchecks/MotorCheck.java @@ -0,0 +1,37 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + +import org.usfirst.frc4904.standard.Util; +import org.usfirst.frc4904.standard.subsystems.motor.Motor; +import edu.wpi.first.wpilibj.command.Command; + +public class MotorCheck extends Command { + protected final Motor[] motors; + protected static final double SET_SPEED = 0.5; + protected static final Util.Range outCurrentRange = new Util.Range(0.1, 0.3); // TODO: Adjust this range + // protected + + public MotorCheck(Motor... motors) { + this.motors = motors; + } + + public void initialize() { + for (Motor motor: motors){ + motor.set(SET_SPEED); + } + } + + public + + public void execute() { + for (Motor motor: motors){ + try { + motor.set(SET_SPEED); + } catch (Exception e) { + //TODO: handle exception + } + } + } + public boolean isFinished(){ + return false; + } +} \ No newline at end of file diff --git a/commands/systemchecks/SystemStatus.java b/commands/systemchecks/SystemStatus.java new file mode 100644 index 00000000..ea9afe13 --- /dev/null +++ b/commands/systemchecks/SystemStatus.java @@ -0,0 +1,5 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + +public enum SystemStatus { + PASS, FAIL; +} \ No newline at end of file From c74bbb4a0a3041f8ac0c776e9b0543c3bef0fd27 Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Mon, 21 Jan 2019 13:35:13 -0800 Subject: [PATCH 02/23] more changes --- commands/systemchecks/MotorCheck.java | 44 +++++++++++++++++++++--- commands/systemchecks/StatusMessage.java | 15 ++++++++ commands/systemchecks/SystemStatus.java | 5 --- 3 files changed, 55 insertions(+), 9 deletions(-) create mode 100644 commands/systemchecks/StatusMessage.java delete mode 100644 commands/systemchecks/SystemStatus.java diff --git a/commands/systemchecks/MotorCheck.java b/commands/systemchecks/MotorCheck.java index 328bf556..1ec6fac3 100644 --- a/commands/systemchecks/MotorCheck.java +++ b/commands/systemchecks/MotorCheck.java @@ -1,16 +1,27 @@ package org.usfirst.frc4904.standard.commands.systemchecks; +import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.Util; import org.usfirst.frc4904.standard.subsystems.motor.Motor; +import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.command.Command; +import java.util.Set; +import java.util.HashMap; +import java.util.Iterator; public class MotorCheck extends Command { protected final Motor[] motors; protected static final double SET_SPEED = 0.5; - protected static final Util.Range outCurrentRange = new Util.Range(0.1, 0.3); // TODO: Adjust this range - // protected + protected static final Util.Range outputCurrentRange = new Util.Range(0.1, 0.3); // TODO: Use Current to judge speedcontrollers + protected HashMap statuses; + + public MotorCheck(String name, Motor... motors) { + super(name); + this.motors = motors; + } public MotorCheck(Motor... motors) { + super("MotorCheck"); this.motors = motors; } @@ -20,14 +31,39 @@ public void initialize() { } } - public + public void initStatuses() { + for (Motor motor : motors) { + statuses.put(motor.getName(), new StatusMessage(StatusMessage.SystemStatus.PASS, "")); + } + } + + public void updateStatus(String key, StatusMessage.SystemStatus status, String errorMessage) { + statuses.put(key, new StatusMessage(status, errorMessage)); + } + + public StatusMessage getStatusMessage(String key) { + return statuses.get(key); + } + + public StatusMessage.SystemStatus getStatus(String key) { + return getStatusMessage(key).status; + } + + public String getError(String key) { + return getStatusMessage(key).errorMessage; + } + + public void outputStatuses() { + LogKitten.wtf(getName() + " Statuses:"); + Set set = statuses.entrySet(); + } public void execute() { for (Motor motor: motors){ try { motor.set(SET_SPEED); } catch (Exception e) { - //TODO: handle exception + updateStatus(motor.getName(), StatusMessage.SystemStatus.FAIL, e.getMessage()); } } } diff --git a/commands/systemchecks/StatusMessage.java b/commands/systemchecks/StatusMessage.java new file mode 100644 index 00000000..8dfad7df --- /dev/null +++ b/commands/systemchecks/StatusMessage.java @@ -0,0 +1,15 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + +public class StatusMessage { + protected SystemStatus status; + protected String errorMessage; + + public StatusMessage(SystemStatus status, String errorMessage) { + this.status = status; + this.errorMessage = errorMessage; + } + + public enum SystemStatus { + PASS, FAIL; + } +} \ No newline at end of file diff --git a/commands/systemchecks/SystemStatus.java b/commands/systemchecks/SystemStatus.java deleted file mode 100644 index ea9afe13..00000000 --- a/commands/systemchecks/SystemStatus.java +++ /dev/null @@ -1,5 +0,0 @@ -package org.usfirst.frc4904.standard.commands.systemchecks; - -public enum SystemStatus { - PASS, FAIL; -} \ No newline at end of file From 2104781661ea8e9a325ec73ca50587cb75b723a2 Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Mon, 21 Jan 2019 14:57:18 -0800 Subject: [PATCH 03/23] added framework for Subsystem checks --- commands/systemchecks/MotorCheck.java | 44 ++---------------- commands/systemchecks/SubsystemCheck.java | 56 +++++++++++++++++++++++ 2 files changed, 61 insertions(+), 39 deletions(-) create mode 100644 commands/systemchecks/SubsystemCheck.java diff --git a/commands/systemchecks/MotorCheck.java b/commands/systemchecks/MotorCheck.java index 1ec6fac3..aadabf6b 100644 --- a/commands/systemchecks/MotorCheck.java +++ b/commands/systemchecks/MotorCheck.java @@ -1,28 +1,21 @@ package org.usfirst.frc4904.standard.commands.systemchecks; -import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.Util; import org.usfirst.frc4904.standard.subsystems.motor.Motor; -import edu.wpi.first.wpilibj.SpeedController; -import edu.wpi.first.wpilibj.command.Command; -import java.util.Set; -import java.util.HashMap; -import java.util.Iterator; -public class MotorCheck extends Command { - protected final Motor[] motors; +public class MotorCheck extends SubsystemCheck { protected static final double SET_SPEED = 0.5; protected static final Util.Range outputCurrentRange = new Util.Range(0.1, 0.3); // TODO: Use Current to judge speedcontrollers - protected HashMap statuses; + protected final Motor[] motors; public MotorCheck(String name, Motor... motors) { - super(name); + super(name, motors); this.motors = motors; + } public MotorCheck(Motor... motors) { - super("MotorCheck"); - this.motors = motors; + this("MotorCheck", motors); } public void initialize() { @@ -31,33 +24,6 @@ public void initialize() { } } - public void initStatuses() { - for (Motor motor : motors) { - statuses.put(motor.getName(), new StatusMessage(StatusMessage.SystemStatus.PASS, "")); - } - } - - public void updateStatus(String key, StatusMessage.SystemStatus status, String errorMessage) { - statuses.put(key, new StatusMessage(status, errorMessage)); - } - - public StatusMessage getStatusMessage(String key) { - return statuses.get(key); - } - - public StatusMessage.SystemStatus getStatus(String key) { - return getStatusMessage(key).status; - } - - public String getError(String key) { - return getStatusMessage(key).errorMessage; - } - - public void outputStatuses() { - LogKitten.wtf(getName() + " Statuses:"); - Set set = statuses.entrySet(); - } - public void execute() { for (Motor motor: motors){ try { diff --git a/commands/systemchecks/SubsystemCheck.java b/commands/systemchecks/SubsystemCheck.java new file mode 100644 index 00000000..dbc3dd96 --- /dev/null +++ b/commands/systemchecks/SubsystemCheck.java @@ -0,0 +1,56 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + +import java.util.HashMap; +import java.util.Map; +import org.usfirst.frc4904.standard.LogKitten; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.Subsystem; + +public abstract class SubsystemCheck extends Command { + protected HashMap statuses; + protected Subsystem[] systems; + + public SubsystemCheck (String name, Subsystem... systems) { + super(name); + this.systems = systems; + for (Subsystem system : systems) { + requires(system); + } + } + + public SubsystemCheck (Subsystem... systems) { + this("SystemCheck", systems); + } + + public void initStatuses() { + for (Subsystem system : systems) { + statuses.put(system.getName(), new StatusMessage(StatusMessage.SystemStatus.PASS, "")); + } + } + + public void updateStatus(String key, StatusMessage.SystemStatus status, String errorMessage) { + statuses.put(key, new StatusMessage(status, errorMessage)); + } + public StatusMessage getStatusMessage(String key) { + return statuses.get(key); + } + + public StatusMessage.SystemStatus getStatus(String key) { + return getStatusMessage(key).status; + } + + public String getError(String key) { + return getStatusMessage(key).errorMessage; + } + + public void outputStatuses() { + LogKitten.wtf(getName() + " Statuses:"); + for (Map.Entry entry : statuses.entrySet()) { + String name = entry.getKey(); + StatusMessage message = entry.getValue(); + LogKitten.wtf("Subsystem: " + name + ", Status: " + (message.status == StatusMessage.SystemStatus.PASS ? "PASS" : "FAIL") + ", ERROR: " + message.errorMessage); + } + } + + +} \ No newline at end of file From 606649c8b0529cd1d52ad399f71dcce7202e9c08 Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Mon, 21 Jan 2019 16:16:32 -0800 Subject: [PATCH 04/23] added solenoid check and lower level system check --- commands/systemchecks/SolenoidCheck.java | 31 ++++++++++++++ commands/systemchecks/SubsystemCheck.java | 42 ++----------------- commands/systemchecks/SystemCheck.java | 51 +++++++++++++++++++++++ 3 files changed, 86 insertions(+), 38 deletions(-) create mode 100644 commands/systemchecks/SolenoidCheck.java create mode 100644 commands/systemchecks/SystemCheck.java diff --git a/commands/systemchecks/SolenoidCheck.java b/commands/systemchecks/SolenoidCheck.java new file mode 100644 index 00000000..7f6c464a --- /dev/null +++ b/commands/systemchecks/SolenoidCheck.java @@ -0,0 +1,31 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + +import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.command.Subsystem; + +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.getMessage()); + } + } + } + public boolean isFinished(){ + return false; + } +} \ No newline at end of file diff --git a/commands/systemchecks/SubsystemCheck.java b/commands/systemchecks/SubsystemCheck.java index dbc3dd96..bf6dd8e8 100644 --- a/commands/systemchecks/SubsystemCheck.java +++ b/commands/systemchecks/SubsystemCheck.java @@ -6,14 +6,12 @@ import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Subsystem; -public abstract class SubsystemCheck extends Command { +public abstract class SubsystemCheck extends SystemCheck { protected HashMap statuses; - protected Subsystem[] systems; - public SubsystemCheck (String name, Subsystem... systems) { - super(name); - this.systems = systems; - for (Subsystem system : systems) { + public SubsystemCheck (String name, Subsystem... subsystems) { + super(name, subsystems); + for (Subsystem system : subsystems) { requires(system); } } @@ -21,36 +19,4 @@ public SubsystemCheck (String name, Subsystem... systems) { public SubsystemCheck (Subsystem... systems) { this("SystemCheck", systems); } - - public void initStatuses() { - for (Subsystem system : systems) { - statuses.put(system.getName(), new StatusMessage(StatusMessage.SystemStatus.PASS, "")); - } - } - - public void updateStatus(String key, StatusMessage.SystemStatus status, String errorMessage) { - statuses.put(key, new StatusMessage(status, errorMessage)); - } - public StatusMessage getStatusMessage(String key) { - return statuses.get(key); - } - - public StatusMessage.SystemStatus getStatus(String key) { - return getStatusMessage(key).status; - } - - public String getError(String key) { - return getStatusMessage(key).errorMessage; - } - - public void outputStatuses() { - LogKitten.wtf(getName() + " Statuses:"); - for (Map.Entry entry : statuses.entrySet()) { - String name = entry.getKey(); - StatusMessage message = entry.getValue(); - LogKitten.wtf("Subsystem: " + name + ", Status: " + (message.status == StatusMessage.SystemStatus.PASS ? "PASS" : "FAIL") + ", ERROR: " + message.errorMessage); - } - } - - } \ No newline at end of file diff --git a/commands/systemchecks/SystemCheck.java b/commands/systemchecks/SystemCheck.java new file mode 100644 index 00000000..386d6e19 --- /dev/null +++ b/commands/systemchecks/SystemCheck.java @@ -0,0 +1,51 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + +import java.util.HashMap; +import java.util.Map; +import org.usfirst.frc4904.standard.LogKitten; +import edu.wpi.first.wpilibj.SendableBase; +import edu.wpi.first.wpilibj.command.Command; + +public abstract class SystemCheck extends Command { + protected HashMap statuses; + protected SendableBase[] systems; + + public SystemCheck(String name, SendableBase... systems) { + super(name); + this.systems = systems; + } + + public SystemCheck(SendableBase... systems) { + this("SystemCheck", systems); + } + + public void initStatuses() { + for (SendableBase system : systems) { + statuses.put(system.getName(), new StatusMessage(StatusMessage.SystemStatus.PASS, "")); + } + } + + public void updateStatus(String key, StatusMessage.SystemStatus status, String errorMessage) { + statuses.put(key, new StatusMessage(status, errorMessage)); + } + public StatusMessage getStatusMessage(String key) { + return statuses.get(key); + } + + public StatusMessage.SystemStatus getStatus(String key) { + return getStatusMessage(key).status; + } + + public String getError(String key) { + return getStatusMessage(key).errorMessage; + } + + public void outputStatuses() { + LogKitten.wtf(getName() + " Statuses:"); + for (Map.Entry entry : statuses.entrySet()) { + String name = entry.getKey(); + StatusMessage message = entry.getValue(); + LogKitten.wtf("Subsystem: " + name + ", Status: " + (message.status == StatusMessage.SystemStatus.PASS ? "PASS" : "FAIL") + ", ERROR: " + message.errorMessage); + } + } +} \ No newline at end of file From 8365f8ca7371230cf599c8fe1084b3f7fa8fd5fe Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Mon, 21 Jan 2019 18:34:27 -0800 Subject: [PATCH 05/23] added compressor check --- commands/systemchecks/CompressorCheck.java | 36 ++++++++++++++++++++++ commands/systemchecks/MotorCheck.java | 3 -- commands/systemchecks/SolenoidCheck.java | 3 -- commands/systemchecks/SystemCheck.java | 10 +++++- 4 files changed, 45 insertions(+), 7 deletions(-) create mode 100644 commands/systemchecks/CompressorCheck.java diff --git a/commands/systemchecks/CompressorCheck.java b/commands/systemchecks/CompressorCheck.java new file mode 100644 index 00000000..a5f83366 --- /dev/null +++ b/commands/systemchecks/CompressorCheck.java @@ -0,0 +1,36 @@ +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, Compressor... compressors) { + super("CompressorCheck", compressors); + this.compressors = compressors; + } + + public void execute() { + for (Compressor compressor : compressors) { + if (compressor.getCompressorNotConnectedFault() || compressor.getCompressorNotConnectedStickyFault() || !compressor.enabled()){ + updateStatus(compressor.getName(), SystemStatus.FAIL, "COMPRESSOR NOT CONNECTED"); + + } + else if (compressor.getCompressorShortedFault() || compressor.getCompressorShortedStickyFault()) { + updateStatus(compressor.getName(), SystemStatus.FAIL, "COMPRESSOR SHORTED"); + } + + else if (compressor.getCompressorCurrentTooHighFault() || compressor.getCompressorCurrentTooHighStickyFault()) { + updateStatus(compressor.getName(), SystemStatus.FAIL, "CURRENT TOO HIGH"); + } + else if (compressor.getPressureSwitchValue()){ //TODO: Check if this works as intended + updateStatus(compressor.getName(), SystemStatus.FAIL, "LOW PRESSURE"); + } + + + + } + } + +} \ No newline at end of file diff --git a/commands/systemchecks/MotorCheck.java b/commands/systemchecks/MotorCheck.java index aadabf6b..d4f1f0da 100644 --- a/commands/systemchecks/MotorCheck.java +++ b/commands/systemchecks/MotorCheck.java @@ -33,7 +33,4 @@ public void execute() { } } } - public boolean isFinished(){ - return false; - } } \ No newline at end of file diff --git a/commands/systemchecks/SolenoidCheck.java b/commands/systemchecks/SolenoidCheck.java index 7f6c464a..aebc7e97 100644 --- a/commands/systemchecks/SolenoidCheck.java +++ b/commands/systemchecks/SolenoidCheck.java @@ -25,7 +25,4 @@ public void initialize() { } } } - public boolean isFinished(){ - return false; - } } \ No newline at end of file diff --git a/commands/systemchecks/SystemCheck.java b/commands/systemchecks/SystemCheck.java index 386d6e19..9ecd3b47 100644 --- a/commands/systemchecks/SystemCheck.java +++ b/commands/systemchecks/SystemCheck.java @@ -19,15 +19,22 @@ public SystemCheck(SendableBase... systems) { this("SystemCheck", systems); } + public boolean isFinished() { + return true; + } public void initStatuses() { for (SendableBase system : systems) { statuses.put(system.getName(), new StatusMessage(StatusMessage.SystemStatus.PASS, "")); } } - public void updateStatus(String key, StatusMessage.SystemStatus status, String errorMessage) { + public void reassignStatus(String key, StatusMessage.SystemStatus status, String errorMessage) { statuses.put(key, new StatusMessage(status, errorMessage)); } + + public void updateStatus(String key, StatusMessage.SystemStatus status, String errorMessage) { + reassignStatus(key, status, statuses.get(key).errorMessage + errorMessage); + } public StatusMessage getStatusMessage(String key) { return statuses.get(key); } @@ -45,6 +52,7 @@ public void outputStatuses() { for (Map.Entry 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.errorMessage); } } From 301071ca1e458b364b23590ae73a0e4aaf5fc5a6 Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Thu, 24 Jan 2019 18:55:09 -0800 Subject: [PATCH 06/23] finished position and velocity motor checks --- commands/systemchecks/MotorCheck.java | 17 ++++++-- commands/systemchecks/PositionMotorCheck.java | 38 ++++++++++++++++++ commands/systemchecks/StatusMessage.java | 5 +++ commands/systemchecks/VelocityMotorCheck.java | 40 +++++++++++++++++++ subsystems/motor/PositionSensorMotor.java | 23 ++++++++++- 5 files changed, 118 insertions(+), 5 deletions(-) create mode 100644 commands/systemchecks/PositionMotorCheck.java create mode 100644 commands/systemchecks/VelocityMotorCheck.java diff --git a/commands/systemchecks/MotorCheck.java b/commands/systemchecks/MotorCheck.java index d4f1f0da..bcc26678 100644 --- a/commands/systemchecks/MotorCheck.java +++ b/commands/systemchecks/MotorCheck.java @@ -4,15 +4,24 @@ import org.usfirst.frc4904.standard.subsystems.motor.Motor; public class MotorCheck extends SubsystemCheck { - protected static final double SET_SPEED = 0.5; + protected static final double DEFAULT_SPEED = 0.5; // TODO: CHECK THIS + protected final double speed; protected static final Util.Range outputCurrentRange = new Util.Range(0.1, 0.3); // TODO: Use Current to judge speedcontrollers protected final Motor[] motors; - public MotorCheck(String name, Motor... motors) { + public MotorCheck(String name, double speed, Motor... motors) { super(name, motors); this.motors = motors; + this.speed = speed; } + public MotorCheck(double speed, Motor... motors) { + this("MotorCheck", speed, motors); + } + + public MotorCheck(String name, Motor... motors) { + this(name, DEFAULT_SPEED, motors); + } public MotorCheck(Motor... motors) { this("MotorCheck", motors); @@ -20,14 +29,14 @@ public MotorCheck(Motor... motors) { public void initialize() { for (Motor motor: motors){ - motor.set(SET_SPEED); + motor.set(speed); } } public void execute() { for (Motor motor: motors){ try { - motor.set(SET_SPEED); + motor.set(speed); } catch (Exception e) { updateStatus(motor.getName(), StatusMessage.SystemStatus.FAIL, e.getMessage()); } diff --git a/commands/systemchecks/PositionMotorCheck.java b/commands/systemchecks/PositionMotorCheck.java new file mode 100644 index 00000000..2f099984 --- /dev/null +++ b/commands/systemchecks/PositionMotorCheck.java @@ -0,0 +1,38 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + +import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; +import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor; + +public class PositionMotorCheck extends SubsystemCheck { + protected final PositionSensorMotor[] positionMotors; + protected final double setPosition; + + public PositionMotorCheck(String name, double setPosition, PositionSensorMotor... positionMotors) { + super(name, positionMotors); + this.positionMotors = positionMotors; + this.setPosition = setPosition; + } + + public PositionMotorCheck(String name, PositionSensorMotor... positionMotors) { + this(name, 50, positionMotors); + } + + public PositionMotorCheck(double setPosition, PositionSensorMotor... positionMotors) { + this("PositionSensorMotorCheck", setPosition, positionMotors); + } + + public PositionMotorCheck(PositionSensorMotor... positionMotors) { + this("PositionSensorMotorCheck", positionMotors); + } + + @Override + public void initialize() { + for (PositionSensorMotor motor : positionMotors) { + try { + motor.setPosition(setPosition); + } catch (Exception e) { + updateStatus(motor.getName(), SystemStatus.FAIL, e.getMessage()); + } + } + } +} \ No newline at end of file diff --git a/commands/systemchecks/StatusMessage.java b/commands/systemchecks/StatusMessage.java index 8dfad7df..4932e99b 100644 --- a/commands/systemchecks/StatusMessage.java +++ b/commands/systemchecks/StatusMessage.java @@ -1,8 +1,13 @@ package org.usfirst.frc4904.standard.commands.systemchecks; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + + public class StatusMessage { protected SystemStatus status; protected String errorMessage; + // protected Exception e; // TODO: incorporate Exceptions public StatusMessage(SystemStatus status, String errorMessage) { this.status = status; diff --git a/commands/systemchecks/VelocityMotorCheck.java b/commands/systemchecks/VelocityMotorCheck.java new file mode 100644 index 00000000..4b488be8 --- /dev/null +++ b/commands/systemchecks/VelocityMotorCheck.java @@ -0,0 +1,40 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + +import org.usfirst.frc4904.standard.subsystems.motor.VelocitySensorMotor; + +public class VelocityMotorCheck extends MotorCheck { + protected final VelocitySensorMotor[] velocityMotors; + + public VelocityMotorCheck(String name, double speed, VelocitySensorMotor... velocityMotors) { + super(name, speed, velocityMotors); + this.velocityMotors = velocityMotors; + } + + public VelocityMotorCheck(String name, VelocitySensorMotor... velocityMotors) { + this(name, DEFAULT_SPEED, velocityMotors); + } + + public VelocityMotorCheck(double speed, VelocitySensorMotor... velocityMotors) { + this("VelocityMotorCheck", speed, velocityMotors); + } + + public VelocityMotorCheck(VelocitySensorMotor... velocityMotors) { + this("VelocityMotorCheck", velocityMotors); + } + + public void initialize() { + for (VelocitySensorMotor motor: velocityMotors){ + motor.set(speed); + } + } + + public void execute() { + for (VelocitySensorMotor motor: velocityMotors){ + try { + motor.set(speed); + } catch (Exception e) { + updateStatus(motor.getName(), StatusMessage.SystemStatus.FAIL, e.getMessage()); + } + } + } +} \ No newline at end of file diff --git a/subsystems/motor/PositionSensorMotor.java b/subsystems/motor/PositionSensorMotor.java index 6a513174..3c391049 100644 --- a/subsystems/motor/PositionSensorMotor.java +++ b/subsystems/motor/PositionSensorMotor.java @@ -1,15 +1,35 @@ 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) { @@ -43,6 +63,7 @@ public PositionSensorMotor(MotionController motionController, SpeedController... } public void setPosition(double position) { - motionController.setSetpoint(position); + double newPosition = positionRange != null ? positionRange.limitValue(position) : position; + motionController.setSetpoint(newPosition); } } From 4e2dce2baba2b11a9c7731b4b440081e4267c229 Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Sun, 27 Jan 2019 14:21:25 -0800 Subject: [PATCH 07/23] motor checks, check framework --- commands/systemchecks/BasicCheck.java | 75 +++++++++++++++++++ commands/systemchecks/Check.java | 26 +++++++ commands/systemchecks/CheckGroup.java | 11 +++ commands/systemchecks/SolenoidCheck.java | 1 - commands/systemchecks/SubsystemCheck.java | 6 -- commands/systemchecks/SystemCheck.java | 55 ++++---------- .../systemchecks/{ => motor}/MotorCheck.java | 6 +- .../{ => motor}/PositionMotorCheck.java | 10 ++- commands/systemchecks/motor/ServoCheck.java | 39 ++++++++++ .../{ => motor}/VelocityMotorCheck.java | 21 ++++-- .../motioncontrollers/MotionController.java | 4 + subsystems/motor/SensorMotor.java | 3 + 12 files changed, 202 insertions(+), 55 deletions(-) create mode 100644 commands/systemchecks/BasicCheck.java create mode 100644 commands/systemchecks/Check.java create mode 100644 commands/systemchecks/CheckGroup.java rename commands/systemchecks/{ => motor}/MotorCheck.java (79%) rename commands/systemchecks/{ => motor}/PositionMotorCheck.java (69%) create mode 100644 commands/systemchecks/motor/ServoCheck.java rename commands/systemchecks/{ => motor}/VelocityMotorCheck.java (51%) diff --git a/commands/systemchecks/BasicCheck.java b/commands/systemchecks/BasicCheck.java new file mode 100644 index 00000000..f1794435 --- /dev/null +++ b/commands/systemchecks/BasicCheck.java @@ -0,0 +1,75 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + +import java.util.HashMap; +import java.util.Map; +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 { + protected HashMap 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, String errorMessage) { + statuses.put(name, new StatusMessage(status, errorMessage)); + } + + public void initStatus(String name) { + setStatus(name, SystemStatus.PASS, ""); + } + public void initStatuses() { + for (String name : systemNames) { + initStatus(name); + } + } + + public void updateStatus(String key, StatusMessage.SystemStatus status, String errorMessage) { + setStatus(key, status, statuses.get(key).errorMessage + errorMessage); + } + public StatusMessage getStatusMessage(String key) { + return statuses.get(key); + } + + public SystemStatus getStatus(String key) { + return getStatusMessage(key).status; + } + + public String getError(String key) { + return getStatusMessage(key).errorMessage; + } + + public void outputStatuses() { + LogKitten.wtf(getName() + " Statuses:"); + for (Map.Entry 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.errorMessage); + } + } +} \ No newline at end of file diff --git a/commands/systemchecks/Check.java b/commands/systemchecks/Check.java new file mode 100644 index 00000000..1cfc00e4 --- /dev/null +++ b/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, String errorMessage); + + default void initStatus(String name) { + setStatus(name, SystemStatus.PASS, ""); + } + + public void initStatuses(); + + public void updateStatus(String key, StatusMessage.SystemStatus status, String errorMessage); + + default void setStatusFail(String key, String errorMessage) { + updateStatus(key, SystemStatus.FAIL, errorMessage); + } + + default void setStatusPass(String key, String errorMessage) { + updateStatus(key, SystemStatus.PASS, errorMessage); + } + + 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..c5ad8a23 --- /dev/null +++ b/commands/systemchecks/CheckGroup.java @@ -0,0 +1,11 @@ +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) { + addParallel(check); + } + } +} \ No newline at end of file diff --git a/commands/systemchecks/SolenoidCheck.java b/commands/systemchecks/SolenoidCheck.java index aebc7e97..0a2e4489 100644 --- a/commands/systemchecks/SolenoidCheck.java +++ b/commands/systemchecks/SolenoidCheck.java @@ -2,7 +2,6 @@ import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.command.Subsystem; public class SolenoidCheck extends SystemCheck { protected final DoubleSolenoid[] solenoids; diff --git a/commands/systemchecks/SubsystemCheck.java b/commands/systemchecks/SubsystemCheck.java index bf6dd8e8..ceb6e4d2 100644 --- a/commands/systemchecks/SubsystemCheck.java +++ b/commands/systemchecks/SubsystemCheck.java @@ -1,14 +1,8 @@ package org.usfirst.frc4904.standard.commands.systemchecks; -import java.util.HashMap; -import java.util.Map; -import org.usfirst.frc4904.standard.LogKitten; -import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Subsystem; public abstract class SubsystemCheck extends SystemCheck { - protected HashMap statuses; - public SubsystemCheck (String name, Subsystem... subsystems) { super(name, subsystems); for (Subsystem system : subsystems) { diff --git a/commands/systemchecks/SystemCheck.java b/commands/systemchecks/SystemCheck.java index 9ecd3b47..8347641f 100644 --- a/commands/systemchecks/SystemCheck.java +++ b/commands/systemchecks/SystemCheck.java @@ -1,59 +1,36 @@ package org.usfirst.frc4904.standard.commands.systemchecks; +import java.util.Arrays; import java.util.HashMap; -import java.util.Map; -import org.usfirst.frc4904.standard.LogKitten; +import java.util.stream.Collectors; +import edu.wpi.first.wpilibj.Sendable; import edu.wpi.first.wpilibj.SendableBase; -import edu.wpi.first.wpilibj.command.Command; -public abstract class SystemCheck extends Command { +public abstract class SystemCheck extends BasicCheck { protected HashMap statuses; protected SendableBase[] systems; - public SystemCheck(String name, SendableBase... systems) { - super(name); + 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); } - public boolean isFinished() { - return true; - } + @Override public void initStatuses() { for (SendableBase system : systems) { - statuses.put(system.getName(), new StatusMessage(StatusMessage.SystemStatus.PASS, "")); - } - } - - public void reassignStatus(String key, StatusMessage.SystemStatus status, String errorMessage) { - statuses.put(key, new StatusMessage(status, errorMessage)); - } - - public void updateStatus(String key, StatusMessage.SystemStatus status, String errorMessage) { - reassignStatus(key, status, statuses.get(key).errorMessage + errorMessage); - } - public StatusMessage getStatusMessage(String key) { - return statuses.get(key); - } - - public StatusMessage.SystemStatus getStatus(String key) { - return getStatusMessage(key).status; - } - - public String getError(String key) { - return getStatusMessage(key).errorMessage; - } - - public void outputStatuses() { - LogKitten.wtf(getName() + " Statuses:"); - for (Map.Entry 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.errorMessage); + initStatus(system.getName()); } } } \ No newline at end of file diff --git a/commands/systemchecks/MotorCheck.java b/commands/systemchecks/motor/MotorCheck.java similarity index 79% rename from commands/systemchecks/MotorCheck.java rename to commands/systemchecks/motor/MotorCheck.java index bcc26678..ff1c55f5 100644 --- a/commands/systemchecks/MotorCheck.java +++ b/commands/systemchecks/motor/MotorCheck.java @@ -1,7 +1,9 @@ -package org.usfirst.frc4904.standard.commands.systemchecks; +package org.usfirst.frc4904.standard.commands.systemchecks.motor; import org.usfirst.frc4904.standard.Util; +import org.usfirst.frc4904.standard.commands.systemchecks.SubsystemCheck; import org.usfirst.frc4904.standard.subsystems.motor.Motor; +import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; public class MotorCheck extends SubsystemCheck { protected static final double DEFAULT_SPEED = 0.5; // TODO: CHECK THIS @@ -38,7 +40,7 @@ public void execute() { try { motor.set(speed); } catch (Exception e) { - updateStatus(motor.getName(), StatusMessage.SystemStatus.FAIL, e.getMessage()); + updateStatus(motor.getName(), SystemStatus.FAIL, e.getMessage()); } } } diff --git a/commands/systemchecks/PositionMotorCheck.java b/commands/systemchecks/motor/PositionMotorCheck.java similarity index 69% rename from commands/systemchecks/PositionMotorCheck.java rename to commands/systemchecks/motor/PositionMotorCheck.java index 2f099984..015e2f3c 100644 --- a/commands/systemchecks/PositionMotorCheck.java +++ b/commands/systemchecks/motor/PositionMotorCheck.java @@ -1,11 +1,14 @@ -package org.usfirst.frc4904.standard.commands.systemchecks; +package org.usfirst.frc4904.standard.commands.systemchecks.motor; +import org.usfirst.frc4904.standard.commands.systemchecks.SubsystemCheck; import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor; public class PositionMotorCheck extends SubsystemCheck { protected final PositionSensorMotor[] positionMotors; protected final double setPosition; + protected static final double DEFAULT_POSITION = 50; + protected static final double POSITION_THRESHOLD = 0.01; public PositionMotorCheck(String name, double setPosition, PositionSensorMotor... positionMotors) { super(name, positionMotors); @@ -14,7 +17,7 @@ public PositionMotorCheck(String name, double setPosition, PositionSensorMotor.. } public PositionMotorCheck(String name, PositionSensorMotor... positionMotors) { - this(name, 50, positionMotors); + this(name, DEFAULT_POSITION, positionMotors); } public PositionMotorCheck(double setPosition, PositionSensorMotor... positionMotors) { @@ -30,6 +33,9 @@ public void initialize() { for (PositionSensorMotor motor : positionMotors) { try { motor.setPosition(setPosition); + if (Math.abs(motor.getMotionController().getSensor().pidGetSafely() - setPosition) > POSITION_THRESHOLD) { + updateStatus(motor.getName(), SystemStatus.FAIL, "POSITION NOT WITHIN THRESHOLD"); + } } catch (Exception e) { updateStatus(motor.getName(), SystemStatus.FAIL, e.getMessage()); } diff --git a/commands/systemchecks/motor/ServoCheck.java b/commands/systemchecks/motor/ServoCheck.java new file mode 100644 index 00000000..0ea66998 --- /dev/null +++ b/commands/systemchecks/motor/ServoCheck.java @@ -0,0 +1,39 @@ +package org.usfirst.frc4904.standard.commands.systemchecks.motor; + +import org.usfirst.frc4904.standard.commands.systemchecks.SubsystemCheck; +import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; +import org.usfirst.frc4904.standard.subsystems.motor.ServoSubsystem; + +public class ServoCheck extends SubsystemCheck { + protected final double angle; + protected static final double DEFAULT_ANGLE = 50; + protected final ServoSubsystem[] servos; + + public ServoCheck(String name, double angle, ServoSubsystem... servos) { + super(name, servos); + this.servos = servos; + this.angle = angle; + } + + public ServoCheck(String name, ServoSubsystem...servos) { + this(name, DEFAULT_ANGLE, servos); + } + + public ServoCheck(double angle, ServoSubsystem... servos) { + this("ServoCheck", angle, servos); + } + + public ServoCheck(ServoSubsystem... servos) { + this("ServoCheck", servos); + } + + public void initialize() { + for (ServoSubsystem servo : servos) { + try { + servo.setAngle(angle); + } catch (Exception e) { + updateStatus(servo.getName(), SystemStatus.FAIL, e.getMessage()); + } + } + } +} \ No newline at end of file diff --git a/commands/systemchecks/VelocityMotorCheck.java b/commands/systemchecks/motor/VelocityMotorCheck.java similarity index 51% rename from commands/systemchecks/VelocityMotorCheck.java rename to commands/systemchecks/motor/VelocityMotorCheck.java index 4b488be8..3d833913 100644 --- a/commands/systemchecks/VelocityMotorCheck.java +++ b/commands/systemchecks/motor/VelocityMotorCheck.java @@ -1,9 +1,14 @@ -package org.usfirst.frc4904.standard.commands.systemchecks; +package org.usfirst.frc4904.standard.commands.systemchecks.motor; + +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.motor.VelocitySensorMotor; public class VelocityMotorCheck extends MotorCheck { protected final VelocitySensorMotor[] velocityMotors; + protected static final double VELOCITY_THRESHOLD = 0.01; // TODO: Change this public VelocityMotorCheck(String name, double speed, VelocitySensorMotor... velocityMotors) { super(name, speed, velocityMotors); @@ -22,18 +27,24 @@ public VelocityMotorCheck(VelocitySensorMotor... velocityMotors) { this("VelocityMotorCheck", velocityMotors); } + @Override public void initialize() { - for (VelocitySensorMotor motor: velocityMotors){ + for (VelocitySensorMotor motor : velocityMotors) { motor.set(speed); } } + @Override public void execute() { - for (VelocitySensorMotor motor: velocityMotors){ + for (VelocitySensorMotor motor : velocityMotors) { try { motor.set(speed); - } catch (Exception e) { - updateStatus(motor.getName(), StatusMessage.SystemStatus.FAIL, e.getMessage()); + if (Math.abs(motor.getMotionController().getSensor().pidGetSafely() - speed) > VELOCITY_THRESHOLD) { + updateStatus(motor.getName(), SystemStatus.FAIL, "SET SPEED NOT WITHIN REQUIRED THRESHOLD"); + } + } + catch (InvalidSensorException e) { + updateStatus(motor.getName(), SystemStatus.FAIL, e.getMessage()); } } } diff --git a/custom/motioncontrollers/MotionController.java b/custom/motioncontrollers/MotionController.java index 0970c4e9..f694ee66 100644 --- a/custom/motioncontrollers/MotionController.java +++ b/custom/motioncontrollers/MotionController.java @@ -85,6 +85,10 @@ public MotionController(PIDSource source) { this(new PIDSensor.PIDSourceWrapper(source)); } + public PIDSensor getSensor() { + return sensor; + } + /** * This should return the motion controller * to a state such that it returns 0. 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(); } From ab815883067c44f7a52ac3d4470ae4eb297410af Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Wed, 30 Jan 2019 16:34:34 -0800 Subject: [PATCH 08/23] made some changes --- commands/systemchecks/BasicCheck.java | 18 +++---- commands/systemchecks/CompressorCheck.java | 8 ++-- commands/systemchecks/NetworkTablesCheck.java | 47 +++++++++++++++++++ commands/systemchecks/SolenoidCheck.java | 2 +- commands/systemchecks/StatusMessage.java | 11 ++--- commands/systemchecks/SubsystemCheck.java | 16 +++++-- commands/systemchecks/motor/MotorCheck.java | 11 +++-- .../motor/PositionMotorCheck.java | 4 +- commands/systemchecks/motor/ServoCheck.java | 2 +- .../motor/VelocityMotorCheck.java | 4 +- 10 files changed, 88 insertions(+), 35 deletions(-) create mode 100644 commands/systemchecks/NetworkTablesCheck.java diff --git a/commands/systemchecks/BasicCheck.java b/commands/systemchecks/BasicCheck.java index f1794435..7e100389 100644 --- a/commands/systemchecks/BasicCheck.java +++ b/commands/systemchecks/BasicCheck.java @@ -2,6 +2,8 @@ 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; @@ -35,12 +37,12 @@ public void end() { outputStatuses(); } - public void setStatus(String name, SystemStatus status, String errorMessage) { - statuses.put(name, new StatusMessage(status, errorMessage)); + 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, ""); + setStatus(name, SystemStatus.PASS, new Exception("NO ERROR")); } public void initStatuses() { for (String name : systemNames) { @@ -48,8 +50,8 @@ public void initStatuses() { } } - public void updateStatus(String key, StatusMessage.SystemStatus status, String errorMessage) { - setStatus(key, status, statuses.get(key).errorMessage + errorMessage); + public void updateStatus(String key, StatusMessage.SystemStatus status, Exception... exceptions) { + setStatus(key, status, Stream.concat(Arrays.stream(getExceptions(key)), Arrays.stream(exceptions)).toArray(Exception[]::new)); } public StatusMessage getStatusMessage(String key) { return statuses.get(key); @@ -59,8 +61,8 @@ public SystemStatus getStatus(String key) { return getStatusMessage(key).status; } - public String getError(String key) { - return getStatusMessage(key).errorMessage; + public Exception[] getExceptions(String key) { + return getStatusMessage(key).exceptions; } public void outputStatuses() { @@ -69,7 +71,7 @@ public void outputStatuses() { 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.errorMessage); + LogKitten.wtf("Subsystem: " + name + ", Status: " + (message.status == StatusMessage.SystemStatus.PASS ? "PASS" : "FAIL") + ", ERROR: " + message.exceptions); } } } \ No newline at end of file diff --git a/commands/systemchecks/CompressorCheck.java b/commands/systemchecks/CompressorCheck.java index a5f83366..bafea503 100644 --- a/commands/systemchecks/CompressorCheck.java +++ b/commands/systemchecks/CompressorCheck.java @@ -14,18 +14,18 @@ public CompressorCheck(String name, Compressor... compressors) { public void execute() { for (Compressor compressor : compressors) { if (compressor.getCompressorNotConnectedFault() || compressor.getCompressorNotConnectedStickyFault() || !compressor.enabled()){ - updateStatus(compressor.getName(), SystemStatus.FAIL, "COMPRESSOR NOT CONNECTED"); + updateStatus(compressor.getName(), SystemStatus.FAIL, new Exception("COMPRESSOR NOT CONNECTED")); } else if (compressor.getCompressorShortedFault() || compressor.getCompressorShortedStickyFault()) { - updateStatus(compressor.getName(), SystemStatus.FAIL, "COMPRESSOR SHORTED"); + updateStatus(compressor.getName(), SystemStatus.FAIL, new Exception("COMPRESSOR SHORTED")); } else if (compressor.getCompressorCurrentTooHighFault() || compressor.getCompressorCurrentTooHighStickyFault()) { - updateStatus(compressor.getName(), SystemStatus.FAIL, "CURRENT TOO HIGH"); + 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, "LOW PRESSURE"); + updateStatus(compressor.getName(), SystemStatus.FAIL, new Exception("LOW PRESSURE")); } diff --git a/commands/systemchecks/NetworkTablesCheck.java b/commands/systemchecks/NetworkTablesCheck.java new file mode 100644 index 00000000..e870cc30 --- /dev/null +++ b/commands/systemchecks/NetworkTablesCheck.java @@ -0,0 +1,47 @@ +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; + } + } \ No newline at end of file diff --git a/commands/systemchecks/SolenoidCheck.java b/commands/systemchecks/SolenoidCheck.java index 0a2e4489..388f9690 100644 --- a/commands/systemchecks/SolenoidCheck.java +++ b/commands/systemchecks/SolenoidCheck.java @@ -20,7 +20,7 @@ public void initialize() { try { solenoid.set(DoubleSolenoid.Value.kForward); } catch (Exception e) { - updateStatus(solenoid.getName(), SystemStatus.FAIL, e.getMessage()); + updateStatus(solenoid.getName(), SystemStatus.FAIL, e); } } } diff --git a/commands/systemchecks/StatusMessage.java b/commands/systemchecks/StatusMessage.java index 4932e99b..874cf7df 100644 --- a/commands/systemchecks/StatusMessage.java +++ b/commands/systemchecks/StatusMessage.java @@ -1,17 +1,12 @@ package org.usfirst.frc4904.standard.commands.systemchecks; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; - - public class StatusMessage { protected SystemStatus status; - protected String errorMessage; - // protected Exception e; // TODO: incorporate Exceptions + protected Exception[] exceptions; - public StatusMessage(SystemStatus status, String errorMessage) { + public StatusMessage(SystemStatus status, Exception... exceptions) { this.status = status; - this.errorMessage = errorMessage; + this.exceptions = exceptions; } public enum SystemStatus { diff --git a/commands/systemchecks/SubsystemCheck.java b/commands/systemchecks/SubsystemCheck.java index ceb6e4d2..65f5d5a4 100644 --- a/commands/systemchecks/SubsystemCheck.java +++ b/commands/systemchecks/SubsystemCheck.java @@ -3,14 +3,22 @@ import edu.wpi.first.wpilibj.command.Subsystem; public abstract class SubsystemCheck extends SystemCheck { - public SubsystemCheck (String name, Subsystem... subsystems) { - super(name, subsystems); + public SubsystemCheck (String name, double timeout, Subsystem... subsystems) { + super(name, timeout, subsystems); for (Subsystem system : subsystems) { requires(system); } } - public SubsystemCheck (Subsystem... systems) { - this("SystemCheck", systems); + 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); } } \ No newline at end of file diff --git a/commands/systemchecks/motor/MotorCheck.java b/commands/systemchecks/motor/MotorCheck.java index ff1c55f5..196ea112 100644 --- a/commands/systemchecks/motor/MotorCheck.java +++ b/commands/systemchecks/motor/MotorCheck.java @@ -11,16 +11,17 @@ public class MotorCheck extends SubsystemCheck { protected static final Util.Range outputCurrentRange = new Util.Range(0.1, 0.3); // TODO: Use Current to judge speedcontrollers protected final Motor[] motors; - public MotorCheck(String name, double speed, Motor... motors) { - super(name, motors); + public MotorCheck(String name, double timeout, double speed, Motor... motors) { + super(name, timeout, motors); this.motors = motors; this.speed = speed; } - public MotorCheck(double speed, Motor... motors) { - this("MotorCheck", speed, motors); + public MotorCheck(String name, double speed, Motor... motors) { + this("MotorCheck", DEFAULT_SPEED, speed, motors); } + public MotorCheck(String name, Motor... motors) { this(name, DEFAULT_SPEED, motors); } @@ -40,7 +41,7 @@ public void execute() { try { motor.set(speed); } catch (Exception e) { - updateStatus(motor.getName(), SystemStatus.FAIL, e.getMessage()); + updateStatus(motor.getName(), SystemStatus.FAIL, e); } } } diff --git a/commands/systemchecks/motor/PositionMotorCheck.java b/commands/systemchecks/motor/PositionMotorCheck.java index 015e2f3c..74107865 100644 --- a/commands/systemchecks/motor/PositionMotorCheck.java +++ b/commands/systemchecks/motor/PositionMotorCheck.java @@ -34,10 +34,10 @@ public void initialize() { try { motor.setPosition(setPosition); if (Math.abs(motor.getMotionController().getSensor().pidGetSafely() - setPosition) > POSITION_THRESHOLD) { - updateStatus(motor.getName(), SystemStatus.FAIL, "POSITION NOT WITHIN THRESHOLD"); + updateStatus(motor.getName(), SystemStatus.FAIL, new Exception("POSITION NOT WITHIN THRESHOLD")); } } catch (Exception e) { - updateStatus(motor.getName(), SystemStatus.FAIL, e.getMessage()); + updateStatus(motor.getName(), SystemStatus.FAIL, e); } } } diff --git a/commands/systemchecks/motor/ServoCheck.java b/commands/systemchecks/motor/ServoCheck.java index 0ea66998..ddb46096 100644 --- a/commands/systemchecks/motor/ServoCheck.java +++ b/commands/systemchecks/motor/ServoCheck.java @@ -32,7 +32,7 @@ public void initialize() { try { servo.setAngle(angle); } catch (Exception e) { - updateStatus(servo.getName(), SystemStatus.FAIL, e.getMessage()); + updateStatus(servo.getName(), SystemStatus.FAIL, e); } } } diff --git a/commands/systemchecks/motor/VelocityMotorCheck.java b/commands/systemchecks/motor/VelocityMotorCheck.java index 3d833913..46ab06ce 100644 --- a/commands/systemchecks/motor/VelocityMotorCheck.java +++ b/commands/systemchecks/motor/VelocityMotorCheck.java @@ -40,11 +40,11 @@ public void execute() { try { motor.set(speed); if (Math.abs(motor.getMotionController().getSensor().pidGetSafely() - speed) > VELOCITY_THRESHOLD) { - updateStatus(motor.getName(), SystemStatus.FAIL, "SET SPEED NOT WITHIN REQUIRED THRESHOLD"); + updateStatus(motor.getName(), SystemStatus.FAIL, new Exception("SET SPEED NOT WITHIN REQUIRED THRESHOLD")); } } catch (InvalidSensorException e) { - updateStatus(motor.getName(), SystemStatus.FAIL, e.getMessage()); + updateStatus(motor.getName(), SystemStatus.FAIL, e); } } } From 8e50165d6647432cf66eeeaf6ec50dc2854d5cf5 Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Wed, 30 Jan 2019 20:05:34 -0800 Subject: [PATCH 09/23] added PID Check --- commands/systemchecks/BasicCheck.java | 3 +- commands/systemchecks/Check.java | 14 ++--- commands/systemchecks/DrivePIDCheck.java | 68 ++++++++++++++++++++++++ commands/systemchecks/SystemCheck.java | 8 --- 4 files changed, 77 insertions(+), 16 deletions(-) create mode 100644 commands/systemchecks/DrivePIDCheck.java diff --git a/commands/systemchecks/BasicCheck.java b/commands/systemchecks/BasicCheck.java index 7e100389..db78091e 100644 --- a/commands/systemchecks/BasicCheck.java +++ b/commands/systemchecks/BasicCheck.java @@ -8,7 +8,7 @@ import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; import edu.wpi.first.wpilibj.command.Command; -public abstract class BasicCheck extends Command { +public abstract class BasicCheck extends Command implements Check { protected HashMap statuses; protected final String[] systemNames; protected static final double DEFAULT_TIMEOUT = 5; @@ -72,6 +72,7 @@ public void outputStatuses() { 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 } } } \ No newline at end of file diff --git a/commands/systemchecks/Check.java b/commands/systemchecks/Check.java index 1cfc00e4..11589824 100644 --- a/commands/systemchecks/Check.java +++ b/commands/systemchecks/Check.java @@ -4,22 +4,22 @@ public interface Check { - public void setStatus(String key, StatusMessage.SystemStatus status, String errorMessage); + public void setStatus(String key, StatusMessage.SystemStatus status, Exception... exceptions); default void initStatus(String name) { - setStatus(name, SystemStatus.PASS, ""); + setStatus(name, SystemStatus.PASS, new Exception("NO ERROR")); } public void initStatuses(); - public void updateStatus(String key, StatusMessage.SystemStatus status, String errorMessage); + public void updateStatus(String key, StatusMessage.SystemStatus status, Exception... exceptions); - default void setStatusFail(String key, String errorMessage) { - updateStatus(key, SystemStatus.FAIL, errorMessage); + default void setStatusFail(String key, Exception... exceptions) { + updateStatus(key, SystemStatus.FAIL, exceptions); } - default void setStatusPass(String key, String errorMessage) { - updateStatus(key, SystemStatus.PASS, errorMessage); + default void setStatusPass(String key, Exception... exceptions) { + updateStatus(key, SystemStatus.PASS, exceptions); } public void outputStatuses(); diff --git a/commands/systemchecks/DrivePIDCheck.java b/commands/systemchecks/DrivePIDCheck.java new file mode 100644 index 00000000..f26c3102 --- /dev/null +++ b/commands/systemchecks/DrivePIDCheck.java @@ -0,0 +1,68 @@ +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 statuses; + protected static final String CHECK_NAME = "DrivePIDCheck"; + protected final double distance; + protected static final double ERROR_THRESHOLD = 2.0; + 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(getExceptions(key)), Arrays.stream(exceptions)).toArray(Exception[]::new)); + } + + public StatusMessage getStatusMessage(String key) { + return statuses.get(key); + } + + public Exception[] getExceptions(String key) { + return getStatusMessage(key).exceptions; + } + + public void outputStatuses() { + LogKitten.wtf(CHECK_NAME + ": " + getExceptions(CHECK_NAME)); // TODO: change logkitten level + } +} \ No newline at end of file diff --git a/commands/systemchecks/SystemCheck.java b/commands/systemchecks/SystemCheck.java index 8347641f..77726bb9 100644 --- a/commands/systemchecks/SystemCheck.java +++ b/commands/systemchecks/SystemCheck.java @@ -3,7 +3,6 @@ import java.util.Arrays; import java.util.HashMap; import java.util.stream.Collectors; -import edu.wpi.first.wpilibj.Sendable; import edu.wpi.first.wpilibj.SendableBase; public abstract class SystemCheck extends BasicCheck { @@ -26,11 +25,4 @@ public SystemCheck(String name, SendableBase... systems) { public SystemCheck(SendableBase... systems) { this("SystemCheck", systems); } - - @Override - public void initStatuses() { - for (SendableBase system : systems) { - initStatus(system.getName()); - } - } } \ No newline at end of file From 18f7388a5d69157bf187aa73971cdf57dd60a827 Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Fri, 1 Feb 2019 10:16:04 -0800 Subject: [PATCH 10/23] TURN PID CHECK --- commands/systemchecks/BasicCheck.java | 10 +-- commands/systemchecks/CompressorCheck.java | 16 ++++- commands/systemchecks/DrivePIDCheck.java | 10 +-- commands/systemchecks/TurnPIDCheck.java | 67 +++++++++++++++++++++ commands/systemchecks/motor/MotorCheck.java | 10 ++- 5 files changed, 94 insertions(+), 19 deletions(-) create mode 100644 commands/systemchecks/TurnPIDCheck.java diff --git a/commands/systemchecks/BasicCheck.java b/commands/systemchecks/BasicCheck.java index db78091e..aef9e7b5 100644 --- a/commands/systemchecks/BasicCheck.java +++ b/commands/systemchecks/BasicCheck.java @@ -51,20 +51,12 @@ public void initStatuses() { } public void updateStatus(String key, StatusMessage.SystemStatus status, Exception... exceptions) { - setStatus(key, status, Stream.concat(Arrays.stream(getExceptions(key)), Arrays.stream(exceptions)).toArray(Exception[]::new)); + 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 SystemStatus getStatus(String key) { - return getStatusMessage(key).status; - } - - public Exception[] getExceptions(String key) { - return getStatusMessage(key).exceptions; - } - public void outputStatuses() { LogKitten.wtf(getName() + " Statuses:"); for (Map.Entry entry : statuses.entrySet()) { diff --git a/commands/systemchecks/CompressorCheck.java b/commands/systemchecks/CompressorCheck.java index bafea503..d3211db2 100644 --- a/commands/systemchecks/CompressorCheck.java +++ b/commands/systemchecks/CompressorCheck.java @@ -6,11 +6,23 @@ public class CompressorCheck extends SystemCheck { protected final Compressor[] compressors; - public CompressorCheck(String name, Compressor... compressors) { - super("CompressorCheck", 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()){ diff --git a/commands/systemchecks/DrivePIDCheck.java b/commands/systemchecks/DrivePIDCheck.java index f26c3102..beb26b07 100644 --- a/commands/systemchecks/DrivePIDCheck.java +++ b/commands/systemchecks/DrivePIDCheck.java @@ -15,7 +15,7 @@ public class DrivePIDCheck extends ChassisMoveDistance implements Check { protected HashMap statuses; protected static final String CHECK_NAME = "DrivePIDCheck"; protected final double distance; - protected static final double ERROR_THRESHOLD = 2.0; + protected static final double ERROR_THRESHOLD = 2.0; // TODO: CHANGE THIS protected double position; public DrivePIDCheck(Chassis chassis, double distance, MotionController motionController) { @@ -51,18 +51,14 @@ public void setStatus(String name, SystemStatus status, Exception... exceptions) public void updateStatus(String key, SystemStatus status, Exception... exceptions) { setStatus(key, status, - Stream.concat(Arrays.stream(getExceptions(key)), Arrays.stream(exceptions)).toArray(Exception[]::new)); + Stream.concat(Arrays.stream(getStatusMessage(key).exceptions), Arrays.stream(exceptions)).toArray(Exception[]::new)); } public StatusMessage getStatusMessage(String key) { return statuses.get(key); } - public Exception[] getExceptions(String key) { - return getStatusMessage(key).exceptions; - } - public void outputStatuses() { - LogKitten.wtf(CHECK_NAME + ": " + getExceptions(CHECK_NAME)); // TODO: change logkitten level + LogKitten.wtf(CHECK_NAME + ": " + getStatusMessage(CHECK_NAME).exceptions); // TODO: change logkitten level } } \ No newline at end of file diff --git a/commands/systemchecks/TurnPIDCheck.java b/commands/systemchecks/TurnPIDCheck.java new file mode 100644 index 00000000..9fb52eed --- /dev/null +++ b/commands/systemchecks/TurnPIDCheck.java @@ -0,0 +1,67 @@ +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; + +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 ERROR_THRESHOLD = 2.0; // TODO: CHANGE THIS + protected double angle; + + public TurnPIDCheck(Chassis chassis, double finalAngle, IMU imu, MotionController motionController) { + super(chassis, finalAngle, imu, motionController); + this.finalAngle = finalAngle; + initStatuses(); + } + + @Override + public void end() { + try { + angle = motionController.getInputSafely(); + } + catch (InvalidSensorException e) { + angle = 0; + updateStatus(CHECK_NAME, SystemStatus.FAIL, e); + } + if (Math.abs(motionController.getSetpoint() - angle) > ERROR_THRESHOLD) { + updateStatus(CHECK_NAME, SystemStatus.FAIL, new Exception("ANGLE TURNED NOT WITHIN ERROR THRESHOLD")); + } + move.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 + } +} \ No newline at end of file diff --git a/commands/systemchecks/motor/MotorCheck.java b/commands/systemchecks/motor/MotorCheck.java index 196ea112..5e0485a2 100644 --- a/commands/systemchecks/motor/MotorCheck.java +++ b/commands/systemchecks/motor/MotorCheck.java @@ -17,10 +17,18 @@ public MotorCheck(String name, double timeout, double speed, Motor... motors) { this.speed = speed; } + public MotorCheck(String name, double speed, Motor... motors) { - this("MotorCheck", DEFAULT_SPEED, speed, motors); + this(name, DEFAULT_TIMEOUT, speed, motors); + } + + public MotorCheck(double timeout, double speed, Motor... motors) { + this("MotorCheck", timeout, speed, motors); } + public MotorCheck(double speed, Motor... motors) { + this(DEFAULT_TIMEOUT, speed, motors); + } public MotorCheck(String name, Motor... motors) { this(name, DEFAULT_SPEED, motors); From 83c86067ef6a0122f1b012d6a4870df0b5e02344 Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Fri, 1 Feb 2019 14:46:39 -0800 Subject: [PATCH 11/23] made position and velocity motor check commands way coooler --- commands/systemchecks/motor/MotorCheck.java | 11 +-- .../motor/PositionMotorCheck.java | 30 +------ .../systemchecks/motor/SensorMotorCheck.java | 85 +++++++++++++++++++ .../motor/VelocityMotorCheck.java | 33 +------ 4 files changed, 98 insertions(+), 61 deletions(-) create mode 100644 commands/systemchecks/motor/SensorMotorCheck.java diff --git a/commands/systemchecks/motor/MotorCheck.java b/commands/systemchecks/motor/MotorCheck.java index 5e0485a2..da3098c5 100644 --- a/commands/systemchecks/motor/MotorCheck.java +++ b/commands/systemchecks/motor/MotorCheck.java @@ -1,5 +1,6 @@ package org.usfirst.frc4904.standard.commands.systemchecks.motor; + import org.usfirst.frc4904.standard.Util; import org.usfirst.frc4904.standard.commands.systemchecks.SubsystemCheck; import org.usfirst.frc4904.standard.subsystems.motor.Motor; @@ -7,7 +8,7 @@ public class MotorCheck extends SubsystemCheck { protected static final double DEFAULT_SPEED = 0.5; // TODO: CHECK THIS - protected final double speed; + protected double speed; protected static final Util.Range outputCurrentRange = new Util.Range(0.1, 0.3); // TODO: Use Current to judge speedcontrollers protected final Motor[] motors; @@ -15,7 +16,6 @@ public MotorCheck(String name, double timeout, double speed, Motor... motors) { super(name, timeout, motors); this.motors = motors; this.speed = speed; - } public MotorCheck(String name, double speed, Motor... motors) { @@ -39,16 +39,17 @@ public MotorCheck(Motor... motors) { } public void initialize() { - for (Motor motor: motors){ + for (Motor motor : motors) { motor.set(speed); } } public void execute() { - for (Motor motor: motors){ + for (Motor motor : motors) { try { motor.set(speed); - } catch (Exception e) { + } + catch (Exception e) { updateStatus(motor.getName(), SystemStatus.FAIL, e); } } diff --git a/commands/systemchecks/motor/PositionMotorCheck.java b/commands/systemchecks/motor/PositionMotorCheck.java index 74107865..6f79b912 100644 --- a/commands/systemchecks/motor/PositionMotorCheck.java +++ b/commands/systemchecks/motor/PositionMotorCheck.java @@ -1,19 +1,10 @@ package org.usfirst.frc4904.standard.commands.systemchecks.motor; -import org.usfirst.frc4904.standard.commands.systemchecks.SubsystemCheck; -import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor; -public class PositionMotorCheck extends SubsystemCheck { - protected final PositionSensorMotor[] positionMotors; - protected final double setPosition; - protected static final double DEFAULT_POSITION = 50; - protected static final double POSITION_THRESHOLD = 0.01; - +public class PositionMotorCheck extends SensorMotorCheck { public PositionMotorCheck(String name, double setPosition, PositionSensorMotor... positionMotors) { - super(name, positionMotors); - this.positionMotors = positionMotors; - this.setPosition = setPosition; + super(name, setPosition, positionMotors); } public PositionMotorCheck(String name, PositionSensorMotor... positionMotors) { @@ -21,24 +12,11 @@ public PositionMotorCheck(String name, PositionSensorMotor... positionMotors) { } public PositionMotorCheck(double setPosition, PositionSensorMotor... positionMotors) { - this("PositionSensorMotorCheck", setPosition, positionMotors); + this("PositionMotorCheck", setPosition, positionMotors); } public PositionMotorCheck(PositionSensorMotor... positionMotors) { - this("PositionSensorMotorCheck", positionMotors); + this("PositionMotorCheck", positionMotors); } - @Override - public void initialize() { - for (PositionSensorMotor motor : positionMotors) { - try { - motor.setPosition(setPosition); - if (Math.abs(motor.getMotionController().getSensor().pidGetSafely() - setPosition) > POSITION_THRESHOLD) { - updateStatus(motor.getName(), SystemStatus.FAIL, new Exception("POSITION NOT WITHIN THRESHOLD")); - } - } catch (Exception e) { - updateStatus(motor.getName(), SystemStatus.FAIL, e); - } - } - } } \ 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..7287fd77 --- /dev/null +++ b/commands/systemchecks/motor/SensorMotorCheck.java @@ -0,0 +1,85 @@ +package org.usfirst.frc4904.standard.commands.systemchecks.motor; + +import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; +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; + +public abstract class SensorMotorCheck extends MotorCheck { + protected SensorMotor[] motors; + 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 double position; + + public SensorMotorCheck(String name, double timeout, double speed, double position, SensorMotor... motors) { + super(name, timeout, motors); + this.motors = motors; + this.speed = speed; + this.position = position; + } + + public SensorMotorCheck(String name, double speed, double position, SensorMotor... motors) { + this(name, DEFAULT_TIMEOUT, speed, position, motors); + } + + public SensorMotorCheck(double timeout, double speed, double position, SensorMotor... motors) { + this("SensorMotorCheck", timeout, speed, position, motors); + } + + public SensorMotorCheck(double speed, double position, SensorMotor... motors) { + this(DEFAULT_TIMEOUT, speed, position, motors); + } + + public SensorMotorCheck(String name, double position, SensorMotor... motors) { + this(name, DEFAULT_TIMEOUT, DEFAULT_SPEED, position, motors); + } + + public SensorMotorCheck(double position, SensorMotor... motors) { + this("SensorMotorCheck", position, motors); + } + + public SensorMotorCheck(String name, SensorMotor... motors) { + this(name, DEFAULT_SPEED, motors); + } + + public SensorMotorCheck(SensorMotor... motors) { + this("SensorMotorCheck", motors); + } + + @Override + public void initialize() { + for (SensorMotor motor : motors) { + if (motor instanceof VelocitySensorMotor) { + ((VelocitySensorMotor) motor).set(speed); + } + } + } + + @Override + public void execute() { + for (SensorMotor motor : motors) { + double input; + try { + input = motor.getMotionController().getInputSafely(); + } + catch (InvalidSensorException e) { + input = 0; + updateStatus(motor.getName(), SystemStatus.FAIL, e); + } + if (motor instanceof VelocitySensorMotor) { + ((VelocitySensorMotor) motor).set(speed); + if (input - speed > VELOCITY_THRESHOLD) { + updateStatus(motor.getName(), SystemStatus.FAIL, new Exception("SET SPEED NOT WITHIN REQUIRED THRESHOLD")); + } + } + else if (motor instanceof PositionSensorMotor) { + ((PositionSensorMotor) motor).setPosition(position); + if (input - position > POSITION_THRESHOLD) { + updateStatus(motor.getName(), SystemStatus.FAIL, new Exception("POSITION NOT WITHIN THRESHOLD")); + } + } + } +} \ No newline at end of file diff --git a/commands/systemchecks/motor/VelocityMotorCheck.java b/commands/systemchecks/motor/VelocityMotorCheck.java index 46ab06ce..e9952b33 100644 --- a/commands/systemchecks/motor/VelocityMotorCheck.java +++ b/commands/systemchecks/motor/VelocityMotorCheck.java @@ -1,18 +1,13 @@ package org.usfirst.frc4904.standard.commands.systemchecks.motor; -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.motor.VelocitySensorMotor; -public class VelocityMotorCheck extends MotorCheck { - protected final VelocitySensorMotor[] velocityMotors; - protected static final double VELOCITY_THRESHOLD = 0.01; // TODO: Change this +public class VelocityMotorCheck extends SensorMotorCheck { public VelocityMotorCheck(String name, double speed, VelocitySensorMotor... velocityMotors) { - super(name, speed, velocityMotors); - this.velocityMotors = velocityMotors; + super(name, speed, 0, velocityMotors); } public VelocityMotorCheck(String name, VelocitySensorMotor... velocityMotors) { @@ -26,26 +21,4 @@ public VelocityMotorCheck(double speed, VelocitySensorMotor... velocityMotors) { public VelocityMotorCheck(VelocitySensorMotor... velocityMotors) { this("VelocityMotorCheck", velocityMotors); } - - @Override - public void initialize() { - for (VelocitySensorMotor motor : velocityMotors) { - motor.set(speed); - } - } - - @Override - public void execute() { - for (VelocitySensorMotor motor : velocityMotors) { - try { - motor.set(speed); - if (Math.abs(motor.getMotionController().getSensor().pidGetSafely() - speed) > VELOCITY_THRESHOLD) { - updateStatus(motor.getName(), SystemStatus.FAIL, new Exception("SET SPEED NOT WITHIN REQUIRED THRESHOLD")); - } - } - catch (InvalidSensorException e) { - updateStatus(motor.getName(), SystemStatus.FAIL, e); - } - } - } } \ No newline at end of file From fbad03736ff34c04d11fad43b2b460f18bd76482 Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Fri, 1 Feb 2019 14:52:55 -0800 Subject: [PATCH 12/23] formatting is very important --- commands/systemchecks/BasicCheck.java | 13 +++++++---- commands/systemchecks/Check.java | 2 +- commands/systemchecks/CheckGroup.java | 1 + commands/systemchecks/CompressorCheck.java | 23 +++++++------------ commands/systemchecks/DrivePIDCheck.java | 3 ++- commands/systemchecks/NetworkTablesCheck.java | 10 ++++---- commands/systemchecks/SolenoidCheck.java | 6 +++-- commands/systemchecks/StatusMessage.java | 3 ++- commands/systemchecks/SubsystemCheck.java | 3 ++- commands/systemchecks/SystemCheck.java | 6 +++-- .../motor/PositionMotorCheck.java | 4 ++-- .../systemchecks/motor/SensorMotorCheck.java | 14 +++++------ commands/systemchecks/motor/ServoCheck.java | 8 ++++--- .../motor/VelocityMotorCheck.java | 4 ++++ 14 files changed, 57 insertions(+), 43 deletions(-) diff --git a/commands/systemchecks/BasicCheck.java b/commands/systemchecks/BasicCheck.java index aef9e7b5..5ccea437 100644 --- a/commands/systemchecks/BasicCheck.java +++ b/commands/systemchecks/BasicCheck.java @@ -1,5 +1,6 @@ package org.usfirst.frc4904.standard.commands.systemchecks; + import java.util.HashMap; import java.util.Map; import java.util.stream.Stream; @@ -44,15 +45,18 @@ public void setStatus(String name, SystemStatus status, Exception... 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)); + 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); } @@ -63,8 +67,9 @@ public void outputStatuses() { 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 + LogKitten.wtf("Subsystem: " + name + ", Status: " + + (message.status == StatusMessage.SystemStatus.PASS ? "PASS" : "FAIL") + ", ERROR: " + message.exceptions); + // TODO:check if the logging of exceptions above works } } } \ No newline at end of file diff --git a/commands/systemchecks/Check.java b/commands/systemchecks/Check.java index 11589824..44a26290 100644 --- a/commands/systemchecks/Check.java +++ b/commands/systemchecks/Check.java @@ -1,9 +1,9 @@ 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) { diff --git a/commands/systemchecks/CheckGroup.java b/commands/systemchecks/CheckGroup.java index c5ad8a23..93dad6da 100644 --- a/commands/systemchecks/CheckGroup.java +++ b/commands/systemchecks/CheckGroup.java @@ -1,5 +1,6 @@ package org.usfirst.frc4904.standard.commands.systemchecks; + import edu.wpi.first.wpilibj.command.CommandGroup; public class CheckGroup extends CommandGroup { diff --git a/commands/systemchecks/CompressorCheck.java b/commands/systemchecks/CompressorCheck.java index d3211db2..7c88f68d 100644 --- a/commands/systemchecks/CompressorCheck.java +++ b/commands/systemchecks/CompressorCheck.java @@ -1,11 +1,12 @@ 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; @@ -25,24 +26,16 @@ public CompressorCheck(Compressor... compressors) { public void execute() { for (Compressor compressor : compressors) { - if (compressor.getCompressorNotConnectedFault() || compressor.getCompressorNotConnectedStickyFault() || !compressor.enabled()){ + if (compressor.getCompressorNotConnectedFault() || compressor.getCompressorNotConnectedStickyFault() + || !compressor.enabled()) { updateStatus(compressor.getName(), SystemStatus.FAIL, new Exception("COMPRESSOR NOT CONNECTED")); - - } - else if (compressor.getCompressorShortedFault() || compressor.getCompressorShortedStickyFault()) { + } else if (compressor.getCompressorShortedFault() || compressor.getCompressorShortedStickyFault()) { updateStatus(compressor.getName(), SystemStatus.FAIL, new Exception("COMPRESSOR SHORTED")); - } - - else if (compressor.getCompressorCurrentTooHighFault() || compressor.getCompressorCurrentTooHighStickyFault()) { + } 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 + } else if (compressor.getPressureSwitchValue()) { // TODO: Check if this works as intended updateStatus(compressor.getName(), SystemStatus.FAIL, new Exception("LOW PRESSURE")); } - - - } } - -} \ No newline at end of file +} \ No newline at end of file diff --git a/commands/systemchecks/DrivePIDCheck.java b/commands/systemchecks/DrivePIDCheck.java index beb26b07..5d62fbe6 100644 --- a/commands/systemchecks/DrivePIDCheck.java +++ b/commands/systemchecks/DrivePIDCheck.java @@ -51,7 +51,8 @@ public void setStatus(String name, SystemStatus status, Exception... 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)); + Stream.concat(Arrays.stream(getStatusMessage(key).exceptions), Arrays.stream(exceptions)) + .toArray(Exception[]::new)); } public StatusMessage getStatusMessage(String key) { diff --git a/commands/systemchecks/NetworkTablesCheck.java b/commands/systemchecks/NetworkTablesCheck.java index e870cc30..8242378c 100644 --- a/commands/systemchecks/NetworkTablesCheck.java +++ b/commands/systemchecks/NetworkTablesCheck.java @@ -1,5 +1,6 @@ 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; @@ -31,11 +32,12 @@ public void initialize() { entry.setDefaultDouble(DEFAULT_ENTRY); try { table.saveEntries(SAVE_FILE); - entries = table.loadEntries(SAVE_FILE); - } catch (PersistentException e) { + 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 + 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")); } } @@ -44,4 +46,4 @@ public void initialize() { public boolean isFinished() { return true; } - } \ No newline at end of file +} \ No newline at end of file diff --git a/commands/systemchecks/SolenoidCheck.java b/commands/systemchecks/SolenoidCheck.java index 388f9690..8a9e75e8 100644 --- a/commands/systemchecks/SolenoidCheck.java +++ b/commands/systemchecks/SolenoidCheck.java @@ -1,5 +1,6 @@ package org.usfirst.frc4904.standard.commands.systemchecks; + import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; import edu.wpi.first.wpilibj.DoubleSolenoid; @@ -18,8 +19,9 @@ public SolenoidCheck(DoubleSolenoid... solenoids) { public void initialize() { for (DoubleSolenoid solenoid : solenoids) { try { - solenoid.set(DoubleSolenoid.Value.kForward); - } catch (Exception e) { + solenoid.set(DoubleSolenoid.Value.kForward); + } + catch (Exception e) { updateStatus(solenoid.getName(), SystemStatus.FAIL, e); } } diff --git a/commands/systemchecks/StatusMessage.java b/commands/systemchecks/StatusMessage.java index 874cf7df..926652d8 100644 --- a/commands/systemchecks/StatusMessage.java +++ b/commands/systemchecks/StatusMessage.java @@ -1,5 +1,6 @@ package org.usfirst.frc4904.standard.commands.systemchecks; + public class StatusMessage { protected SystemStatus status; protected Exception[] exceptions; @@ -8,7 +9,7 @@ public StatusMessage(SystemStatus status, Exception... exceptions) { this.status = status; this.exceptions = exceptions; } - + public enum SystemStatus { PASS, FAIL; } diff --git a/commands/systemchecks/SubsystemCheck.java b/commands/systemchecks/SubsystemCheck.java index 65f5d5a4..517146cf 100644 --- a/commands/systemchecks/SubsystemCheck.java +++ b/commands/systemchecks/SubsystemCheck.java @@ -1,9 +1,10 @@ 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) { + public SubsystemCheck(String name, double timeout, Subsystem... subsystems) { super(name, timeout, subsystems); for (Subsystem system : subsystems) { requires(system); diff --git a/commands/systemchecks/SystemCheck.java b/commands/systemchecks/SystemCheck.java index 77726bb9..a9ca637b 100644 --- a/commands/systemchecks/SystemCheck.java +++ b/commands/systemchecks/SystemCheck.java @@ -1,5 +1,6 @@ package org.usfirst.frc4904.standard.commands.systemchecks; + import java.util.Arrays; import java.util.HashMap; import java.util.stream.Collectors; @@ -10,7 +11,8 @@ public abstract class SystemCheck extends BasicCheck { 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)); + super(name, timeout, Arrays.asList(systems).stream().map(system -> system.getName()).collect(Collectors.toList()) + .toArray(String[]::new)); this.systems = systems; } @@ -21,7 +23,7 @@ public SystemCheck(double timeout, SendableBase... systems) { public SystemCheck(String name, SendableBase... systems) { this(name, DEFAULT_TIMEOUT, systems); } - + public SystemCheck(SendableBase... systems) { this("SystemCheck", systems); } diff --git a/commands/systemchecks/motor/PositionMotorCheck.java b/commands/systemchecks/motor/PositionMotorCheck.java index 6f79b912..7e4c723e 100644 --- a/commands/systemchecks/motor/PositionMotorCheck.java +++ b/commands/systemchecks/motor/PositionMotorCheck.java @@ -1,8 +1,9 @@ package org.usfirst.frc4904.standard.commands.systemchecks.motor; + import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor; -public class PositionMotorCheck extends SensorMotorCheck { +public class PositionMotorCheck extends SensorMotorCheck { public PositionMotorCheck(String name, double setPosition, PositionSensorMotor... positionMotors) { super(name, setPosition, positionMotors); } @@ -18,5 +19,4 @@ public PositionMotorCheck(double setPosition, PositionSensorMotor... positionMot public PositionMotorCheck(PositionSensorMotor... positionMotors) { this("PositionMotorCheck", positionMotors); } - } \ No newline at end of file diff --git a/commands/systemchecks/motor/SensorMotorCheck.java b/commands/systemchecks/motor/SensorMotorCheck.java index 7287fd77..df37d4cc 100644 --- a/commands/systemchecks/motor/SensorMotorCheck.java +++ b/commands/systemchecks/motor/SensorMotorCheck.java @@ -1,5 +1,6 @@ package org.usfirst.frc4904.standard.commands.systemchecks.motor; + import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor; @@ -9,9 +10,8 @@ public abstract class SensorMotorCheck extends MotorCheck { protected SensorMotor[] motors; 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 static final double POSITION_THRESHOLD = 2.0; // TODO: TEST THIS + protected static final double VELOCITY_THRESHOLD = 2.0; // TODO: TEST THIS protected double position; public SensorMotorCheck(String name, double timeout, double speed, double position, SensorMotor... motors) { @@ -42,7 +42,7 @@ public SensorMotorCheck(double position, SensorMotor... motors) { } public SensorMotorCheck(String name, SensorMotor... motors) { - this(name, DEFAULT_SPEED, motors); + this(name, DEFAULT_POSITION, motors); } public SensorMotorCheck(SensorMotor... motors) { @@ -64,7 +64,7 @@ public void execute() { double input; try { input = motor.getMotionController().getInputSafely(); - } + } catch (InvalidSensorException e) { input = 0; updateStatus(motor.getName(), SystemStatus.FAIL, e); @@ -74,11 +74,11 @@ public void execute() { if (input - speed > VELOCITY_THRESHOLD) { updateStatus(motor.getName(), SystemStatus.FAIL, new Exception("SET SPEED NOT WITHIN REQUIRED THRESHOLD")); } - } - else if (motor instanceof PositionSensorMotor) { + } else if (motor instanceof PositionSensorMotor) { ((PositionSensorMotor) motor).setPosition(position); if (input - position > POSITION_THRESHOLD) { updateStatus(motor.getName(), SystemStatus.FAIL, new Exception("POSITION NOT WITHIN THRESHOLD")); + } } } } diff --git a/commands/systemchecks/motor/ServoCheck.java b/commands/systemchecks/motor/ServoCheck.java index ddb46096..4a544021 100644 --- a/commands/systemchecks/motor/ServoCheck.java +++ b/commands/systemchecks/motor/ServoCheck.java @@ -1,5 +1,6 @@ package org.usfirst.frc4904.standard.commands.systemchecks.motor; + import org.usfirst.frc4904.standard.commands.systemchecks.SubsystemCheck; import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; import org.usfirst.frc4904.standard.subsystems.motor.ServoSubsystem; @@ -15,7 +16,7 @@ public ServoCheck(String name, double angle, ServoSubsystem... servos) { this.angle = angle; } - public ServoCheck(String name, ServoSubsystem...servos) { + public ServoCheck(String name, ServoSubsystem... servos) { this(name, DEFAULT_ANGLE, servos); } @@ -30,8 +31,9 @@ public ServoCheck(ServoSubsystem... servos) { public void initialize() { for (ServoSubsystem servo : servos) { try { - servo.setAngle(angle); - } catch (Exception e) { + servo.setAngle(angle); + } + catch (Exception e) { updateStatus(servo.getName(), SystemStatus.FAIL, e); } } diff --git a/commands/systemchecks/motor/VelocityMotorCheck.java b/commands/systemchecks/motor/VelocityMotorCheck.java index e9952b33..a59df809 100644 --- a/commands/systemchecks/motor/VelocityMotorCheck.java +++ b/commands/systemchecks/motor/VelocityMotorCheck.java @@ -6,6 +6,10 @@ public class VelocityMotorCheck extends SensorMotorCheck { + public VelocityMotorCheck(String name, double timeout, double speed, VelocitySensorMotor... velocityMotors) { + super(name, timeout, speed, 0, velocityMotors); + } + public VelocityMotorCheck(String name, double speed, VelocitySensorMotor... velocityMotors) { super(name, speed, 0, velocityMotors); } From 25f2092c6dad0781a27b95f1eb0f5c8f9a5d03bc Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Fri, 1 Feb 2019 15:00:05 -0800 Subject: [PATCH 13/23] made checkgroup less jank --- commands/systemchecks/CheckGroup.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/commands/systemchecks/CheckGroup.java b/commands/systemchecks/CheckGroup.java index 93dad6da..87562674 100644 --- a/commands/systemchecks/CheckGroup.java +++ b/commands/systemchecks/CheckGroup.java @@ -6,7 +6,12 @@ public class CheckGroup extends CommandGroup { public CheckGroup(String name, BasicCheck... checks) { for (BasicCheck check : checks) { - addParallel(check); + if (check instanceof SubsystemCheck) { //TODO: Think of better logic for this + addSequential(check); + } + else { + addParallel(check); + } } } } \ No newline at end of file From a680ebf3b23a6e2cab2a63abfa5fb919a0b50222 Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Fri, 1 Feb 2019 15:15:56 -0800 Subject: [PATCH 14/23] reverted motioncontroller change --- custom/motioncontrollers/MotionController.java | 4 ---- 1 file changed, 4 deletions(-) diff --git a/custom/motioncontrollers/MotionController.java b/custom/motioncontrollers/MotionController.java index f694ee66..0970c4e9 100644 --- a/custom/motioncontrollers/MotionController.java +++ b/custom/motioncontrollers/MotionController.java @@ -85,10 +85,6 @@ public MotionController(PIDSource source) { this(new PIDSensor.PIDSourceWrapper(source)); } - public PIDSensor getSensor() { - return sensor; - } - /** * This should return the motion controller * to a state such that it returns 0. From 608fd370375b60b3938b576a883b3f84ce04272c Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Sun, 3 Feb 2019 11:50:17 -0800 Subject: [PATCH 15/23] some more stuff --- commands/systemchecks/BasicCheck.java | 2 +- commands/systemchecks/DefaultException.java | 15 +++++++++++++++ commands/systemchecks/TurnPIDCheck.java | 12 +++++++++--- subsystems/motor/PositionSensorMotor.java | 3 +-- 4 files changed, 26 insertions(+), 6 deletions(-) create mode 100644 commands/systemchecks/DefaultException.java diff --git a/commands/systemchecks/BasicCheck.java b/commands/systemchecks/BasicCheck.java index 5ccea437..325cc385 100644 --- a/commands/systemchecks/BasicCheck.java +++ b/commands/systemchecks/BasicCheck.java @@ -43,7 +43,7 @@ public void setStatus(String name, SystemStatus status, Exception... exceptions) } public void initStatus(String name) { - setStatus(name, SystemStatus.PASS, new Exception("NO ERROR")); + setStatus(name, SystemStatus.PASS, new DefaultException()); } public void initStatuses() { diff --git a/commands/systemchecks/DefaultException.java b/commands/systemchecks/DefaultException.java new file mode 100644 index 00000000..9e1ee34b --- /dev/null +++ b/commands/systemchecks/DefaultException.java @@ -0,0 +1,15 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + + +public class DefaultException extends Exception { + protected static final long serialVersionUID = 4902L; + protected static final String DEFAULT_MESSAGE = "NO ERRORS"; + + public DefaultException(String message) { + super(message); + } + + public DefaultException() { + this(DEFAULT_MESSAGE); + } +} diff --git a/commands/systemchecks/TurnPIDCheck.java b/commands/systemchecks/TurnPIDCheck.java index 9fb52eed..401480d3 100644 --- a/commands/systemchecks/TurnPIDCheck.java +++ b/commands/systemchecks/TurnPIDCheck.java @@ -16,15 +16,21 @@ 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 ERROR_THRESHOLD = 2.0; // TODO: CHANGE THIS + protected static final double DEFAULT_THRESHOLD = 2.0; // TODO: Test + protected final double errorThreshold; protected double angle; - public TurnPIDCheck(Chassis chassis, double finalAngle, IMU imu, MotionController motionController) { + public TurnPIDCheck(Chassis chassis, double finalAngle, IMU imu, MotionController motionController, double errorThreshold) { super(chassis, finalAngle, imu, motionController); this.finalAngle = finalAngle; + this.errorThreshold = errorThreshold; initStatuses(); } + public TurnPIDCheck(Chassis chassis, double finalAngle, IMU imu, MotionController motionController) { + this(chassis, finalAngle, imu, motionController, DEFAULT_THRESHOLD); + } + @Override public void end() { try { @@ -34,7 +40,7 @@ public void end() { angle = 0; updateStatus(CHECK_NAME, SystemStatus.FAIL, e); } - if (Math.abs(motionController.getSetpoint() - angle) > ERROR_THRESHOLD) { + if (Math.abs(motionController.getSetpoint() - angle) > errorThreshold) { updateStatus(CHECK_NAME, SystemStatus.FAIL, new Exception("ANGLE TURNED NOT WITHIN ERROR THRESHOLD")); } move.cancel(); diff --git a/subsystems/motor/PositionSensorMotor.java b/subsystems/motor/PositionSensorMotor.java index 3c391049..9a4aa8ee 100644 --- a/subsystems/motor/PositionSensorMotor.java +++ b/subsystems/motor/PositionSensorMotor.java @@ -63,7 +63,6 @@ public PositionSensorMotor(MotionController motionController, SpeedController... } public void setPosition(double position) { - double newPosition = positionRange != null ? positionRange.limitValue(position) : position; - motionController.setSetpoint(newPosition); + motionController.setSetpoint(positionRange != null ? positionRange.limitValue(position) : position); } } From 7d813c1a14e9359bf449d71fba21a54c04d7b8dd Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Mon, 4 Feb 2019 16:48:06 -0800 Subject: [PATCH 16/23] syntax --- commands/systemchecks/BasicCheck.java | 2 +- commands/systemchecks/motor/MotorCheck.java | 94 +++++++++---------- .../systemchecks/motor/SensorMotorCheck.java | 4 +- commands/systemchecks/motor/ServoCheck.java | 2 +- 4 files changed, 51 insertions(+), 51 deletions(-) diff --git a/commands/systemchecks/BasicCheck.java b/commands/systemchecks/BasicCheck.java index 325cc385..f7f3e64c 100644 --- a/commands/systemchecks/BasicCheck.java +++ b/commands/systemchecks/BasicCheck.java @@ -10,9 +10,9 @@ import edu.wpi.first.wpilibj.command.Command; public abstract class BasicCheck extends Command implements Check { + protected static final double DEFAULT_TIMEOUT = 5; protected HashMap statuses; protected final String[] systemNames; - protected static final double DEFAULT_TIMEOUT = 5; public BasicCheck(String checkName, double timeout, String... systemNames) { super(checkName, timeout); diff --git a/commands/systemchecks/motor/MotorCheck.java b/commands/systemchecks/motor/MotorCheck.java index da3098c5..d1fdaa3b 100644 --- a/commands/systemchecks/motor/MotorCheck.java +++ b/commands/systemchecks/motor/MotorCheck.java @@ -7,51 +7,51 @@ import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; public class MotorCheck extends SubsystemCheck { - protected static final double DEFAULT_SPEED = 0.5; // TODO: CHECK THIS - protected double speed; - protected static final Util.Range outputCurrentRange = new Util.Range(0.1, 0.3); // TODO: Use Current to judge speedcontrollers - protected final Motor[] motors; - - public MotorCheck(String name, double timeout, double speed, Motor... motors) { - super(name, timeout, motors); - this.motors = motors; - this.speed = speed; - } - - public MotorCheck(String name, double speed, Motor... motors) { - this(name, DEFAULT_TIMEOUT, speed, motors); - } - - public MotorCheck(double timeout, double speed, Motor... motors) { - this("MotorCheck", timeout, speed, motors); - } - - public MotorCheck(double speed, Motor... motors) { - this(DEFAULT_TIMEOUT, speed, motors); - } - - public MotorCheck(String name, Motor... motors) { - this(name, DEFAULT_SPEED, motors); - } - - public MotorCheck(Motor... motors) { - this("MotorCheck", motors); - } - - public void initialize() { - for (Motor motor : motors) { - motor.set(speed); - } - } - - public void execute() { - for (Motor motor : motors) { - try { - motor.set(speed); - } - catch (Exception e) { - updateStatus(motor.getName(), SystemStatus.FAIL, e); - } - } - } + protected static final double DEFAULT_SPEED = 0.5; // TODO: CHECK THIS + protected static final Util.Range outputCurrentRange = new Util.Range(0.1, 0.3); // TODO: Use Current to judge speedcontrollers + protected double speed; + protected final Motor[] motors; + + public MotorCheck(String name, double timeout, double speed, Motor... motors) { + super(name, timeout, motors); + this.motors = motors; + this.speed = speed; + } + + public MotorCheck(String name, double speed, Motor... motors) { + this(name, DEFAULT_TIMEOUT, speed, motors); + } + + public MotorCheck(double timeout, double speed, Motor... motors) { + this("MotorCheck", timeout, speed, motors); + } + + public MotorCheck(double speed, Motor... motors) { + this(DEFAULT_TIMEOUT, speed, motors); + } + + public MotorCheck(String name, Motor... motors) { + this(name, DEFAULT_SPEED, motors); + } + + public MotorCheck(Motor... motors) { + this("MotorCheck", motors); + } + + public void initialize() { + for (Motor motor : motors) { + motor.set(speed); + } + } + + public void execute() { + for (Motor motor : motors) { + try { + motor.set(speed); + } + catch (Exception e) { + updateStatus(motor.getName(), SystemStatus.FAIL, e); + } + } + } } \ No newline at end of file diff --git a/commands/systemchecks/motor/SensorMotorCheck.java b/commands/systemchecks/motor/SensorMotorCheck.java index df37d4cc..e1a832a3 100644 --- a/commands/systemchecks/motor/SensorMotorCheck.java +++ b/commands/systemchecks/motor/SensorMotorCheck.java @@ -8,10 +8,10 @@ import org.usfirst.frc4904.standard.subsystems.motor.VelocitySensorMotor; public abstract class SensorMotorCheck extends MotorCheck { - protected SensorMotor[] motors; 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 static final double VELOCITY_THRESHOLD = 2.0; // TODO: TEST THIS + protected SensorMotor[] motors; protected double position; public SensorMotorCheck(String name, double timeout, double speed, double position, SensorMotor... motors) { diff --git a/commands/systemchecks/motor/ServoCheck.java b/commands/systemchecks/motor/ServoCheck.java index 4a544021..312fe7a1 100644 --- a/commands/systemchecks/motor/ServoCheck.java +++ b/commands/systemchecks/motor/ServoCheck.java @@ -6,8 +6,8 @@ import org.usfirst.frc4904.standard.subsystems.motor.ServoSubsystem; public class ServoCheck extends SubsystemCheck { - protected final double angle; protected static final double DEFAULT_ANGLE = 50; + protected final double angle; protected final ServoSubsystem[] servos; public ServoCheck(String name, double angle, ServoSubsystem... servos) { From 5e70af66abb19e3462e56edd472dd1d3cd23f9cf Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Wed, 6 Feb 2019 15:56:09 -0800 Subject: [PATCH 17/23] changes --- commands/systemchecks/BaseCheck.java | 79 +++++++++++++++++++ commands/systemchecks/BasicCheck.java | 75 ------------------ commands/systemchecks/CheckGroup.java | 22 +++--- commands/systemchecks/NetworkTablesCheck.java | 5 +- commands/systemchecks/SystemCheck.java | 2 +- subsystems/motor/PositionSensorMotor.java | 18 +++-- 6 files changed, 105 insertions(+), 96 deletions(-) create mode 100644 commands/systemchecks/BaseCheck.java delete mode 100644 commands/systemchecks/BasicCheck.java diff --git a/commands/systemchecks/BaseCheck.java b/commands/systemchecks/BaseCheck.java new file mode 100644 index 00000000..ebb75061 --- /dev/null +++ b/commands/systemchecks/BaseCheck.java @@ -0,0 +1,79 @@ +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 BaseCheck extends Command implements Check { + protected static final double DEFAULT_TIMEOUT = 5; + protected HashMap statuses; + protected final String[] systemNames; + + public BaseCheck(String checkName, double timeout, String... systemNames) { + super(checkName, timeout); + this.systemNames = systemNames; + initStatuses(); + } + + public BaseCheck(double timeout, String... systemNames) { + this("Check", timeout, systemNames); + } + + public BaseCheck(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); + } + + 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 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/BasicCheck.java b/commands/systemchecks/BasicCheck.java deleted file mode 100644 index f7f3e64c..00000000 --- a/commands/systemchecks/BasicCheck.java +++ /dev/null @@ -1,75 +0,0 @@ -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 static final double DEFAULT_TIMEOUT = 5; - protected HashMap statuses; - protected final String[] systemNames; - - 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 DefaultException()); - } - - 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 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 - } - } -} \ No newline at end of file diff --git a/commands/systemchecks/CheckGroup.java b/commands/systemchecks/CheckGroup.java index 87562674..f364e31d 100644 --- a/commands/systemchecks/CheckGroup.java +++ b/commands/systemchecks/CheckGroup.java @@ -1,17 +1,19 @@ package org.usfirst.frc4904.standard.commands.systemchecks; +import edu.wpi.first.wpilibj.command.Command; 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); - } - } - } + 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/NetworkTablesCheck.java b/commands/systemchecks/NetworkTablesCheck.java index 8242378c..ecaf8c2e 100644 --- a/commands/systemchecks/NetworkTablesCheck.java +++ b/commands/systemchecks/NetworkTablesCheck.java @@ -7,9 +7,8 @@ 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"; +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; diff --git a/commands/systemchecks/SystemCheck.java b/commands/systemchecks/SystemCheck.java index a9ca637b..3a9579d6 100644 --- a/commands/systemchecks/SystemCheck.java +++ b/commands/systemchecks/SystemCheck.java @@ -6,7 +6,7 @@ import java.util.stream.Collectors; import edu.wpi.first.wpilibj.SendableBase; -public abstract class SystemCheck extends BasicCheck { +public abstract class SystemCheck extends BaseCheck { protected HashMap statuses; protected SendableBase[] systems; diff --git a/subsystems/motor/PositionSensorMotor.java b/subsystems/motor/PositionSensorMotor.java index 9a4aa8ee..b733641e 100644 --- a/subsystems/motor/PositionSensorMotor.java +++ b/subsystems/motor/PositionSensorMotor.java @@ -9,25 +9,29 @@ 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) { + 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) { + + 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) { + public PositionSensorMotor(String name, Util.Range positionRange, MotionController motionController, + SpeedController... motors) { super(name, false, new IdentityModifier(), motionController, motors); this.positionRange = positionRange; } @@ -61,7 +65,7 @@ public PositionSensorMotor(SpeedModifier speedModifier, MotionController motionC public PositionSensorMotor(MotionController motionController, SpeedController... motors) { this("PositionSensorMotor", motionController, motors); } - + public void setPosition(double position) { motionController.setSetpoint(positionRange != null ? positionRange.limitValue(position) : position); } From 50aaf1aaf069a932215d68572f55f438b6ada78f Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Mon, 11 Feb 2019 23:31:29 -0800 Subject: [PATCH 18/23] overall review, documentation, and cleanup --- commands/systemchecks/BaseCheck.java | 77 +++++++++++++-- commands/systemchecks/BatteryCheck.java | 55 +++++++++++ commands/systemchecks/CANCheck.java | 55 +++++++++++ commands/systemchecks/Check.java | 72 +++++++++++--- commands/systemchecks/CheckGroup.java | 9 ++ commands/systemchecks/CompressorCheck.java | 93 ++++++++++++------- commands/systemchecks/DefaultException.java | 15 --- commands/systemchecks/DrivePIDCheck.java | 93 ++++++++++--------- commands/systemchecks/NetworkTablesCheck.java | 76 ++++++++------- commands/systemchecks/SolenoidCheck.java | 80 +++++++++++----- commands/systemchecks/StatusMessage.java | 10 +- commands/systemchecks/SubsystemCheck.java | 57 +++++++++--- commands/systemchecks/SystemCheck.java | 65 +++++++++---- commands/systemchecks/TurnPIDCheck.java | 4 +- commands/systemchecks/motor/MotorCheck.java | 2 +- .../systemchecks/motor/SensorMotorCheck.java | 6 +- commands/systemchecks/motor/ServoCheck.java | 2 +- 17 files changed, 568 insertions(+), 203 deletions(-) create mode 100644 commands/systemchecks/BatteryCheck.java create mode 100644 commands/systemchecks/CANCheck.java delete mode 100644 commands/systemchecks/DefaultException.java diff --git a/commands/systemchecks/BaseCheck.java b/commands/systemchecks/BaseCheck.java index ebb75061..b675f262 100644 --- a/commands/systemchecks/BaseCheck.java +++ b/commands/systemchecks/BaseCheck.java @@ -9,21 +9,44 @@ 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); } @@ -38,31 +61,73 @@ public void end() { outputStatuses(); } - public void setStatus(String name, SystemStatus status, Exception... exceptions) { - statuses.put(name, new StatusMessage(status, exceptions)); + /** + * 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)); } - public void initStatus(String name) { - setStatus(name, SystemStatus.PASS); + /** + * 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); } } - public void updateStatus(String key, StatusMessage.SystemStatus status, Exception... exceptions) { + /** + * 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 ordinance + * depending on the status of the system + */ public void outputStatuses() { - LogKitten.wtf(getName() + " Statuses:"); + LogKitten.d(getName() + " Statuses:"); for (Map.Entry entry : statuses.entrySet()) { String name = entry.getKey(); StatusMessage message = entry.getValue(); diff --git a/commands/systemchecks/BatteryCheck.java b/commands/systemchecks/BatteryCheck.java new file mode 100644 index 00000000..1d35dfe8 --- /dev/null +++ b/commands/systemchecks/BatteryCheck.java @@ -0,0 +1,55 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + + +import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; +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"; + + /** + * @param name + * name of check + * @param timeout + * duration of check + */ + public BatteryCheck(String name, double timeout) { + super(name, timeout, systemName); + } + + /** + * @param name + * name of check + * @param timeout + * duration of check + */ + public BatteryCheck(double timeout) { + super("RobotControllerCheck", timeout); + } + + /** + * @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..dd11a7fe --- /dev/null +++ b/commands/systemchecks/CANCheck.java @@ -0,0 +1,55 @@ +package org.usfirst.frc4904.standard.commands.systemchecks; + + +import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; +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 index 44a26290..79a83d42 100644 --- a/commands/systemchecks/Check.java +++ b/commands/systemchecks/Check.java @@ -4,23 +4,69 @@ import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; public interface Check { - public void setStatus(String key, StatusMessage.SystemStatus status, Exception... exceptions); + /** + * 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); - default void initStatus(String name) { - setStatus(name, SystemStatus.PASS, new Exception("NO ERROR")); - } + /** + * Initializes the status of system to PASS + * + * @param key + * name of system + */ + default void initStatus(String key) { + setStatus(key, SystemStatus.PASS); + } - public void initStatuses(); + /** + * Initializes statuses of all systems + */ + public void initStatuses(); - public void updateStatus(String key, StatusMessage.SystemStatus status, Exception... exceptions); + /** + * 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); - default void setStatusFail(String key, Exception... exceptions) { - updateStatus(key, SystemStatus.FAIL, 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); + } - default void setStatusPass(String key, Exception... exceptions) { - updateStatus(key, SystemStatus.PASS, exceptions); - } + /** + * Sets the status of a system to PASS + * + * @param key + * name of system + */ + default void setStatusPass(String key) { + setStatus(key, SystemStatus.PASS); + } - public void outputStatuses(); + /** + * 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 index f364e31d..075c3269 100644 --- a/commands/systemchecks/CheckGroup.java +++ b/commands/systemchecks/CheckGroup.java @@ -4,7 +4,16 @@ 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) { diff --git a/commands/systemchecks/CompressorCheck.java b/commands/systemchecks/CompressorCheck.java index 7c88f68d..18987197 100644 --- a/commands/systemchecks/CompressorCheck.java +++ b/commands/systemchecks/CompressorCheck.java @@ -4,38 +4,65 @@ import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; import edu.wpi.first.wpilibj.Compressor; +/** + * System check on compressors + */ 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")); - } - } - } + 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/DefaultException.java b/commands/systemchecks/DefaultException.java deleted file mode 100644 index 9e1ee34b..00000000 --- a/commands/systemchecks/DefaultException.java +++ /dev/null @@ -1,15 +0,0 @@ -package org.usfirst.frc4904.standard.commands.systemchecks; - - -public class DefaultException extends Exception { - protected static final long serialVersionUID = 4902L; - protected static final String DEFAULT_MESSAGE = "NO ERRORS"; - - public DefaultException(String message) { - super(message); - } - - public DefaultException() { - this(DEFAULT_MESSAGE); - } -} diff --git a/commands/systemchecks/DrivePIDCheck.java b/commands/systemchecks/DrivePIDCheck.java index 5d62fbe6..47428c0e 100644 --- a/commands/systemchecks/DrivePIDCheck.java +++ b/commands/systemchecks/DrivePIDCheck.java @@ -11,55 +11,62 @@ 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 HashMap 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; + protected HashMap 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 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 end() { + try { + position = motionController.getInputSafely(); + } + catch (InvalidSensorException e) { + position = 0; + updateStatusFail(CHECK_NAME, e); + } + if (Math.abs(motionController.getSetpoint() - position) > ERROR_THRESHOLD) { + updateStatusFail(CHECK_NAME, new Exception("DISTANCE DRIVEN NOT WITHIN ERROR THRESHOLD")); + } + chassisMove.cancel(); + motionController.disable(); + motionController.reset(); + runOnce = false; + } - public void initStatuses() { - initStatus(CHECK_NAME); - } + public void initStatuses() { + initStatus(CHECK_NAME); + } - public void setStatus(String name, SystemStatus status, Exception... exceptions) { - statuses.put(name, new StatusMessage(status, exceptions)); - } + 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 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 StatusMessage getStatusMessage(String key) { + return statuses.get(key); + } - public void outputStatuses() { - LogKitten.wtf(CHECK_NAME + ": " + getStatusMessage(CHECK_NAME).exceptions); // TODO: change logkitten level - } + public void outputStatuses() { + if (getStatusMessage(CHECK_NAME).status == SystemStatus.PASS) { + LogKitten.d(CHECK_NAME + ": PASS"); + } else { + LogKitten.e(CHECK_NAME + ": " + getStatusMessage(CHECK_NAME).exceptions); + } + } } \ No newline at end of file diff --git a/commands/systemchecks/NetworkTablesCheck.java b/commands/systemchecks/NetworkTablesCheck.java index ecaf8c2e..fb1cd880 100644 --- a/commands/systemchecks/NetworkTablesCheck.java +++ b/commands/systemchecks/NetworkTablesCheck.java @@ -7,42 +7,52 @@ 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; + 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; - public NetworkTablesCheck(double timeout) { - super("NetworkTableCheck", timeout, CHECK_NAME); - } + /** + * @param timeout + * duration of check + */ + public NetworkTablesCheck(double timeout) { + super("NetworkTableCheck", timeout, CHECK_NAME); + } - public NetworkTablesCheck() { - this(DEFAULT_TIMEOUT); - } + 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")); - } - } + /** + * 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; - } + @Override + public boolean isFinished() { + return true; + } } \ No newline at end of file diff --git a/commands/systemchecks/SolenoidCheck.java b/commands/systemchecks/SolenoidCheck.java index 8a9e75e8..67ac8b27 100644 --- a/commands/systemchecks/SolenoidCheck.java +++ b/commands/systemchecks/SolenoidCheck.java @@ -4,26 +4,64 @@ import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; import edu.wpi.first.wpilibj.DoubleSolenoid; +/** + * System Check of solenoids + */ 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); - } - } - } + 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 index 926652d8..4325bcc5 100644 --- a/commands/systemchecks/StatusMessage.java +++ b/commands/systemchecks/StatusMessage.java @@ -1,10 +1,18 @@ 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; diff --git a/commands/systemchecks/SubsystemCheck.java b/commands/systemchecks/SubsystemCheck.java index 517146cf..7a1d3b4f 100644 --- a/commands/systemchecks/SubsystemCheck.java +++ b/commands/systemchecks/SubsystemCheck.java @@ -3,23 +3,50 @@ import edu.wpi.first.wpilibj.command.Subsystem; +/** + * SystemCheck that takes in Subsystems + */ public abstract class SubsystemCheck extends SystemCheck { - public SubsystemCheck(String name, double timeout, Subsystem... subsystems) { - super(name, timeout, subsystems); - for (Subsystem system : subsystems) { - requires(system); - } - } + /** + * @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); + } + } - public SubsystemCheck(String name, Subsystem... systems) { - this(name, DEFAULT_TIMEOUT, systems); - } + /** + * @param name + * name of check + * @param subsystems + * subsystems being checked + */ + public SubsystemCheck(String name, Subsystem... systems) { + this(name, DEFAULT_TIMEOUT, systems); + } - public SubsystemCheck(double timeout, Subsystem... systems) { - this("SubsystemCheck", timeout, systems); - } + /** + * @param timeout + * duration of check + * @param subsystems + * subsystems being checked + */ + public SubsystemCheck(double timeout, Subsystem... systems) { + this("SubsystemCheck", timeout, systems); + } - public SubsystemCheck(Subsystem... systems) { - this("SubsystemCheck", 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 index 3a9579d6..65bcd491 100644 --- a/commands/systemchecks/SystemCheck.java +++ b/commands/systemchecks/SystemCheck.java @@ -6,25 +6,58 @@ 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; - protected SendableBase[] systems; + protected HashMap statuses; - 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; - } + /** + * 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)); + } - public SystemCheck(double timeout, SendableBase... systems) { - this("SystemCheck", timeout, systems); - } + /** + * 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); + } - public SystemCheck(String name, SendableBase... systems) { - this(name, DEFAULT_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); + } - public SystemCheck(SendableBase... systems) { - this("SystemCheck", 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 index 401480d3..5c4f0500 100644 --- a/commands/systemchecks/TurnPIDCheck.java +++ b/commands/systemchecks/TurnPIDCheck.java @@ -38,10 +38,10 @@ public void end() { } catch (InvalidSensorException e) { angle = 0; - updateStatus(CHECK_NAME, SystemStatus.FAIL, e); + updateStatusFail(CHECK_NAME, e); } if (Math.abs(motionController.getSetpoint() - angle) > errorThreshold) { - updateStatus(CHECK_NAME, SystemStatus.FAIL, new Exception("ANGLE TURNED NOT WITHIN ERROR THRESHOLD")); + updateStatusFail(CHECK_NAME, new Exception("ANGLE TURNED NOT WITHIN ERROR THRESHOLD")); } move.cancel(); motionController.disable(); diff --git a/commands/systemchecks/motor/MotorCheck.java b/commands/systemchecks/motor/MotorCheck.java index d1fdaa3b..9c4e6660 100644 --- a/commands/systemchecks/motor/MotorCheck.java +++ b/commands/systemchecks/motor/MotorCheck.java @@ -50,7 +50,7 @@ public void execute() { motor.set(speed); } catch (Exception e) { - updateStatus(motor.getName(), SystemStatus.FAIL, e); + updateStatusFail(motor.getName(), e); } } } diff --git a/commands/systemchecks/motor/SensorMotorCheck.java b/commands/systemchecks/motor/SensorMotorCheck.java index e1a832a3..bf47ecd9 100644 --- a/commands/systemchecks/motor/SensorMotorCheck.java +++ b/commands/systemchecks/motor/SensorMotorCheck.java @@ -67,17 +67,17 @@ public void execute() { } catch (InvalidSensorException e) { input = 0; - updateStatus(motor.getName(), SystemStatus.FAIL, e); + updateStatusFail(motor.getName(), e); } if (motor instanceof VelocitySensorMotor) { ((VelocitySensorMotor) motor).set(speed); if (input - speed > VELOCITY_THRESHOLD) { - updateStatus(motor.getName(), SystemStatus.FAIL, new Exception("SET SPEED NOT WITHIN REQUIRED THRESHOLD")); + updateStatusFail(motor.getName(), new Exception("SET SPEED NOT WITHIN REQUIRED THRESHOLD")); } } else if (motor instanceof PositionSensorMotor) { ((PositionSensorMotor) motor).setPosition(position); if (input - position > POSITION_THRESHOLD) { - updateStatus(motor.getName(), SystemStatus.FAIL, new Exception("POSITION NOT WITHIN THRESHOLD")); + updateStatusFail(motor.getName(), new Exception("POSITION NOT WITHIN THRESHOLD")); } } } diff --git a/commands/systemchecks/motor/ServoCheck.java b/commands/systemchecks/motor/ServoCheck.java index 312fe7a1..e0f3d2f3 100644 --- a/commands/systemchecks/motor/ServoCheck.java +++ b/commands/systemchecks/motor/ServoCheck.java @@ -34,7 +34,7 @@ public void initialize() { servo.setAngle(angle); } catch (Exception e) { - updateStatus(servo.getName(), SystemStatus.FAIL, e); + updateStatusFail(servo.getName(), e); } } } From 01614e3d37122902714aeb93850fca2525210d5c Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Tue, 12 Feb 2019 08:53:54 -0800 Subject: [PATCH 19/23] more formatting and docs --- commands/systemchecks/BaseCheck.java | 2 +- commands/systemchecks/BatteryCheck.java | 1 - commands/systemchecks/CANCheck.java | 1 - commands/systemchecks/CompressorCheck.java | 1 - commands/systemchecks/DrivePIDCheck.java | 5 +- commands/systemchecks/NetworkTablesCheck.java | 1 - commands/systemchecks/SolenoidCheck.java | 1 - commands/systemchecks/TurnPIDCheck.java | 105 ++++++++++-------- commands/systemchecks/motor/MotorCheck.java | 3 +- .../systemchecks/motor/SensorMotorCheck.java | 1 - commands/systemchecks/motor/ServoCheck.java | 65 ++++++----- 11 files changed, 94 insertions(+), 92 deletions(-) diff --git a/commands/systemchecks/BaseCheck.java b/commands/systemchecks/BaseCheck.java index b675f262..671b73a0 100644 --- a/commands/systemchecks/BaseCheck.java +++ b/commands/systemchecks/BaseCheck.java @@ -123,7 +123,7 @@ public StatusMessage getStatusMessage(String key) { } /** - * Logs the statuses of all systems with variable ordinance + * Logs the statuses of all systems with variable severity * depending on the status of the system */ public void outputStatuses() { diff --git a/commands/systemchecks/BatteryCheck.java b/commands/systemchecks/BatteryCheck.java index 1d35dfe8..2d40c293 100644 --- a/commands/systemchecks/BatteryCheck.java +++ b/commands/systemchecks/BatteryCheck.java @@ -1,7 +1,6 @@ package org.usfirst.frc4904.standard.commands.systemchecks; -import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; import edu.wpi.first.hal.can.CANStatus; import edu.wpi.first.wpilibj.RobotController; diff --git a/commands/systemchecks/CANCheck.java b/commands/systemchecks/CANCheck.java index dd11a7fe..948bbe53 100644 --- a/commands/systemchecks/CANCheck.java +++ b/commands/systemchecks/CANCheck.java @@ -1,7 +1,6 @@ package org.usfirst.frc4904.standard.commands.systemchecks; -import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; import edu.wpi.first.hal.can.CANStatus; import edu.wpi.first.wpilibj.RobotController; diff --git a/commands/systemchecks/CompressorCheck.java b/commands/systemchecks/CompressorCheck.java index 18987197..f81f1e3e 100644 --- a/commands/systemchecks/CompressorCheck.java +++ b/commands/systemchecks/CompressorCheck.java @@ -1,7 +1,6 @@ package org.usfirst.frc4904.standard.commands.systemchecks; -import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; import edu.wpi.first.wpilibj.Compressor; /** diff --git a/commands/systemchecks/DrivePIDCheck.java b/commands/systemchecks/DrivePIDCheck.java index 47428c0e..ef5d8133 100644 --- a/commands/systemchecks/DrivePIDCheck.java +++ b/commands/systemchecks/DrivePIDCheck.java @@ -66,7 +66,10 @@ public void outputStatuses() { if (getStatusMessage(CHECK_NAME).status == SystemStatus.PASS) { LogKitten.d(CHECK_NAME + ": PASS"); } else { - LogKitten.e(CHECK_NAME + ": " + getStatusMessage(CHECK_NAME).exceptions); + 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 index fb1cd880..7c694d30 100644 --- a/commands/systemchecks/NetworkTablesCheck.java +++ b/commands/systemchecks/NetworkTablesCheck.java @@ -1,7 +1,6 @@ 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; diff --git a/commands/systemchecks/SolenoidCheck.java b/commands/systemchecks/SolenoidCheck.java index 67ac8b27..c5763dcf 100644 --- a/commands/systemchecks/SolenoidCheck.java +++ b/commands/systemchecks/SolenoidCheck.java @@ -1,7 +1,6 @@ package org.usfirst.frc4904.standard.commands.systemchecks; -import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; import edu.wpi.first.wpilibj.DoubleSolenoid; /** diff --git a/commands/systemchecks/TurnPIDCheck.java b/commands/systemchecks/TurnPIDCheck.java index 5c4f0500..7675113e 100644 --- a/commands/systemchecks/TurnPIDCheck.java +++ b/commands/systemchecks/TurnPIDCheck.java @@ -13,61 +13,68 @@ import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; 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; + 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; - public TurnPIDCheck(Chassis chassis, double finalAngle, IMU imu, MotionController motionController, double errorThreshold) { - super(chassis, finalAngle, imu, motionController); - this.finalAngle = finalAngle; - this.errorThreshold = errorThreshold; - initStatuses(); - } + public TurnPIDCheck(Chassis chassis, double finalAngle, IMU imu, MotionController motionController, double errorThreshold) { + super(chassis, finalAngle, imu, motionController); + this.finalAngle = finalAngle; + this.errorThreshold = errorThreshold; + initStatuses(); + } - public TurnPIDCheck(Chassis chassis, double finalAngle, IMU imu, MotionController motionController) { - this(chassis, finalAngle, imu, motionController, DEFAULT_THRESHOLD); - } + public TurnPIDCheck(Chassis chassis, double finalAngle, IMU imu, MotionController motionController) { + this(chassis, finalAngle, imu, motionController, DEFAULT_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; - } + @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; + } - public void initStatuses() { - initStatus(CHECK_NAME); - } + public void initStatuses() { + initStatus(CHECK_NAME); + } - public void setStatus(String name, SystemStatus status, Exception... exceptions) { - statuses.put(name, new StatusMessage(status, exceptions)); - } + 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 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 StatusMessage getStatusMessage(String key) { + return statuses.get(key); + } - public void outputStatuses() { - LogKitten.wtf(CHECK_NAME + ": " + getStatusMessage(CHECK_NAME).exceptions); // TODO: change logkitten level - } + 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 index 9c4e6660..bd0c02de 100644 --- a/commands/systemchecks/motor/MotorCheck.java +++ b/commands/systemchecks/motor/MotorCheck.java @@ -4,10 +4,9 @@ import org.usfirst.frc4904.standard.Util; import org.usfirst.frc4904.standard.commands.systemchecks.SubsystemCheck; import org.usfirst.frc4904.standard.subsystems.motor.Motor; -import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; public class MotorCheck extends SubsystemCheck { - protected static final double DEFAULT_SPEED = 0.5; // TODO: CHECK THIS + protected static final double DEFAULT_SPEED = 0.5; protected static final Util.Range outputCurrentRange = new Util.Range(0.1, 0.3); // TODO: Use Current to judge speedcontrollers protected double speed; protected final Motor[] motors; diff --git a/commands/systemchecks/motor/SensorMotorCheck.java b/commands/systemchecks/motor/SensorMotorCheck.java index bf47ecd9..45be5af9 100644 --- a/commands/systemchecks/motor/SensorMotorCheck.java +++ b/commands/systemchecks/motor/SensorMotorCheck.java @@ -1,7 +1,6 @@ package org.usfirst.frc4904.standard.commands.systemchecks.motor; -import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor; import org.usfirst.frc4904.standard.subsystems.motor.SensorMotor; diff --git a/commands/systemchecks/motor/ServoCheck.java b/commands/systemchecks/motor/ServoCheck.java index e0f3d2f3..ff00c37b 100644 --- a/commands/systemchecks/motor/ServoCheck.java +++ b/commands/systemchecks/motor/ServoCheck.java @@ -2,40 +2,39 @@ import org.usfirst.frc4904.standard.commands.systemchecks.SubsystemCheck; -import org.usfirst.frc4904.standard.commands.systemchecks.StatusMessage.SystemStatus; import org.usfirst.frc4904.standard.subsystems.motor.ServoSubsystem; public class ServoCheck extends SubsystemCheck { - protected static final double DEFAULT_ANGLE = 50; - protected final double angle; - protected final ServoSubsystem[] servos; - - public ServoCheck(String name, double angle, ServoSubsystem... servos) { - super(name, servos); - this.servos = servos; - this.angle = angle; - } - - public ServoCheck(String name, ServoSubsystem... servos) { - this(name, DEFAULT_ANGLE, servos); - } - - public ServoCheck(double angle, ServoSubsystem... servos) { - this("ServoCheck", angle, servos); - } - - public ServoCheck(ServoSubsystem... servos) { - this("ServoCheck", servos); - } - - public void initialize() { - for (ServoSubsystem servo : servos) { - try { - servo.setAngle(angle); - } - catch (Exception e) { - updateStatusFail(servo.getName(), e); - } - } - } + protected static final double DEFAULT_ANGLE = 50; + protected final double angle; + protected final ServoSubsystem[] servos; + + public ServoCheck(String name, double angle, ServoSubsystem... servos) { + super(name, servos); + this.servos = servos; + this.angle = angle; + } + + public ServoCheck(String name, ServoSubsystem... servos) { + this(name, DEFAULT_ANGLE, servos); + } + + public ServoCheck(double angle, ServoSubsystem... servos) { + this("ServoCheck", angle, servos); + } + + public ServoCheck(ServoSubsystem... servos) { + this("ServoCheck", servos); + } + + public void initialize() { + for (ServoSubsystem servo : servos) { + try { + servo.setAngle(angle); + } + catch (Exception e) { + updateStatusFail(servo.getName(), e); + } + } + } } \ No newline at end of file From f6712f489ed8112ba679b72301f60ff59233c2dd Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Tue, 12 Feb 2019 08:56:52 -0800 Subject: [PATCH 20/23] more formatting and docs --- commands/systemchecks/TurnPIDCheck.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/commands/systemchecks/TurnPIDCheck.java b/commands/systemchecks/TurnPIDCheck.java index 7675113e..7cc6b600 100644 --- a/commands/systemchecks/TurnPIDCheck.java +++ b/commands/systemchecks/TurnPIDCheck.java @@ -12,6 +12,9 @@ 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"; From 0a6aae7b3ab09401e9f9fb230fe7b3b5994e9092 Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Tue, 12 Feb 2019 09:38:50 -0800 Subject: [PATCH 21/23] fixes docs --- commands/systemchecks/BatteryCheck.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/commands/systemchecks/BatteryCheck.java b/commands/systemchecks/BatteryCheck.java index 2d40c293..c41a08f1 100644 --- a/commands/systemchecks/BatteryCheck.java +++ b/commands/systemchecks/BatteryCheck.java @@ -13,6 +13,8 @@ public class BatteryCheck extends BaseCheck { protected static final String systemName = "BATTERY"; /** + * Systemcheck of battery status + * * @param name * name of check * @param timeout @@ -23,6 +25,8 @@ public BatteryCheck(String name, double timeout) { } /** + * Systemcheck of battery status + * * @param name * name of check * @param timeout @@ -33,6 +37,8 @@ public BatteryCheck(double timeout) { } /** + * Systemcheck of battery status + * * @param name * name of check * @param timeout From b285dc0520899d7c680bf3bf2dc0bd6dcbc96c84 Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Wed, 13 Feb 2019 22:15:11 -0800 Subject: [PATCH 22/23] more docs, added voltage and current checks for motors --- commands/systemchecks/motor/MotorCheck.java | 96 +++++++++++- .../motor/PositionMotorCheck.java | 51 +++++- .../systemchecks/motor/SensorMotorCheck.java | 121 ++++++++++++++- commands/systemchecks/motor/ServoCheck.java | 38 +++++ .../motor/VelocityMotorCheck.java | 69 +++++++-- subsystems/motor/Motor.java | 145 ++++++++++-------- 6 files changed, 427 insertions(+), 93 deletions(-) diff --git a/commands/systemchecks/motor/MotorCheck.java b/commands/systemchecks/motor/MotorCheck.java index bd0c02de..3643e296 100644 --- a/commands/systemchecks/motor/MotorCheck.java +++ b/commands/systemchecks/motor/MotorCheck.java @@ -1,48 +1,115 @@ 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: Use Current to judge speedcontrollers + 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 { @@ -51,6 +118,33 @@ public void execute() { 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 index 7e4c723e..e6ded29d 100644 --- a/commands/systemchecks/motor/PositionMotorCheck.java +++ b/commands/systemchecks/motor/PositionMotorCheck.java @@ -3,20 +3,55 @@ import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor; +/** + * Systemcheck of PositionSensorMotors + */ public class PositionMotorCheck extends SensorMotorCheck { - public PositionMotorCheck(String name, double setPosition, PositionSensorMotor... positionMotors) { - super(name, setPosition, positionMotors); + /** + * 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); } - public PositionMotorCheck(String name, PositionSensorMotor... positionMotors) { - this(name, DEFAULT_POSITION, positionMotors); + /** + * Systemcheck of PositionSensorMotors + * + * @param name + * Name of check + * @param motors + * PositionSensorMotors to test + */ + public PositionMotorCheck(String name, PositionSensorMotor... motors) { + this(name, DEFAULT_POSITION, motors); } - public PositionMotorCheck(double setPosition, PositionSensorMotor... positionMotors) { - this("PositionMotorCheck", setPosition, positionMotors); + /** + * Systemcheck of PositionSensorMotors + * + * @param position + * Position to set PositionSensorMotors + * @param motors + * PositionSensorMotors to test + */ + public PositionMotorCheck(double position, PositionSensorMotor... motors) { + this("PositionMotorCheck", position, motors); } - public PositionMotorCheck(PositionSensorMotor... positionMotors) { - this("PositionMotorCheck", positionMotors); + /** + * 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 index 45be5af9..073fcd41 100644 --- a/commands/systemchecks/motor/SensorMotorCheck.java +++ b/commands/systemchecks/motor/SensorMotorCheck.java @@ -6,57 +6,151 @@ 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 SensorMotor[] motors; + 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) { @@ -70,15 +164,32 @@ public void execute() { } if (motor instanceof VelocitySensorMotor) { ((VelocitySensorMotor) motor).set(speed); - if (input - speed > VELOCITY_THRESHOLD) { + VICheck(motor); + if (((VelocitySensorMotor) motor).getMotionController().onTarget() && input - speed > VELOCITY_THRESHOLD) { updateStatusFail(motor.getName(), new Exception("SET SPEED NOT WITHIN REQUIRED THRESHOLD")); } } else if (motor instanceof PositionSensorMotor) { - ((PositionSensorMotor) motor).setPosition(position); - if (input - position > POSITION_THRESHOLD) { + 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 index ff00c37b..88623fca 100644 --- a/commands/systemchecks/motor/ServoCheck.java +++ b/commands/systemchecks/motor/ServoCheck.java @@ -4,29 +4,67 @@ 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 { diff --git a/commands/systemchecks/motor/VelocityMotorCheck.java b/commands/systemchecks/motor/VelocityMotorCheck.java index a59df809..d9c3d9d6 100644 --- a/commands/systemchecks/motor/VelocityMotorCheck.java +++ b/commands/systemchecks/motor/VelocityMotorCheck.java @@ -1,28 +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 { - - public VelocityMotorCheck(String name, double timeout, double speed, VelocitySensorMotor... velocityMotors) { - super(name, timeout, speed, 0, velocityMotors); + /** + * 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); } - public VelocityMotorCheck(String name, double speed, VelocitySensorMotor... velocityMotors) { - super(name, speed, 0, velocityMotors); + /** + * 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); } - public VelocityMotorCheck(String name, VelocitySensorMotor... velocityMotors) { - this(name, DEFAULT_SPEED, velocityMotors); + /** + * Systemcheck of VelocitySensorMotors + * + * @param name + * Name of check + * @param motors + * VelocitySensorMotors to test + */ + public VelocityMotorCheck(String name, VelocitySensorMotor... motors) { + this(name, DEFAULT_SPEED, motors); } - public VelocityMotorCheck(double speed, VelocitySensorMotor... velocityMotors) { - this("VelocityMotorCheck", speed, velocityMotors); + /** + * Systemcheck of VelocitySensorMotors + * + * @param speed + * Speed to set VelocitySensorMotors + * @param motors + * VelocitySensorMotors to test + */ + public VelocityMotorCheck(double speed, VelocitySensorMotor... motors) { + this("VelocityMotorCheck", speed, motors); } - public VelocityMotorCheck(VelocitySensorMotor... velocityMotors) { - this("VelocityMotorCheck", velocityMotors); + /** + * 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."); } } } From 0bf66547d0d5921aa513916febe91fa1d5773843 Mon Sep 17 00:00:00 2001 From: Nikhil Suresh Date: Wed, 13 Feb 2019 22:46:05 -0800 Subject: [PATCH 23/23] pr changes --- commands/systemchecks/BatteryCheck.java | 2 - commands/systemchecks/DrivePIDCheck.java | 55 +++++++++++++++++-- commands/systemchecks/TurnPIDCheck.java | 45 +++++++++++++++ .../systemchecks/motor/SensorMotorCheck.java | 2 +- 4 files changed, 97 insertions(+), 7 deletions(-) diff --git a/commands/systemchecks/BatteryCheck.java b/commands/systemchecks/BatteryCheck.java index c41a08f1..e0829329 100644 --- a/commands/systemchecks/BatteryCheck.java +++ b/commands/systemchecks/BatteryCheck.java @@ -27,8 +27,6 @@ public BatteryCheck(String name, double timeout) { /** * Systemcheck of battery status * - * @param name - * name of check * @param timeout * duration of check */ diff --git a/commands/systemchecks/DrivePIDCheck.java b/commands/systemchecks/DrivePIDCheck.java index ef5d8133..addd20f5 100644 --- a/commands/systemchecks/DrivePIDCheck.java +++ b/commands/systemchecks/DrivePIDCheck.java @@ -15,18 +15,49 @@ * Checks the accuracy of DrivePID by comparing the position of robot to setpoint */ public class DrivePIDCheck extends ChassisMoveDistance implements Check { - protected HashMap statuses; + 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 static final double ERROR_THRESHOLD = 2.0; // TODO: CHANGE THIS protected double position; + protected final double errorThreshold; - public DrivePIDCheck(Chassis chassis, double distance, MotionController motionController) { + /** + * 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(); @@ -35,33 +66,49 @@ public void end() { position = 0; updateStatusFail(CHECK_NAME, e); } - if (Math.abs(motionController.getSetpoint() - position) > ERROR_THRESHOLD) { + 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"); diff --git a/commands/systemchecks/TurnPIDCheck.java b/commands/systemchecks/TurnPIDCheck.java index 7cc6b600..c9095ba0 100644 --- a/commands/systemchecks/TurnPIDCheck.java +++ b/commands/systemchecks/TurnPIDCheck.java @@ -23,6 +23,20 @@ public class TurnPIDCheck extends ChassisTurn implements Check { 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; @@ -30,10 +44,25 @@ public TurnPIDCheck(Chassis chassis, double finalAngle, IMU imu, MotionControlle 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 { @@ -50,26 +79,42 @@ public void end() { 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"); diff --git a/commands/systemchecks/motor/SensorMotorCheck.java b/commands/systemchecks/motor/SensorMotorCheck.java index 073fcd41..138453d3 100644 --- a/commands/systemchecks/motor/SensorMotorCheck.java +++ b/commands/systemchecks/motor/SensorMotorCheck.java @@ -166,7 +166,7 @@ public void execute() { ((VelocitySensorMotor) motor).set(speed); VICheck(motor); if (((VelocitySensorMotor) motor).getMotionController().onTarget() && input - speed > VELOCITY_THRESHOLD) { - updateStatusFail(motor.getName(), new Exception("SET SPEED NOT WITHIN REQUIRED THRESHOLD")); + updateStatusFail(motor.getName(), new Exception("SPEED NOT WITHIN REQUIRED THRESHOLD")); } } else if (motor instanceof PositionSensorMotor) { if (!motor.getMotionController().onTarget()) {