TiltUp and TiltDown

src/main/java/frc/robot/Robot.java

@@ -138,26 +138,26 @@ public void teleopInit() {
     Timer.delay(.5);
 
     if (bootCycle && enableCalibration){
-      // CommandScheduler.getInstance().schedule(new CalibrateDriveTrain());
+      CommandScheduler.getInstance().schedule(new CalibrateDriveTrain());
       CommandScheduler.getInstance().schedule(new CalibrateTilt());
     } else CommandScheduler.getInstance().schedule(new TiltMagToLow());
 
-    // RobotContainer.m_drivetrain.maxDrivePower(.1);
+    RobotContainer.m_drivetrain.maxDrivePower(.1);
 
     // RobotContainer.m_shooterCam.setCamMode(1); // default to regular vision mode, not tracking mode
     // RobotContainer.m_shooterCam.ledOff();
 
     // RobotContainer.m_intakeCam.setCamMode(1); // default to regular vision mode, not tracking mode
     // RobotContainer.m_intakeCam.ledOff();
 
-    // CommandScheduler.getInstance().schedule(new DriveSpeed());
     CommandScheduler.getInstance().run();
 
     bootCycle = false;
 
     System.out.println("//////////////////// Teleop /////////////////");
     // CommandScheduler.getInstance().schedule(new LoadMagazine());
-    // CommandScheduler.getInstance().schedule(new Drive());
+    RobotContainer.m_drivetrain.setHighSpeedDriveMode();
+    CommandScheduler.getInstance().schedule(new Drive());
     // RobotContainer.m_led.clearLED();
   }
 

src/main/java/frc/robot/RobotContainer.java

@@ -47,7 +47,6 @@ public class RobotContainer {
   private final Drive m_Drive = new Drive();
   private final FieldCentric m_FieldCentric = new FieldCentric();
   private final GetColor m_GetColor = new GetColor();
-  private final DriveSpeed m_DriveSpeed = new DriveSpeed();
   private final RobotCentric m_RobotCentric = new RobotCentric();
   private final SnakeMode m_SnakeMode = new SnakeMode();
   private final RotateWheelToColor m_RotateWheelToColor = new RotateWheelToColor();
@@ -62,6 +61,8 @@ public class RobotContainer {
   private final TargetTrackModeEngage m_TargetTrackModeEngage = new TargetTrackModeEngage();
   private final TargetTrackModeDisengage m_TargetTrackModeDisengage = new TargetTrackModeDisengage();
   private final TiltToControlWheel m_TiltToControlWheel = new TiltToControlWheel();
+  private final TiltDown m_TiltDown = new TiltDown();
+  private final TiltUp m_TiltUp = new TiltUp();
   // private final MoveZipline m_MoveZipline = new MoveZipline();
   private final TiltMagToLow m_TiltMagToLow = new TiltMagToLow();
   // private final TiltNudge m_TiltNudge = new TiltNudge();
@@ -101,18 +102,14 @@ public RobotContainer() {
   private void configureButtonBindings() {
     // Driver Buttons
 
-    btnA.whenPressed(m_InvertDrivetrain, false);
+    btnB.whenPressed(m_InvertDrivetrain, false);
     btnRT.whenPressed(m_ShootBall, true);
-    btnLT.whenPressed(m_DriveSpeed, false);
-    btnLT.whenReleased(m_DriveSpeed, false);
-    btnLB.whenPressed(m_DriveSpeed,false);
-    btnLB.whenReleased(m_DriveSpeed, false);
     btnRB.whenPressed(m_IntakeBall, false);
     btnRB.whenReleased(m_StopIntake, false);
-    btnY.whenPressed(m_TargetTrackModeEngage, false);
-    btnY.whenReleased(m_TargetTrackModeDisengage, false);
-    btnB.whenPressed(m_ReverseIntake, false);
-    btnB.whenReleased(m_StopIntake, false);
+    btnLB.whenPressed(m_ReverseIntake, false);
+    btnLB.whenReleased(m_StopIntake, false);
+    btnA.whenPressed(m_TiltDown, false);
+    btnY.whenPressed(m_TiltUp, false);
 
   }
 

src/main/java/frc/robot/commands/TiltDown.java

@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import java.text.RuleBasedCollator;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.Robot;
+import frc.robot.RobotContainer;
+
+import frc.robot.RobotMap;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class TiltDown extends InstantCommand {
+  double endPos;
+
+  public TiltDown() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    endPos = ((RobotContainer.m_tilt.getTiltPosition() / RobotMap.PULSES_PER_DEGREE) - 5) * RobotMap.PULSES_PER_DEGREE;
+
+    if (endPos < RobotMap.MAGAZINE_LOW) {
+      endPos = RobotMap.MAGAZINE_LOW;
+    }
+    if (endPos > RobotMap.MAGAZINE_VERTICAL) {
+      endPos = RobotMap.MAGAZINE_VERTICAL;
+    }
+
+    RobotContainer.m_tilt.setTiltAngle(endPos);
+  }
+}

src/main/java/frc/robot/commands/TiltUp.java

@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import java.text.RuleBasedCollator;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.Robot;
+import frc.robot.RobotContainer;
+
+import frc.robot.RobotMap;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class TiltUp extends InstantCommand {
+  double endPos;
+
+  public TiltUp() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    endPos = ((RobotContainer.m_tilt.getTiltPosition() / RobotMap.PULSES_PER_DEGREE) + 5) * RobotMap.PULSES_PER_DEGREE;
+
+    if (endPos < RobotMap.MAGAZINE_LOW) {
+      endPos = RobotMap.MAGAZINE_LOW;
+    }
+    if (endPos > RobotMap.MAGAZINE_VERTICAL) {
+      endPos = RobotMap.MAGAZINE_VERTICAL;
+    }
+
+    RobotContainer.m_tilt.setTiltAngle(endPos);
+  }
+}