Brandon's recommended changes to get things running

src/main/java/frc/robot/Constants.java

@@ -55,7 +55,7 @@ public static class SystemConfig {
     public static final double SHOOTER_TOLERANCE = 2; // deg
     public static final double ELEVATOR_TOLERANCE = 0.1; // m
 
-    public static final double ELEVATOR_SPEED = 0.5;
+    public static final double ELEVATOR_SPEED = 0.7;
     public static final double PIVOT_SPEED = 0.5;
     public static final double SHOOTER_SPEED = 0.5;
     public static final double GRABBER_SPEED = 0.5;

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

@@ -39,6 +39,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -118,23 +119,31 @@ private void configureBindings()
 
     // Operator Controls
 
-    operatorController.rightTrigger().whileTrue(new ShootCommand(-.1));
-    operatorController.leftTrigger().whileTrue(new ShootCommand(.1));
+    operatorController.rightTrigger().whileTrue(new ShootCommand(-1.0));
+    // operatorController.leftTrigger().whileTrue(new ShootCommand(.1));
+
+    operatorController.leftBumper().whileTrue(new IntakeCommand(.3));
+    operatorController.rightBumper().whileTrue(new IntakeCommand(-.3));
+    // operatorController.leftBumper().whileTrue(new IntakeCommand(.1));
+    // operatorController.leftBumper().whileTrue(new ParallelCommandGroup(
+    //   new IntakeCommand(.1),
+    //   new ShootCommand(-.4)
+    // ));
+    operatorController.x().onTrue(new ElevatortoPositionCommand(0.5, 1));
+    operatorController.y().onTrue(new ElevatortoPositionCommand(0.5, 2));
+    operatorController.b().onTrue(new ElevatortoPositionCommand(0.5, 3));
+    operatorController.a().onTrue(new ElevatortoPositionCommand(0.5, 4));
+    operatorController.povDown().whileTrue(new ElevatorDownCommand(0.5));
 
-    operatorController.rightBumper().whileTrue(new IntakeCommand(-.1));
-    operatorController.leftBumper().whileTrue(new IntakeCommand(.1));
-
-
-
     // Driver Controls
     driverController.y().onTrue(new InstantCommand(() -> {climber.LatchServo();}));
     driverController.b().onTrue(new InstantCommand(() -> {climber.UnlatchServo();}));
 
-    driverController.rightTrigger().whileTrue(new ExtendWinchCommand(-.1));
-    driverController.leftTrigger().whileTrue(new ExtendWinchCommand(.1));
+    driverController.leftTrigger().whileTrue(new ExtendWinchCommand(-.1));
+    driverController.rightTrigger().whileTrue(new ExtendWinchCommand(.1));
 
-    driverController.rightBumper().whileTrue(new GrabCageCommand(.1));
-    driverController.leftBumper().whileTrue(new GrabCageCommand(-1.));
+    driverController.rightBumper().whileTrue(new GrabCageCommand(.4));
+    driverController.leftBumper().whileTrue(new GrabCageCommand(-.4));
 
   }
   public void setMotorBrake(boolean brake)

src/main/java/frc/robot/commands/ClimberCommands/ExtendWinchCommand.java

@@ -14,15 +14,19 @@ public ExtendWinchCommand(Double _speed)
     }
 
     @Override
-    public void initialize() {}
+    public void initialize() {
+        RobotContainer.climber.Winch(0);
+    }
 
     @Override
     public void execute() {
         RobotContainer.climber.Winch(speed);
     }
 
     @Override
-    public void end(boolean interrupted) {}
+    public void end(boolean interrupted) {
+        RobotContainer.climber.Winch(0);
+    }
 
     @Override
     public boolean isFinished() {

src/main/java/frc/robot/commands/ClimberCommands/GrabCageCommand.java

@@ -14,15 +14,19 @@ public GrabCageCommand(Double _speed)
     }
 
     @Override
