diff --git a/dist/FRCUserProgram.jar b/dist/FRCUserProgram.jar index 6d906ac..92315d8 100644 Binary files a/dist/FRCUserProgram.jar and b/dist/FRCUserProgram.jar differ diff --git a/src/org/usfirst/frc/team3316/robot/humanIO/Joysticks.java b/src/org/usfirst/frc/team3316/robot/humanIO/Joysticks.java index 1b52f60..26d5816 100644 --- a/src/org/usfirst/frc/team3316/robot/humanIO/Joysticks.java +++ b/src/org/usfirst/frc/team3316/robot/humanIO/Joysticks.java @@ -14,6 +14,7 @@ import org.usfirst.frc.team3316.robot.stacker.commands.HoldContainer; import org.usfirst.frc.team3316.robot.stacker.commands.MoveStackerToFloor; import org.usfirst.frc.team3316.robot.stacker.commands.MoveStackerToStep; +import org.usfirst.frc.team3316.robot.stacker.commands.MoveStackerToTote; import org.usfirst.frc.team3316.robot.stacker.commands.OpenGripper; import org.usfirst.frc.team3316.robot.stacker.commands.ReleaseContainer; @@ -66,7 +67,7 @@ public boolean get() public JoystickButton moveStackerToFloor, moveStackerToStep, - pickup; //pickup replaces moveStackerToTote + moveStackerToTote; //pickup replaces moveStackerToTote public JoystickButton holdContainer, releaseContainer; @@ -113,9 +114,9 @@ public void initButtons () (int) config.get("BUTTON_MOVE_STACKER_TO_STEP")); moveStackerToStep.whenPressed(new MoveStackerToStep()); - pickup = new JoystickButton(joystickOperator, + moveStackerToTote = new JoystickButton(joystickOperator, (int) config.get("BUTTON_MOVE_STACKER_TO_TOTE")); - pickup.whenPressed(new PickupSequence()); + moveStackerToTote.whenPressed(new MoveStackerToTote()); holdContainer = new JoystickButton(joystickOperator, (int) config.get("BUTTON_HOLD_CONTAINER")); diff --git a/src/org/usfirst/frc/team3316/robot/humanIO/SDB.java b/src/org/usfirst/frc/team3316/robot/humanIO/SDB.java index d049734..d1e266f 100644 --- a/src/org/usfirst/frc/team3316/robot/humanIO/SDB.java +++ b/src/org/usfirst/frc/team3316/robot/humanIO/SDB.java @@ -19,6 +19,7 @@ import org.usfirst.frc.team3316.robot.config.Config.ConfigException; import org.usfirst.frc.team3316.robot.logger.DBugLogger; +import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class SDB @@ -56,6 +57,15 @@ public void run () put ("Speed Right", Robot.chassis.getSpeedRight()); put ("Speed Center", Robot.chassis.getSpeedCenter()); + if (Robot.rollerGripper.getGamePieceCollected() == null) + { + put ("Game Piece Collected", null); + } + else + { + put ("Game Piece Collected", Robot.rollerGripper.getGamePieceCollected().toString()); + } + /* * Integrator testing * should be removed @@ -79,6 +89,11 @@ private void put (String name, boolean b) { SmartDashboard.putBoolean(name, b); } + + private void put (String name, String s) + { + SmartDashboard.putString(name, s); + } } DBugLogger logger = Robot.logger; diff --git a/src/org/usfirst/frc/team3316/robot/sequences/PickupSequence.java b/src/org/usfirst/frc/team3316/robot/sequences/PickupSequence.java index e5c97c2..15e092d 100644 --- a/src/org/usfirst/frc/team3316/robot/sequences/PickupSequence.java +++ b/src/org/usfirst/frc/team3316/robot/sequences/PickupSequence.java @@ -19,10 +19,10 @@ public PickupSequence() { addSequential(new MoveStackerToTote()); addSequential(new CloseGripper()); - addSequential(new WaitForGamePiece()); + //addSequential(new WaitForGamePiece()); //TODO: check if this should be changed to a sequence of floor --> step - moveToEndPosition = new MoveStackerToFloor(); + //moveToEndPosition = new MoveStackerToFloor(); } protected void end () @@ -31,6 +31,6 @@ protected void end () * This is in end because we don't want this to execute if the sequence * was cancelled (and interrupted was called instead of end) */ - moveToEndPosition.start(); + //moveToEndPosition.start(); } } diff --git a/src/org/usfirst/frc/team3316/robot/stacker/commands/MoveStacker.java b/src/org/usfirst/frc/team3316/robot/stacker/commands/MoveStacker.java index df1eb86..0500d8a 100644 --- a/src/org/usfirst/frc/team3316/robot/stacker/commands/MoveStacker.java +++ b/src/org/usfirst/frc/team3316/robot/stacker/commands/MoveStacker.java @@ -23,7 +23,7 @@ public abstract class MoveStacker extends Command public MoveStacker() { requires(Robot.stacker); - setpoint = setSetpointState(); + setpoint = this.setSetpointState(); } protected void initialize() {} @@ -38,6 +38,7 @@ protected void execute() protected boolean isFinished() { + if (Robot.stacker.getPosition() == setpoint) { return true; @@ -62,6 +63,7 @@ private void set () { setSuccessful = true; } + Robot.stacker.setSetpointState(setpoint); } protected abstract StackerPosition setSetpointState(); diff --git a/src/org/usfirst/frc/team3316/robot/subsystems/Stacker.java b/src/org/usfirst/frc/team3316/robot/subsystems/Stacker.java index 4b463d3..5a7e2e9 100644 --- a/src/org/usfirst/frc/team3316/robot/subsystems/Stacker.java +++ b/src/org/usfirst/frc/team3316/robot/subsystems/Stacker.java @@ -22,7 +22,6 @@ public class Stacker extends Subsystem { private class StackerManager extends TimerTask { - private StackerPosition currentState; private StackerPosition setpointState; @@ -67,6 +66,7 @@ else if (currentState == StackerPosition.Floor) if (!Robot.stacker.getSwitchRatchetLeft() || !Robot.stacker.getSwitchRatchetRight()) { + setpointState = null; return null; } @@ -99,6 +99,7 @@ else if (currentState == StackerPosition.Floor) if (!Robot.stacker.getSwitchRatchetLeft() || !Robot.stacker.getSwitchRatchetRight()) { + setpointState = null; return null; } @@ -146,6 +147,7 @@ else if (currentState == StackerPosition.Step) return setpointState; } + public void run () { currentState = Robot.stacker.getPosition(); @@ -272,9 +274,19 @@ public boolean closeSolenoidGripper () return true; } + double[] prevHeights = new double[100]; + public double getHeight () { - return (1 / heightIR.getVoltage()); + double currentHeight = (1 / heightIR.getVoltage()); + double avg = currentHeight; + for(int i=0; i