Final Commit before AZ North Regional

src/com/team3925/frc2018/OI.java

@@ -1,15 +1,22 @@
 package com.team3925.frc2018;
 
+import com.team3925.frc2018.commands.CloseGrabbers;
+import com.team3925.frc2018.commands.DecrementAdjustElevator;
 import com.team3925.frc2018.commands.DriveManual.DriveManualInput;
+import com.team3925.frc2018.commands.DropCube;
+import com.team3925.frc2018.commands.IncrementAdjustElevator;
 import com.team3925.frc2018.commands.OpenGrabbers;
-import com.team3925.frc2018.commands.ReverseIntakeWheels;
+import com.team3925.frc2018.commands.RunElevator;
 import com.team3925.frc2018.commands.RunIntakeWheels;
 import com.team3925.frc2018.commands.ShiftHigh;
 import com.team3925.frc2018.commands.ShiftLow;
+import com.team3925.frc2018.commands.ShootCube;
+import com.team3925.frc2018.subsystems.Elevator.ElevatorState;
 
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.buttons.Button;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
+import edu.wpi.first.wpilibj.buttons.Trigger;
 
 public class OI implements DriveManualInput{
 
@@ -19,10 +26,21 @@ public class OI implements DriveManualInput{
 
 	private static OI instance;
 
-	private Button grab;
-	private Button suck;
-	private Button shoot;
-	private Button shift;
+	private Button drivetrain_Shift;
+
+	private Button jogElevatorTop;
+	private Button jogElevatorScaleHigh;
+	private Button jogElevatorScaleMiddle;
+	private Button jogElevatorScaleLow;
+	private Button jogElevatorSwitch;
+	private Button jogElevatorBottom;
+	private Trigger dropCube;
+	private Trigger shootCube;
+	private Button intakeCube;
+	private Button openIntake;
+
+	private Trigger tuneUp;
+	private Trigger tuneDown;
 
 	public static OI getInstance() {
 		if (instance == null)
@@ -35,18 +53,70 @@ private OI() {
 		wheel = new Joystick(1);
 		xbox  = new Joystick(2);
 
-		grab = new JoystickButton(xbox, 1);
-		suck = new JoystickButton(xbox, 2);
-		shoot = new JoystickButton(xbox, 4);
+		jogElevatorTop = new JoystickButton(xbox, 9);
+		jogElevatorScaleHigh = new JoystickButton(xbox, 4);
+		jogElevatorScaleMiddle = new JoystickButton(xbox, 2);
+		jogElevatorScaleLow = new JoystickButton(xbox, 1);
+		jogElevatorSwitch = new JoystickButton(xbox, 3);
+		jogElevatorBottom = new JoystickButton(xbox, 10);
+
+		drivetrain_Shift = new JoystickButton(wheel, 5);
+
+		openIntake = new JoystickButton(xbox, 6);
+		intakeCube = new JoystickButton(xbox, 5);
+
+		dropCube = new Trigger() {
+			@Override
+			public boolean get() {
+				return xbox.getPOV() == 180;
+			}
+		};
+
+		shootCube = new Trigger() {
+
+			@Override
+			public boolean get() {
+				return xbox.getPOV() == 0;
+			}
+		};
+
+		tuneUp = new Trigger() {
+
+			@Override
+			public boolean get() {
+				return xbox.getRawAxis(3) > 0.7;
+			}
+		};
+
+		tuneDown = new Trigger() {
+			@Override
+			public boolean get() {
+				return xbox.getRawAxis(2) > 0.7;
+			}
+		};
+
+
+		jogElevatorTop.whenPressed(new RunElevator(ElevatorState.TOP));
+		jogElevatorScaleHigh.whenPressed(new RunElevator(ElevatorState.SCALE_MAX));
+		jogElevatorScaleMiddle.whenPressed(new RunElevator(ElevatorState.SCALE_MED));
+		jogElevatorScaleLow.whenPressed(new RunElevator(ElevatorState.SCALE_LOW));
+		jogElevatorSwitch.whenPressed(new RunElevator(ElevatorState.SWITCH));
+		jogElevatorBottom.whenPressed(new RunElevator(ElevatorState.BOTTOM));
 
+		dropCube.whenActive(new DropCube());
+		shootCube.whenActive(new ShootCube());
 
-		shift = new JoystickButton(wheel, 5);
+		drivetrain_Shift.whenPressed(new ShiftLow());
+		drivetrain_Shift.whenReleased(new ShiftHigh());
 
-		shoot.whileHeld(new ReverseIntakeWheels());
-		suck.whileHeld(new RunIntakeWheels());
-		grab.whileHeld(new OpenGrabbers());
-		shift.whileHeld(new ShiftLow());
-		shift.whenInactive(new ShiftHigh());
+		openIntake.whenPressed(new OpenGrabbers());
+		openIntake.whenReleased(new CloseGrabbers());
+
+		intakeCube.whenPressed(new RunIntakeWheels(1));
+		intakeCube.whenReleased(new RunIntakeWheels(0));
+
+		tuneUp.whenActive(new IncrementAdjustElevator());
+		tuneDown.whenActive(new DecrementAdjustElevator());
 	}
 
 	@Override
@@ -59,10 +129,17 @@ public double getLeft() {
 	}
 
 	public double getElevator() {
-		return -xbox.getRawAxis(5);
+		return -xbox.getRawAxis(1);
+	}
+
+	public double getLiftIntake() {
+		return stick.getRawAxis(2);
 	}
 
 	public double getRawElevator() {
-		return -xbox.getRawAxis(1);
+		return stick.getRawAxis(5);
+	}
+	public boolean getTestButton() {
+		return wheel.getRawButton(4);
 	}
 }

src/com/team3925/frc2018/Robot.java

@@ -1,31 +1,44 @@
 package com.team3925.frc2018;
 
+import javax.swing.text.DefaultStyledDocument.ElementBuffer;
+
+import com.team3925.frc2018.commands.Center_Left_Switch_Auto;
 import com.team3925.frc2018.commands.DriveManual;
+import com.team3925.frc2018.commands.RunElevator;
 import com.team3925.frc2018.commands.RunElevatorRaw;
-import com.team3925.frc2018.subsystems.Drivetrain;
+import com.team3925.frc2018.commands.RunIntakeLiftRaw;
+import com.team3925.frc2018.commands.ShootCube;
 import com.team3925.frc2018.subsystems.Elevator;
+import com.team3925.frc2018.subsystems.Elevator.ElevatorState;
+
 import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
 
 public class Robot extends IterativeRobot {
 	DriveManual drive;
+	RunElevator elevate;
 	RunElevatorRaw elevateRaw;
+	RunIntakeLiftRaw liftRaw;
+	Command testAuto;
 
 	@Override
 	public void robotInit() {
 		drive = new DriveManual(OI.getInstance());
 		elevateRaw = new RunElevatorRaw();
+		liftRaw = new RunIntakeLiftRaw();
+		testAuto = new Center_Left_Switch_Auto();
 	}
 
 	@Override
 	public void autonomousInit() {
+		Elevator.getInstance().zero();
+		testAuto.start();
 	}
 
 	public void teleopInit() {
 		Elevator.getInstance().zero();
-		Drivetrain.getInstance().getGyroHeading();
 		drive.start();
-		elevateRaw.start();
 	}
 
 	@Override

src/com/team3925/frc2018/RobotMap.java

@@ -27,12 +27,12 @@ public static final class DrivetrainMap {
 
 		public static final PigeonIMU DRIVETRAIN_IMU = new PigeonIMU(LEFT_SLAVE_A);
 
-		public static final DoubleSolenoid SHIFT_SOLENOID = new DoubleSolenoid(0, 1);
+		public static final DoubleSolenoid SHIFT_SOLENOID = new DoubleSolenoid(4, 5);
 	}
 
 	public static final class IntakeMap {
 		public static final WPI_TalonSRX LEFT_INTAKE = CTREControllerFactory.createDefaultTalon(11);
-		public static final WPI_TalonSRX RIGHT_INTAKE = CTREControllerFactory.createDefaultTalon(12);
+		public static final WPI_VictorSPX RIGHT_INTAKE = CTREControllerFactory.createDefaultVictor(12);
 
 		public static final WPI_TalonSRX LIFT_MOTOR = CTREControllerFactory.createDefaultTalon(13);
 

src/com/team3925/frc2018/commands/Center_Left_Switch_Auto.java

@@ -0,0 +1,20 @@
+package com.team3925.frc2018.commands;
+
+import com.team3925.frc2018.subsystems.Elevator.ElevatorState;
+import com.team3925.utils.MotionProfileCommand;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+public class Center_Left_Switch_Auto extends CommandGroup{
+
+	public Center_Left_Switch_Auto() {
+		addParallel(new RunElevator(ElevatorState.BOTTOM));
+		addParallel(new MotionProfileCommand("CENTER_LEFTSWITCH"));
+		addSequential(new WaitCommand(2));
+		addSequential(new RunElevator(ElevatorState.SWITCH));
+		addSequential(new OpenGrabbers());
+		addSequential(new RunIntakeWheels(-0.5));
+	}
+
+}

src/com/team3925/frc2018/commands/ReverseIntakeWheels.java → src/com/team3925/frc2018/commands/CloseGrabbers.java

@@ -4,21 +4,15 @@
 
 import edu.wpi.first.wpilibj.command.Command;
 
-public class ReverseIntakeWheels extends Command{
+public class CloseGrabbers extends Command{
 
 	@Override
 	protected void initialize() {
-		Intake.getInstance().setIntakeRollers(-0.6);
+		Intake.getInstance().setGrabber(false);
 	}
 
-	@Override
-	protected void end() {
-		Intake.getInstance().setIntakeRollers(0);
-	}
-
 	@Override
 	protected boolean isFinished() {
-		return false;
+		return true;
 	}
-
 }

src/com/team3925/frc2018/commands/DecrementAdjustElevator.java

@@ -0,0 +1,20 @@
+package com.team3925.frc2018.commands;
+
+import com.team3925.frc2018.subsystems.Elevator;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class DecrementAdjustElevator extends Command{
+
+
+	@Override
+	protected void initialize() {
+		Elevator.getInstance().setPosition(Elevator.elevatorHeight - Elevator.TELEOP_ELEVATOR_INCREMENT);
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return true;
+	}
+
+}

src/com/team3925/frc2018/commands/DriveManual.java

@@ -1,6 +1,9 @@
 package com.team3925.frc2018.commands;
 
+import javax.print.attribute.standard.PrinterResolution;
+
 import com.team3925.frc2018.subsystems.Drivetrain;
+import com.team3925.frc2018.subsystems.Elevator;
 
 import edu.wpi.first.wpilibj.command.Command;
 
@@ -13,12 +16,12 @@ public interface DriveManualInput {
 	}
 
 	private final boolean doReverseWhenReversing;
-
-
+
 	private DriveManualInput input;
 	private double prelimLeft, prelimRight;
 	private double fwd, turn;
 	private double scale;
+	private static final double K_HEIGHT_SUBTRACTION = 0.9;
 
 	public DriveManual(DriveManualInput input) {
 		this.input = input;
@@ -46,8 +49,13 @@ protected void execute() {
 			prelimLeft *= scale;
 			prelimRight *= scale;
 		}
-		Drivetrain.getInstance().setRaw(prelimLeft, prelimRight);
+
+		Drivetrain.getInstance().setRaw(
+				prelimLeft * (1 - (Elevator.getInstance().getElevatorHeightPercentage() * K_HEIGHT_SUBTRACTION)),
+				prelimRight * (1 - (Elevator.getInstance().getElevatorHeightPercentage() * K_HEIGHT_SUBTRACTION))
+		);
 	}
+
 
 	@Override
 	protected boolean isFinished() {

src/com/team3925/frc2018/commands/DropCube.java

@@ -0,0 +1,27 @@
+package com.team3925.frc2018.commands;
+
+import com.team3925.frc2018.subsystems.Intake;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+public class DropCube extends CommandGroup{
+	public DropCube() {
+		addParallel(new OpenGrabbers());
+		addSequential(new RunIntakeWheels(-0.25));
+		addSequential(new WaitCommand(0.5));
+		addParallel(new RunIntakeWheels(0));
+		addSequential(new CloseGrabbers());
+	}
+
+	@Override
+	protected void interrupted() {
+		Intake.getInstance().setGrabber(false);
+		Intake.getInstance().setIntakeRollers(0);
+	}
+
+	@Override
+	protected void end() {
+		this.interrupted();
+	}
+}

src/com/team3925/frc2018/commands/IncrementAdjustElevator.java

@@ -0,0 +1,20 @@
+package com.team3925.frc2018.commands;
+
+import com.team3925.frc2018.subsystems.Elevator;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class IncrementAdjustElevator extends Command{
+
+
+	@Override
+	protected void initialize() {
+		Elevator.getInstance().setPosition(Elevator.elevatorHeight + Elevator.TELEOP_ELEVATOR_INCREMENT);
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return true;
+	}
+
+}

src/com/team3925/frc2018/commands/OpenGrabbers.java

@@ -11,14 +11,8 @@ protected void initialize() {
 		Intake.getInstance().setGrabber(true);
 	}
 
-	@Override
-	protected void end() {
-		Intake.getInstance().setGrabber(false);
-	}
-
 	@Override
 	protected boolean isFinished() {
-		return false;
+		return true;
 	}
-
 }