-    public void initialize() {}
+    public void initialize() {
+        RobotContainer.climber.Grab(0);
+    }
 
     @Override
     public void execute() {
         RobotContainer.climber.Grab(speed);
     }
 
     @Override
-    public void end(boolean interrupted) {}
+    public void end(boolean interrupted) {
+        RobotContainer.climber.Grab(0);
+    }
 
     @Override
     public boolean isFinished() {

src/main/java/frc/robot/commands/ElevatorCommands/ElevatortoPositionCommand.java

@@ -0,0 +1,77 @@
+package frc.robot.commands.ElevatorCommands;
+
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Robot;
+import frc.robot.RobotContainer;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.wpilibj.Timer;
+
+public class ElevatortoPositionCommand extends Command 
+{
+    private Debouncer m_debouncer;
+    private final double speed;
+    private final int level_1_position = 50;
+    private final int level_2_position = 150;
+    private final int level_3_position = 300; 
+    private final int level_4_position = 600;
+    private final int level; 
+
+    public ElevatortoPositionCommand(Double _speed, int _level)
+    {
+        addRequirements(RobotContainer.elevator);
+        speed = _speed;
+        level = _level; 
+        double debounceTime = .25;
+        DebounceType debounceType = DebounceType.kRising;
+        m_debouncer = new Debouncer(debounceTime, debounceType);
+    }
+
+    @Override
+    public void initialize() {}
+
+    public void moveToPosition(int position){
+        boolean isAtPosition = false;
+        boolean isStalled = m_debouncer.calculate(isAtPosition);
+        while(!isStalled){
+            if (!isAtPosition && RobotContainer.elevator.getPositionElevator1() >= level_3_position && RobotContainer.elevator.getPositionElevator2() >= level_3_position) {
+                RobotContainer.elevator.setSpeedEndEffectorTilt(speed);
+                Timer.delay(1.0);
+                RobotContainer.elevator.setSpeedEndEffectorTilt(0);
+            }
+            RobotContainer.elevator.setSpeedElevator1(speed);
+            RobotContainer.elevator.setSpeedElevator2(speed);
+            isAtPosition = (RobotContainer.elevator.getPositionElevator1() >= position && RobotContainer.elevator.getPositionElevator2() >= position);
+            isStalled = m_debouncer.calculate(isAtPosition);
+            Timer.delay(.1);
+        }
+        RobotContainer.elevator.setSpeedElevator1(0);
+        RobotContainer.elevator.setSpeedElevator2(0);
+    }
+
+    @Override
+    public void execute() 
+    {
+        switch(level){
+            case 1:
+                moveToPosition(level_1_position);
+            case 2:
+                moveToPosition(level_2_position);
+            case 3:
+                moveToPosition(level_3_position);
+            case 4:
+                moveToPosition(level_4_position);
+            default:
+                moveToPosition(level_1_position);
+        }
+    }
+
+    @Override
+    public void end(boolean interrupted) {}
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.RobotContainer;
+import frc.robot.Robot;
 
 public class IntakeCommand extends Command 
 {
@@ -16,17 +17,22 @@ public IntakeCommand(Double _speed)
     @Override
     public void initialize() {
         RobotContainer.endEffector.setSpeedEndEffectorIntake(0);
+        RobotContainer.endEffector.setSpeedEndEffectorMotor(0);
     }
 
     @Override
     public void execute() 
     {
-        RobotContainer.endEffector.setSpeedEndEffectorIntake(speed);
+        boolean isStalled = Robot.isStalled(); 
+        RobotContainer.endEffector.setSpeedEndEffectorIntake(speed/3);
+        // Intake moves much faster than Motor!
+        RobotContainer.endEffector.setSpeedEndEffectorMotor(isStalled?0:speed*1.5);
     }
 
     @Override
     public void end(boolean interrupted) {
         RobotContainer.endEffector.setSpeedEndEffectorIntake(0);
+        RobotContainer.endEffector.setSpeedEndEffectorMotor(0);
     }
 
     @Override

src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java

@@ -14,16 +14,20 @@ public ShootCommand(Double _speed)
     }
 
     @Override
-    public void initialize() {}
+    public void initialize() {
+        RobotContainer.endEffector.setSpeedEndEffectorMotor(0);
+    }
 
     @Override
     public void execute() 
     {
-        RobotContainer.endEffector.setSpeedEndEffectorMotor(speed);
+        RobotContainer.endEffector.setSpeedEndEffectorMotor(speed*2);
     }
 
     @Override
-    public void end(boolean interrupted) {}
+    public void end(boolean interrupted) {
+        RobotContainer.endEffector.setSpeedEndEffectorMotor(0);
+    }
 
     @Override
     public boolean isFinished() {

src/main/java/frc/robot/subsystems/climb/ClimberSubsystem.java

@@ -16,7 +16,7 @@
 //     }
 
 //     @Override
-//     public void periodic() {
+//     public void () {
 //         updateEntries();
 //     }
 

src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java

@@ -2,12 +2,10 @@
 
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.config.SparkMaxConfig;
-import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
@@ -17,46 +15,17 @@ public class ElevatorSubsystem extends SubsystemBase
 {
     private final SparkMax elevator1 = new SparkMax(CANConfig.ELEVATOR_LEFT, MotorType.kBrushless);
     private final SparkMax elevator2 = new SparkMax(CANConfig.ELEVATOR_RIGHT, MotorType.kBrushless);
-    private final SparkMax endEffectorTilt = new SparkMax(CANConfig.END_EFFECTOR_TILT, MotorType.kBrushless);
-
     private SparkMaxConfig elevator1Config = new SparkMaxConfig();
     private SparkMaxConfig elevator2Config = new SparkMaxConfig();
-
-    private double startingHeight = 15.25;
-    private double endingHeight = 59.25;
-    private double startingEncoder = 0;
-    private double endingEncoder = 110;
-
-    private double inchesPerEncoder = (endingHeight - startingHeight)/(endingEncoder - startingEncoder);
-
-    private PIDController elevatorPID = new PIDController(.5,0,0);
+    private final SparkMax endEffectorTilt = new SparkMax(CANConfig.END_EFFECTOR_TILT, MotorType.kBrushless);   
 
     public ElevatorSubsystem()
     {
         elevator1Config.inverted(true);
         elevator2Config.inverted(false);
-        elevator1Config.idleMode(IdleMode.kBrake);
-        elevator2Config.idleMode(IdleMode.kBrake);
         elevator1.configure(elevator1Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-        elevator2.configure(elevator2Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-        elevatorPID.setTolerance(1);
-    }
-
-    public void goToHeight(double height){
-        height = height - 15.25; //15.25 is starting height
-        double voltage = elevatorPID.calculate(this.getHeight(), height);
-        if(Math.abs(voltage) > 3){
-            voltage = (voltage)/Math.abs(voltage)*3;
-        }
-        SmartDashboard.putNumber("elevator PID Voltage", voltage);
-        elevator1.setVoltage(voltage);
-        elevator2.setVoltage(voltage);
-
+        elevator2.configure(elevator2Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);     
     }
-    public double getHeight(){
-        return elevator1.getEncoder().getPosition()*inchesPerEncoder;
-    }
-
 
     public void setSpeedElevator1(double speed) 
     {
@@ -68,17 +37,21 @@ public void setSpeedElevator2(double speed)
         elevator2.set(speed);
     }
 
-    public void setSpeedEndEffectorTilt(double speed){
+    public void setSpeedEndEffectorTilt(double speed) 
+    {
         endEffectorTilt.set(speed);
     }
 
     public void setSpeed(double speed){
         elevator1.set(speed);
         elevator2.set(speed);
     }
-
-    //elevator positions
-
+    public double getPositionElevator1(){
+       return elevator1.getAbsoluteEncoder().getPosition();
+    }
+    public double getPositionElevator2(){
+        return elevator2.getAbsoluteEncoder().getPosition();
+    }
 
     @Override
     public void periodic()