From b8cfb4ec88b9aebd0f3cba81abbf05f0a4b8c20a Mon Sep 17 00:00:00 2001 From: BenBernardCIS Date: Sat, 21 Apr 2018 17:00:12 -0400 Subject: [PATCH] Added intake selftest. --- .../robot/commands/testing/FullSelfTest.java | 1 + .../robot/commands/testing/TestCarriage.java | 3 +- .../robot/commands/testing/TestIntake.java | 132 ++++++++++++++++++ 3 files changed, 135 insertions(+), 1 deletion(-) create mode 100644 src/main/java/org/frc5687/powerup/robot/commands/testing/TestIntake.java diff --git a/src/main/java/org/frc5687/powerup/robot/commands/testing/FullSelfTest.java b/src/main/java/org/frc5687/powerup/robot/commands/testing/FullSelfTest.java index 8f57c691..d609c792 100644 --- a/src/main/java/org/frc5687/powerup/robot/commands/testing/FullSelfTest.java +++ b/src/main/java/org/frc5687/powerup/robot/commands/testing/FullSelfTest.java @@ -21,6 +21,7 @@ public FullSelfTest(Robot robot) { addSequential(new ConfirmTest(robot.getOI(), "Please be sure that the arm is clear and press Start to continue.", "Test started.", "Test aborted.")); addSequential(new ConfirmTest(robot.getOI(), "Please be sure that the intake is clear and press Start to continue.", "Test started.", "Test aborted.")); + addSequential(new TestIntake(robot.getIntake(), robot.getPDP(), robot.getLights())); addSequential(new ConfirmTest(robot.getOI(), "Tests complete.", "Test started.", "Test aborted.")); diff --git a/src/main/java/org/frc5687/powerup/robot/commands/testing/TestCarriage.java b/src/main/java/org/frc5687/powerup/robot/commands/testing/TestCarriage.java index 9613badc..36b794d1 100644 --- a/src/main/java/org/frc5687/powerup/robot/commands/testing/TestCarriage.java +++ b/src/main/java/org/frc5687/powerup/robot/commands/testing/TestCarriage.java @@ -1,6 +1,7 @@ package org.frc5687.powerup.robot.commands.testing; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.CommandGroup; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.frc5687.powerup.robot.Constants; @@ -14,7 +15,7 @@ import org.frc5687.powerup.robot.subsystems.Lights; import org.frc5687.powerup.robot.utils.PDP; -public class TestCarriage extends CommandGroup { +public class TestCarriage extends Command { private Carriage _carriage; private PDP _pdp; private Lights _lights; diff --git a/src/main/java/org/frc5687/powerup/robot/commands/testing/TestIntake.java b/src/main/java/org/frc5687/powerup/robot/commands/testing/TestIntake.java new file mode 100644 index 00000000..e5ed1c5d --- /dev/null +++ b/src/main/java/org/frc5687/powerup/robot/commands/testing/TestIntake.java @@ -0,0 +1,132 @@ +package org.frc5687.powerup.robot.commands.testing; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.frc5687.powerup.robot.Constants; +import org.frc5687.powerup.robot.RobotMap; +import org.frc5687.powerup.robot.commands.auto.paths.FarLeftToRightScaleDeadPartOne; +import org.frc5687.powerup.robot.subsystems.Carriage; +import org.frc5687.powerup.robot.subsystems.Intake; +import org.frc5687.powerup.robot.subsystems.Lights; +import org.frc5687.powerup.robot.utils.PDP; + +public class TestIntake extends Command { + private Intake _intake; + private PDP _pdp; + private Lights _lights; + + private State _state = State.LEFT; + private long _endMillis; + + private static long _runMillis = 500; + + private final double _targetAmps = 3; + + private boolean _pass = true; + + private double _maxAmps = 0; + + public TestIntake(Intake intake, PDP pdp, Lights lights){ + // Run the carriage up to the hall efect + requires(intake); + + _intake = intake; + _pdp = pdp; + _lights = lights; + + } + + @Override + protected void initialize() { + DriverStation.reportError("Starting intake test", false); + _state = State.LEFT; + _maxAmps = 0; + _endMillis = System.currentTimeMillis() + _runMillis; + } + + protected void execute() { + + switch (_state) { + case LEFT: + _lights.setBoth(Constants.Lights.TEST_RUNNING, Constants.Lights.TEST_RUNNING); + _intake.drive(Constants.Intake.INTAKE_SPEED, 0); + _maxAmps = Math.max(_maxAmps, _pdp.getCurrent(RobotMap.PDP.INTAKE_LEFT_SP)); + if (System.currentTimeMillis() > _endMillis) { + _intake.drive(0, 0); + _state = State.LEFTDONE; + } + break; + case LEFTDONE: + if (_maxAmps < _targetAmps) { + _pass = false; + SmartDashboard.putBoolean("SelfTest/Intake/Left/Amps/Passed", false); + DriverStation.reportError("Target amperage not reached on left intake. Expected " + _targetAmps + " but measured " + _maxAmps + ".", false); + } else { + _pass = true; + SmartDashboard.putBoolean("SelfTest/Intake/Left/Amps/Passed", true); + DriverStation.reportError("Amp draw passed on left intake. Expected " + _targetAmps + " and measured " + _maxAmps + ".", false); + } + _maxAmps = 0; + _endMillis = System.currentTimeMillis() + _runMillis; + _state = State.RIGHT; + break; + case RIGHT: + _lights.setBoth(Constants.Lights.TEST_RUNNING, Constants.Lights.TEST_RUNNING); + _intake.drive(0, Constants.Intake.INTAKE_SPEED); + _maxAmps = Math.max(_maxAmps, _pdp.getCurrent(RobotMap.PDP.INTAKE_RIGHT_SP_PROTO)); + if (System.currentTimeMillis() > _endMillis) { + _intake.drive(0, 0); + _state = State.RIGHTDONE; + } + break; + case RIGHTDONE: + if (_maxAmps < _targetAmps) { + _pass = false; + SmartDashboard.putBoolean("SelfTest/Intake/Right/Amps/Passed", false); + DriverStation.reportError("Target amperage not reached on right intake. Expected " + _targetAmps + " but measured " + _maxAmps + ".", false); + } else { + _pass = true; + SmartDashboard.putBoolean("SelfTest/Intake/Right/Amps/Passed", true); + DriverStation.reportError("Amp draw passed on right intake. Expected " + _targetAmps + " and measured " + _maxAmps + ".", false); + } + _endMillis = System.currentTimeMillis() + _runMillis; + _state = State.WAIT; + break; + case WAIT: + if (!_intake.cubeIsSecured()) { + SmartDashboard.putBoolean("SelfTest/Intake/CubeIR", false); + DriverStation.reportError("Cube not detected .", false); + } else { + SmartDashboard.putBoolean("SelfTest/Intake/CubeIR", true); + DriverStation.reportError("Cube detected .", false); + } + _intake.drive(0,0 ); + if (System.currentTimeMillis() > _endMillis) { + _state = State.DONE; + } + break; + } + } + + @Override + protected void end() { + _intake.drive(0, 0); + } + + @Override + protected boolean isFinished() { + return _state == State.DONE; + } + + private enum State { + LEFT, + LEFTDONE, + RIGHT, + RIGHTDONE, + WAIT, + DONE + } + +}