From 44a32ce0ad8f8622722992f9bb9225f3d827ad0b Mon Sep 17 00:00:00 2001 From: Ryan Olney Date: Mon, 26 Feb 2018 17:37:40 -0800 Subject: [PATCH 1/3] Added Bling stuff to buttons --- src/com/spartronics4915/frc2018/Robot.java | 37 +++++---- .../frc2018/subsystems/Harvester.java | 9 ++- .../frc2018/subsystems/LED.java | 78 ++++++++----------- 3 files changed, 56 insertions(+), 68 deletions(-) diff --git a/src/com/spartronics4915/frc2018/Robot.java b/src/com/spartronics4915/frc2018/Robot.java index 1607546..fe2fa79 100755 --- a/src/com/spartronics4915/frc2018/Robot.java +++ b/src/com/spartronics4915/frc2018/Robot.java @@ -263,6 +263,8 @@ public void teleopInit() mDrive.setOpenLoop(DriveSignal.NEUTRAL); mLED.setVisionLampOn(); + mLED.setBlingState(BlingState.SOLID); + mLED.setBlingState(BlingState.BLUE); } catch (Throwable t) { @@ -299,19 +301,22 @@ public void teleopPeriodic() { mLifter.setWantedState(ScissorLift.WantedState.OFF); mGrabber.setWantedState(ArticulatedGrabber.WantedState.TRANSPORT); - mLED.setBlingState(BlingState.SCISSOR_OFF); + mLED.setBlingState(BlingState.SOLID); + mLED.setBlingState(BlingState.YELLOW); } if (mControlBoard.readButton(Buttons.SCISSOR_SWITCH)) { mLifter.setWantedState(ScissorLift.WantedState.SWITCH); - mLED.setBlingState(BlingState.SCISSOR_SWITCH); + mLED.setBlingState(BlingState.BLINK); + mLED.setBlingState(BlingState.YELLOW); } if (mControlBoard.readButton(Buttons.SCISSOR_SCALE)) { mLifter.setWantedState(ScissorLift.WantedState.SCALE); - mLED.setBlingState(BlingState.SCISSOR_SCALE); + mLED.setBlingState(BlingState.FAST_BLINK); + mLED.setBlingState(BlingState.YELLOW); } if (mControlBoard.readButton(Buttons.GRABBER_DROP_CUBE)) @@ -322,37 +327,43 @@ public void teleopPeriodic() if (mControlBoard.readButton(Buttons.HARVESTER_OPEN)) { mHarvester.setWantedState(Harvester.WantedState.OPEN); - mLED.setBlingState(BlingState.OPEN_HARVESTER); + mLED.setBlingState(BlingState.SOLID); + mLED.setBlingState(BlingState.BLUE); } if (mControlBoard.readButton(Buttons.HARVESTER_CLOSE)) { mHarvester.setWantedState(Harvester.WantedState.HARVEST); - mLED.setBlingState(BlingState.CLOSE_HARVESTER); + mLED.setBlingState(BlingState.BLINK); + mLED.setBlingState(BlingState.BLUE); } if (mControlBoard.readButton(Buttons.HARVESTER_EJECT)) { mHarvester.setWantedState(Harvester.WantedState.EJECT); - mLED.setBlingState(BlingState.EJECT_HARVESTER); + mLED.setBlingState(BlingState.FAST_BLINK); + mLED.setBlingState(BlingState.BLUE); } if (mControlBoard.readButton(Buttons.SUPERSTRUCTURE_CARRY_CUBE)) { mSuperstructure.setWantedState(Superstructure.WantedState.TRANSFER_CUBE_TO_GRABBER); - mLED.setBlingState(BlingState.CARRY_CUBE); + mLED.setBlingState(BlingState.BLINK); + mLED.setBlingState(BlingState.GREEN); } if (mControlBoard.readButton(Buttons.HARVESTER_CLIMB)) { mSuperstructure.setWantedState(Superstructure.WantedState.CLIMB); - mLED.setBlingState(BlingState.CLIMB); + mLED.setBlingState(BlingState.SOLID); + mLED.setBlingState(BlingState.RED); } if (mControlBoard.readButton(Buttons.CLIMBER_STOP)) { mClimber.setWantedState(Climber.WantedState.HOLD); - mLED.setBlingState(BlingState.STOP_CLIMBER); + mLED.setBlingState(BlingState.BLINK); + mLED.setBlingState(BlingState.RED); } if (mControlBoard.readButton(Buttons.CLIMB_IDLE_TEST)) @@ -379,14 +390,6 @@ public void teleopPeriodic() { mGrabber.setWantedState(ArticulatedGrabber.WantedState.PREPARE_INTAKE); } - if (DriverStation.getInstance().getMatchTime() < kMatchDurationSeconds) - { - mLED.setBlingState(BlingState.TELEOP); - } - if (DriverStation.getInstance().getMatchTime() < kEndgameDurationSeconds) - { - mLED.setBlingState(BlingState.ENDGAME); - } allButTestPeriodic(); } catch (Throwable t) diff --git a/src/com/spartronics4915/frc2018/subsystems/Harvester.java b/src/com/spartronics4915/frc2018/subsystems/Harvester.java index d3741e0..d315911 100644 --- a/src/com/spartronics4915/frc2018/subsystems/Harvester.java +++ b/src/com/spartronics4915/frc2018/subsystems/Harvester.java @@ -203,7 +203,7 @@ private SystemState handleClosing() //motors off and bars in mMotorLeft.set(0.0); mMotorRight.set(0.0); - if (mWantedState == WantedState.OPEN || mWantedState == WantedState.AUTOHARVEST) + if (mWantedState == WantedState.OPEN || mWantedState == WantedState.AUTOHARVEST) { return defaultStateTransfer(); //all defaultStateTransfers return the wanted state } @@ -227,7 +227,7 @@ private SystemState handleOpening() } return SystemState.OPENING; } - + private SystemState handleAutoHarvesting() { mMotorLeft.set(0.0); @@ -288,7 +288,7 @@ private SystemState handleHugging() } return SystemState.HUGGING; } - + public WantedState getWantedState() { return mWantedState; @@ -346,7 +346,8 @@ private boolean isCubeHeld() @Override public void outputToSmartDashboard() { - if(!isInitialized()) return; + if (!isInitialized()) + return; dashboardPutState(mSystemState.toString()); dashboardPutWantedState(mWantedState.toString()); dashboardPutBoolean("mSolenoid", mSolenoid.get()); diff --git a/src/com/spartronics4915/frc2018/subsystems/LED.java b/src/com/spartronics4915/frc2018/subsystems/LED.java index d0043d1..71b28e8 100755 --- a/src/com/spartronics4915/frc2018/subsystems/LED.java +++ b/src/com/spartronics4915/frc2018/subsystems/LED.java @@ -54,7 +54,7 @@ public enum SystemState { OFF, FIXED_ON, BLINKING, RANGE_FINDING } - + public enum WantedState { OFF, FIXED_ON, BLINK, FIND_RANGE, WARN @@ -62,7 +62,7 @@ public enum WantedState public enum BlingState { - OFF, SCISSOR_OFF, SCISSOR_SWITCH, TELEOP, ENDGAME, SCISSOR_SCALE, CLIMB, OPEN_HARVESTER, CLOSE_HARVESTER, EJECT_HARVESTER, CARRY_CUBE, STOP_CLIMBER + OFF, YELLOW, BLINK, GREEN, SOLID, FAST_BLINK, CLIMB, BLUE, CLOSE_HARVESTER, EJECT_HARVESTER, CARRY_CUBE, RED } private SystemState mSystemState = SystemState.OFF; @@ -80,29 +80,25 @@ public enum BlingState /* *0 : OFF(GREY) - *1 : ? - *2 : ? - *3 : ? - *4 : ? + *1 : BLUE + *2 : YELLOW + *3 : RED + *4 : GREEN *5 : ? *6 : ? - *7 : ? - *8 : ? - *9 : ? + *7 : FAST BLINK + *8 : SOLID + *9 : BLINK */ - private final byte[] kOff = "0".getBytes(); //GREY - private final byte[] kScissorOff = "1".getBytes(); //YELLOW/BLUE ALTERNATING || FADE BLUE INTO YELLOW AND BACK - private final byte[] kScissorSwitch = "2".getBytes(); //YELLOW - private final byte[] kScissorScale = "3".getBytes(); //FLASHING YELLOW - private final byte[] kTeleop = "4".getBytes(); //FADE GREEN - private final byte[] kEndGame = "5".getBytes(); //FLASHING GREEN - private final byte[] kClimb = "6".getBytes(); //FLASHING RED - private final byte[] kStopClimber = "7".getBytes(); //RED - private final byte[] kOpenHarvester = "8".getBytes(); //BLUE - private final byte[] kCloseHarvester = "9".getBytes(); //FLASHING BLUE - private final byte[] kEjectHarvester = "10".getBytes(); //FADE BLUE - private final byte[] kCarryCube = "11".getBytes(); //YELLOW CHASING BLUE || FADE YELLOW + private final byte[] kOff = "0".getBytes(); + private final byte[] kBlue = "1".getBytes(); + private final byte[] kYellow = "2".getBytes(); + private final byte[] kRed = "3".getBytes(); + private final byte[] kGreen = "4".getBytes(); + private final byte[] kFastBlink = "7".getBytes(); + private final byte[] kSolid = "8".getBytes(); + private final byte[] kBlink = "9".getBytes(); public LED() { @@ -318,38 +314,26 @@ public synchronized void setBlingState(BlingState b) case OFF: mBling.write(kOff, kOff.length); break; - case SCISSOR_OFF: - mBling.write(kScissorOff, kScissorOff.length); - break; - case SCISSOR_SWITCH: - mBling.write(kScissorSwitch, kScissorSwitch.length); - break; - case SCISSOR_SCALE: - mBling.write(kScissorScale, kScissorScale.length); - break; - case TELEOP: - mBling.write(kTeleop, kTeleop.length); - break; - case ENDGAME: - mBling.write(kEndGame, kEndGame.length); + case BLUE: + mBling.write(kBlue, kBlue.length); break; - case CLIMB: - mBling.write(kClimb, kClimb.length); + case YELLOW: + mBling.write(kYellow, kYellow.length); break; - case STOP_CLIMBER: - mBling.write(kStopClimber, kStopClimber.length); + case RED: + mBling.write(kRed, kRed.length); break; - case OPEN_HARVESTER: - mBling.write(kOpenHarvester, kOpenHarvester.length); + case GREEN: + mBling.write(kGreen, kGreen.length); break; - case CLOSE_HARVESTER: - mBling.write(kCloseHarvester, kCloseHarvester.length); + case FAST_BLINK: + mBling.write(kFastBlink, kFastBlink.length); break; - case EJECT_HARVESTER: - mBling.write(kEjectHarvester, kEjectHarvester.length); + case SOLID: + mBling.write(kSolid, kSolid.length); break; - case CARRY_CUBE: - mBling.write(kCarryCube, kCarryCube.length); + case BLINK: + mBling.write(kBlink, kBlink.length); break; default: mBling.write(kOff, kOff.length); From 092e1311ea81dd5850f4625e18d304332c46472b Mon Sep 17 00:00:00 2001 From: Ryan Olney Date: Sun, 18 Mar 2018 16:49:25 -0700 Subject: [PATCH 2/3] new harvester code --- .../spartronics4915/frc2018/Constants.java | 5 +- .../spartronics4915/frc2018/ControlBoard.java | 3 + .../frc2018/ControlBoardInterface.java | 1 + src/com/spartronics4915/frc2018/Robot.java | 9 +- .../auto/modes/PlaceOptimizedFromCMode.java | 2 +- .../frc2018/subsystems/Harvester.java | 116 +++++++++++------- .../frc2018/subsystems/Superstructure.java | 4 +- 7 files changed, 91 insertions(+), 49 deletions(-) diff --git a/src/com/spartronics4915/frc2018/Constants.java b/src/com/spartronics4915/frc2018/Constants.java index 411d20b..5d2ed11 100755 --- a/src/com/spartronics4915/frc2018/Constants.java +++ b/src/com/spartronics4915/frc2018/Constants.java @@ -40,7 +40,7 @@ public class Constants extends ConstantsBase public static final int kHarvesterLeftMotorId = kUseTestbedConstants ? 18 : 8; public static final int kHarvesterRightMotorId = kUseTestbedConstants ? 16 : 9; - public static final int kNumTalons = 8; // total talon count on robot (not testbed) + public static final int kNumTalons = 9; // total talon count on robot (not testbed) public static final int kNumPDPs = 1; // doesn't always show up in CANProbe public static final int kNumPCMs = 1; // Pressure control module (pneumatics) @@ -52,7 +52,8 @@ public class Constants extends ConstantsBase public static final int kScissorBrakeSolenoidId = 2; //PCM 0 public static final int kGrabberSetupSolenoidId = 3; //PCM 0 public static final int kGrabberSolenoidId = 4; //PCM 0 - public static final int kHarvesterSolenoidId = 5; //PCM 0 + public static final int kHarvesterSolenoid1Id = 5; //PCM 0 + public static final int kHarvesterSolenoid2Id = 8; //PCM 0 public static final int kClimberStabilizationSolenoidId1 = 6; //PCM 0 public static final int kClimberStabilizationSolenoidId2 = 7; //PCM 0 diff --git a/src/com/spartronics4915/frc2018/ControlBoard.java b/src/com/spartronics4915/frc2018/ControlBoard.java index 12ab683..2982781 100755 --- a/src/com/spartronics4915/frc2018/ControlBoard.java +++ b/src/com/spartronics4915/frc2018/ControlBoard.java @@ -114,6 +114,9 @@ public boolean readButton(Buttons b) case HARVESTER_EJECT: result = mButtonBoard.getRawButtonPressed(4) || mDrivestick.getRawButtonPressed(3); break; + case HARVESTER_PARTIALLY_CLOSE: + result = mButtonBoard.getRawButtonPressed(9); + break; case SUPERSTRUCTURE_CARRY_CUBE: result = mButtonBoard.getRawButtonPressed(6) || mDrivestick.getRawButtonPressed(13); break; diff --git a/src/com/spartronics4915/frc2018/ControlBoardInterface.java b/src/com/spartronics4915/frc2018/ControlBoardInterface.java index 2df0453..728fb11 100755 --- a/src/com/spartronics4915/frc2018/ControlBoardInterface.java +++ b/src/com/spartronics4915/frc2018/ControlBoardInterface.java @@ -22,6 +22,7 @@ enum Buttons HARVESTER_OPEN, HARVESTER_CLOSE, HARVESTER_EJECT, + HARVESTER_PARTIALLY_CLOSE, SUPERSTRUCTURE_CARRY_CUBE, CLIMBER_TOGGLE, GRABBER_FAST_OPEN, diff --git a/src/com/spartronics4915/frc2018/Robot.java b/src/com/spartronics4915/frc2018/Robot.java index b44fea3..f8523fe 100755 --- a/src/com/spartronics4915/frc2018/Robot.java +++ b/src/com/spartronics4915/frc2018/Robot.java @@ -337,7 +337,7 @@ public void teleopPeriodic() if (mControlBoard.readButton(Buttons.HARVESTER_OPEN)) { - mHarvester.setWantedState(Harvester.WantedState.OPEN); + mHarvester.setWantedState(Harvester.WantedState.FULLOPEN); mLED.setBlingState(BlingState.SOLID); mLED.setBlingState(BlingState.BLUE); } @@ -348,6 +348,13 @@ public void teleopPeriodic() mLED.setBlingState(BlingState.BLINK); mLED.setBlingState(BlingState.BLUE); } + + if (mControlBoard.readButton(Buttons.HARVESTER_PARTIALLY_CLOSE)) + { + mHarvester.setWantedState(Harvester.WantedState.PARTIALLYCLOSE); + mLED.setBlingState(BlingState.BLINK); + mLED.setBlingState(BlingState.BLUE); + } if (mControlBoard.readButton(Buttons.HARVESTER_EJECT)) { diff --git a/src/com/spartronics4915/frc2018/auto/modes/PlaceOptimizedFromCMode.java b/src/com/spartronics4915/frc2018/auto/modes/PlaceOptimizedFromCMode.java index b3cdf36..7adab19 100644 --- a/src/com/spartronics4915/frc2018/auto/modes/PlaceOptimizedFromCMode.java +++ b/src/com/spartronics4915/frc2018/auto/modes/PlaceOptimizedFromCMode.java @@ -71,7 +71,7 @@ else if (Util.getGameSpecificMessage().charAt(0) == 'R') if (Util.getGameSpecificMessage().charAt(0) == 'R') // TODO: Add a way to pick up a second cube if we went to the scale { PathContainer secondPath = new DriveSecondCubeToCScalePath(); - runAction(new ParallelAction(new SeriesAction(new WaitForPathMarkerAction("openharvester"), new ActuateHarvesterAction(Harvester.WantedState.OPEN)), + runAction(new ParallelAction(new SeriesAction(new WaitForPathMarkerAction("openharvester"), new ActuateHarvesterAction(Harvester.WantedState.PARTIALLYCLOSE)), new DrivePathAction(new DriveReverseToSecondCubeFromCSwitchPath()))); runAction(new TurnToHeadingAction(Rotation2d.fromDegrees(180))); runAction(new ParallelAction(new SeriesAction(new WaitForPathMarkerAction("aquirecube"), new ForceEndPathAction()), diff --git a/src/com/spartronics4915/frc2018/subsystems/Harvester.java b/src/com/spartronics4915/frc2018/subsystems/Harvester.java index aa31ebb..08443ad 100644 --- a/src/com/spartronics4915/frc2018/subsystems/Harvester.java +++ b/src/com/spartronics4915/frc2018/subsystems/Harvester.java @@ -35,8 +35,11 @@ public static Harvester getInstance() public enum SystemState { - CLOSING, - OPENING, + FULLOPENING, + PARTIALLYCLOSING, + //FULLCLOSING, + //CLOSING, + //PARTIALLYCLOSING, HARVESTING, EJECTING, HUGGING, @@ -46,7 +49,11 @@ public enum SystemState public enum WantedState { - OPEN, + FULLOPEN, + PARTIALLYCLOSE, + //CUBEHOLD, + //FULLCLOSE, + //PARTIALLYCLOSE, HARVEST, EJECT, HUG, @@ -57,7 +64,8 @@ public enum WantedState private SystemState mSystemState = SystemState.DISABLING; private WantedState mWantedState = WantedState.DISABLE; private SpartIRSensor mCubeHeldSensor = null; - private LazySolenoid mSolenoid = null; + private LazySolenoid mSolenoid1 = null; + private LazySolenoid mSolenoid2 = null; private TalonSRX4915 mMotorRight = null; private TalonSRX4915 mMotorLeft = null; private Timer mTimer; @@ -73,7 +81,8 @@ private Harvester() try { - mSolenoid = new LazySolenoid(Constants.kHarvesterSolenoidId); // Changes value of Solenoid + mSolenoid1 = new LazySolenoid(Constants.kHarvesterSolenoid1Id); // Changes value of Solenoid1 + mSolenoid2 = new LazySolenoid(Constants.kHarvesterSolenoid2Id); // Changes value of Solenoid2 mCubeHeldSensor = new SpartIRSensor(Constants.kGrabberCubeDistanceRangeFinderId); mMotorRight = TalonSRX4915Factory.createDefaultMotor(Constants.kHarvesterRightMotorId); // change value of motor mMotorLeft = TalonSRX4915Factory.createDefaultMotor(Constants.kHarvesterLeftMotorId); // change value of motor @@ -92,9 +101,14 @@ private Harvester() logError("Left Motor is invalid"); success = false; } - if (!mSolenoid.isValid()) + if (!mSolenoid1.isValid()) { - logError("Solenoid is invalid"); + logError("Solenoid 1 is invalid"); + success = false; + } + if (!mSolenoid2.isValid()) + { + logError("Solenoid 2 is invalid"); success = false; } } @@ -118,7 +132,7 @@ public void onStart(double timestamp) { if (mSystemState == SystemState.DISABLING) - mSystemState = SystemState.CLOSING; + mSystemState = SystemState.FULLOPENING; } } @@ -130,11 +144,11 @@ public void onLoop(double timestamp) SystemState newState; // calls the wanted handle case for the given systemState switch (mSystemState) { - case CLOSING: - newState = handleClosing(); + case FULLOPENING: + newState = handleFullOpening(); break; - case OPENING: - newState = handleOpening(); + case PARTIALLYCLOSING: + newState = handlePartiallyClosing(); break; case HARVESTING: newState = handleHarvesting(); @@ -149,10 +163,10 @@ public void onLoop(double timestamp) newState = handleAutoHarvesting(); break; case DISABLING: - newState = handleClosing(); + newState = handleFullOpening(); break; default: - newState = handleClosing(); + newState = handleFullOpening(); } if (newState != mSystemState) { @@ -179,44 +193,53 @@ private SystemState defaultStateTransfer() // transitions the systemState given switch (mWantedState) { - case OPEN: - mSolenoid.set(kSolenoidOpen); - return SystemState.OPENING; + case FULLOPEN: + mSolenoid1.set(kSolenoidClose); + mSolenoid2.set(kSolenoidClose); + return SystemState.FULLOPENING; + case PARTIALLYCLOSE: + mSolenoid1.set(kSolenoidOpen); + mSolenoid2.set(kSolenoidClose); + return SystemState.PARTIALLYCLOSING; case HARVEST: - mSolenoid.set(kSolenoidClose); + mSolenoid1.set(kSolenoidOpen); + mSolenoid2.set(kSolenoidOpen); return SystemState.HARVESTING; case EJECT: - mSolenoid.set(kSolenoidClose); + mSolenoid1.set(kSolenoidOpen); + mSolenoid2.set(kSolenoidOpen); return SystemState.EJECTING; case HUG: - mSolenoid.set(kSolenoidClose); + mSolenoid1.set(kSolenoidOpen); + mSolenoid2.set(kSolenoidOpen); return SystemState.HUGGING; case AUTOHARVEST: - mSolenoid.set(kSolenoidOpen); + mSolenoid1.set(kSolenoidOpen); + mSolenoid2.set(kSolenoidClose); return SystemState.AUTOHARVESTING; default: return mSystemState; } } - private SystemState handleClosing() + private SystemState handleFullOpening() { - //motors off and bars in mMotorLeft.set(0.0); mMotorRight.set(0.0); - if (mWantedState == WantedState.OPEN || mWantedState == WantedState.AUTOHARVEST) + if (mWantedState == WantedState.PARTIALLYCLOSE || mWantedState == WantedState.AUTOHARVEST || mWantedState == WantedState.HARVEST) { - return defaultStateTransfer(); //all defaultStateTransfers return the wanted state + return defaultStateTransfer(); } - return SystemState.CLOSING; + return SystemState.FULLOPENING; } - private SystemState handleOpening() + private SystemState handlePartiallyClosing() { // due to mechanical stuck issue, run motors reverse, open bars and turn off motors after timeout mMotorLeft.set(0.0); mMotorRight.set(0.0); - if (mWantedState == WantedState.HARVEST || mWantedState == WantedState.EJECT || mWantedState == WantedState.AUTOHARVEST) + if (mWantedState == WantedState.HARVEST || mWantedState == WantedState.EJECT || mWantedState == WantedState.AUTOHARVEST + || mWantedState == WantedState.FULLOPEN) { return defaultStateTransfer(); } @@ -226,7 +249,7 @@ private SystemState handleOpening() //mMotorLeft.set(-1.0); //mMotorRight.set(-1.0); } - return SystemState.OPENING; + return SystemState.PARTIALLYCLOSING; } private SystemState handleAutoHarvesting() @@ -268,9 +291,11 @@ private SystemState handleEjecting() mMotorLeft.set(-1.0); if (!isCubeHeld() && mTimer.hasPeriodPassed(kEjectTimePeriod)) { //Cube is gone! Transition to Open (turn off motor) to prevent damage - mSolenoid.set(kSolenoidOpen); - setWantedState(WantedState.OPEN); - return SystemState.OPENING; + mSolenoid1.set(kSolenoidOpen); + mMotorRight.set(0.0); + mMotorLeft.set(0.0); + setWantedState(WantedState.PARTIALLYCLOSE); + return SystemState.PARTIALLYCLOSING; } else { @@ -283,7 +308,7 @@ private SystemState handleHugging() //motors off and bars closing go to closed when cube is gone mMotorLeft.set(0.0); mMotorRight.set(0.0); - if (mWantedState == WantedState.OPEN || mWantedState == WantedState.EJECT) + if (mWantedState == WantedState.PARTIALLYCLOSE || mWantedState == WantedState.EJECT) { return defaultStateTransfer(); } @@ -297,7 +322,7 @@ public WantedState getWantedState() public void setWantedState(WantedState wantedState) { - if (wantedState == WantedState.HARVEST || wantedState == WantedState.OPEN) + if (wantedState == WantedState.HARVEST || wantedState == WantedState.PARTIALLYCLOSE) { logNotice("TIMER SET---------------------------------------------"); mTimer.reset(); @@ -312,8 +337,8 @@ public boolean atTarget() boolean t = false; switch (mSystemState) { - case OPENING: - if (mWantedState == WantedState.OPEN) + case PARTIALLYCLOSING: + if (mWantedState == WantedState.PARTIALLYCLOSE) t = true; break; case HARVESTING: @@ -348,10 +373,15 @@ private boolean isCubeHeld() public void outputToSmartDashboard() { if (!isInitialized()) + { + dashboardPutBoolean("IRSensor CubeHeld", isCubeHeld()); + dashboardPutNumber("Cube Distance: ", mCubeHeldSensor.getDistance()); return; + } dashboardPutState(mSystemState.toString()); dashboardPutWantedState(mWantedState.toString()); - dashboardPutBoolean("mSolenoid", mSolenoid.get()); + dashboardPutBoolean("mSolenoid1", mSolenoid1.get()); + dashboardPutBoolean("mSolenoid2", mSolenoid2.get()); dashboardPutBoolean("IRSensor CubeHeld", isCubeHeld()); dashboardPutNumber("MotorRight", mMotorRight.get()); dashboardPutNumber("MotorLeft", mMotorLeft.get()); @@ -363,7 +393,7 @@ public synchronized void stop() { mSystemState = SystemState.DISABLING; mWantedState = WantedState.DISABLE; - mSolenoid.set(kSolenoidClose); + mSolenoid1.set(kSolenoidClose); mMotorLeft.set(0.0); mMotorRight.set(0.0); } @@ -398,24 +428,24 @@ public boolean checkSystem(String variant) logNotice("basic check ------"); logNotice(" mMotorRight:\n" + mMotorRight.dumpState()); logNotice(" mMotorLeft:\n" + mMotorLeft.dumpState()); - logNotice(" mSolenoid: " + mSolenoid.get()); + logNotice(" mSolenoid: " + mSolenoid1.get()); logNotice(" isCubeHeld: " + isCubeHeld()); } if (variant.equals("solenoid") || allTests) { logNotice("solenoid check ------"); logNotice("on 4s"); - mSolenoid.set(kSolenoidOpen); + mSolenoid1.set(kSolenoidOpen); Timer.delay(4.0); logNotice(" isCubeHeld: " + isCubeHeld()); logNotice("off"); - mSolenoid.set(kSolenoidClose); + mSolenoid1.set(kSolenoidClose); } if (variant.equals("motors") || allTests) { logNotice("motors check ------"); logNotice("open arms (2s)"); - mSolenoid.set(kSolenoidOpen); + mSolenoid1.set(kSolenoidOpen); Timer.delay(2.0); logNotice("left motor fwd .5 (4s)"); // in @@ -440,7 +470,7 @@ public boolean checkSystem(String variant) mMotorRight.set(0); Timer.delay(.5); // let motors spin down - mSolenoid.set(kSolenoidClose); + mSolenoid1.set(kSolenoidClose); } if (variant.equals("IRSensor") || allTests) { diff --git a/src/com/spartronics4915/frc2018/subsystems/Superstructure.java b/src/com/spartronics4915/frc2018/subsystems/Superstructure.java index 609582f..fbf1902 100755 --- a/src/com/spartronics4915/frc2018/subsystems/Superstructure.java +++ b/src/com/spartronics4915/frc2018/subsystems/Superstructure.java @@ -145,8 +145,8 @@ public void onLoop(double timestamp) } break; case OPENING_HARVESTER: // Transfer cube from harvester to scissor - if (mHarvester.getWantedState() != Harvester.WantedState.OPEN) - mHarvester.setWantedState(Harvester.WantedState.OPEN); + if (mHarvester.getWantedState() != Harvester.WantedState.PARTIALLYCLOSE) + mHarvester.setWantedState(Harvester.WantedState.PARTIALLYCLOSE); if (mWantedState == WantedState.TRANSFER_CUBE_TO_GRABBER) { if (mHarvester.atTarget()) From 51979f9173e1c4def4f019127309dc27eb2f1efa Mon Sep 17 00:00:00 2001 From: Ryan Olney Date: Mon, 19 Mar 2018 14:40:35 -0700 Subject: [PATCH 3/3] dded harvester code for additional pneumatics --- src/com/spartronics4915/frc2018/Constants.java | 6 ++---- src/com/spartronics4915/frc2018/Robot.java | 3 +-- src/com/spartronics4915/frc2018/subsystems/Harvester.java | 6 +++--- 3 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/com/spartronics4915/frc2018/Constants.java b/src/com/spartronics4915/frc2018/Constants.java index 5d2ed11..488d730 100755 --- a/src/com/spartronics4915/frc2018/Constants.java +++ b/src/com/spartronics4915/frc2018/Constants.java @@ -40,7 +40,7 @@ public class Constants extends ConstantsBase public static final int kHarvesterLeftMotorId = kUseTestbedConstants ? 18 : 8; public static final int kHarvesterRightMotorId = kUseTestbedConstants ? 16 : 9; - public static final int kNumTalons = 9; // total talon count on robot (not testbed) + public static final int kNumTalons = 8; // total talon count on robot (not testbed) public static final int kNumPDPs = 1; // doesn't always show up in CANProbe public static final int kNumPCMs = 1; // Pressure control module (pneumatics) @@ -53,9 +53,7 @@ public class Constants extends ConstantsBase public static final int kGrabberSetupSolenoidId = 3; //PCM 0 public static final int kGrabberSolenoidId = 4; //PCM 0 public static final int kHarvesterSolenoid1Id = 5; //PCM 0 - public static final int kHarvesterSolenoid2Id = 8; //PCM 0 - public static final int kClimberStabilizationSolenoidId1 = 6; //PCM 0 - public static final int kClimberStabilizationSolenoidId2 = 7; //PCM 0 + public static final int kHarvesterSolenoid2Id = 6; //PCM 0 // PWM (Servo) Pins ---------------------------- diff --git a/src/com/spartronics4915/frc2018/Robot.java b/src/com/spartronics4915/frc2018/Robot.java index f8523fe..376dd62 100755 --- a/src/com/spartronics4915/frc2018/Robot.java +++ b/src/com/spartronics4915/frc2018/Robot.java @@ -389,10 +389,9 @@ public void teleopPeriodic() if (mControlBoard.readButton(Buttons.GRABBER_FAST_OPEN)) { - mClimber.setWantedState(Climber.WantedState.HOLD); + mGrabber.setWantedState(ArticulatedGrabber.WantedState.FAST_OPENED); mLED.setBlingState(BlingState.BLINK); mLED.setBlingState(BlingState.RED); - mGrabber.setWantedState(ArticulatedGrabber.WantedState.FAST_OPENED); } if (mControlBoard.readButton(Buttons.CLIMB_IDLE_TEST)) diff --git a/src/com/spartronics4915/frc2018/subsystems/Harvester.java b/src/com/spartronics4915/frc2018/subsystems/Harvester.java index 08443ad..6dcc1a5 100644 --- a/src/com/spartronics4915/frc2018/subsystems/Harvester.java +++ b/src/com/spartronics4915/frc2018/subsystems/Harvester.java @@ -308,7 +308,7 @@ private SystemState handleHugging() //motors off and bars closing go to closed when cube is gone mMotorLeft.set(0.0); mMotorRight.set(0.0); - if (mWantedState == WantedState.PARTIALLYCLOSE || mWantedState == WantedState.EJECT) + if (mWantedState == WantedState.PARTIALLYCLOSE || mWantedState == WantedState.EJECT || mWantedState == WantedState.HARVEST || mWantedState == WantedState.FULLOPEN) { return defaultStateTransfer(); } @@ -374,8 +374,8 @@ public void outputToSmartDashboard() { if (!isInitialized()) { - dashboardPutBoolean("IRSensor CubeHeld", isCubeHeld()); - dashboardPutNumber("Cube Distance: ", mCubeHeldSensor.getDistance()); +// dashboardPutBoolean("IRSensor CubeHeld", isCubeHeld()); +// dashboardPutNumber("Cube Distance: ", mCubeHeldSensor.getDistance()); return; } dashboardPutState(mSystemState.toString());