Climber excelsior

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -5,11 +5,11 @@
 
 package com.stuypulse.robot;
 
-import com.stuypulse.robot.commands.ElevatorDrive;
-import com.stuypulse.robot.commands.ElevatorToBottom;
-import com.stuypulse.robot.commands.ElevatorToHeight;
-import com.stuypulse.robot.commands.ElevatorToTop;
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
+import com.stuypulse.robot.commands.elevator.ElevatorDrive;
+import com.stuypulse.robot.commands.elevator.ElevatorToBottom;
+import com.stuypulse.robot.commands.elevator.ElevatorToHeight;
+import com.stuypulse.robot.commands.elevator.ElevatorToTop;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.subsystems.Elevator;
 import com.stuypulse.stuylib.input.Gamepad;
@@ -67,8 +67,7 @@ public void configureAutons() {
         autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton());
         autonChooser.addOption("Elevator To Bottom", new ElevatorToBottom().untilReady());
         autonChooser.addOption("Elevator To Top", new ElevatorToTop().untilReady());
-        autonChooser.addOption("Elevator To Height", new ElevatorToHeight(1.2).untilReady());
-
+
         SmartDashboard.putData("Autonomous", autonChooser);
     }
 

src/main/java/com/stuypulse/robot/commands/ElevatorDrive.java → src/main/java/com/stuypulse/robot/commands/elevator/ElevatorDrive.java

@@ -1,4 +1,4 @@
-package com.stuypulse.robot.commands;
+package com.stuypulse.robot.commands.elevator;
 
 import com.stuypulse.robot.subsystems.Elevator;
 

src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToAmp.java

@@ -0,0 +1,9 @@
+package com.stuypulse.robot.commands.elevator;
+
+import static com.stuypulse.robot.constants.Settings.Elevator.*;
+
+public class ElevatorToAmp extends ElevatorToHeight {
+    public ElevatorToAmp() {
+         super(AMP_HEIGHT);
+    }
+}

src/main/java/com/stuypulse/robot/commands/ElevatorToBottom.java → src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToBottom.java

@@ -1,4 +1,4 @@
-package com.stuypulse.robot.commands;
+package com.stuypulse.robot.commands.elevator;
 
 import static com.stuypulse.robot.constants.Settings.Elevator.*;
 

src/main/java/com/stuypulse/robot/commands/ElevatorToHeight.java → src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToHeight.java

@@ -1,4 +1,4 @@
-package com.stuypulse.robot.commands;
+package com.stuypulse.robot.commands.elevator;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 

src/main/java/com/stuypulse/robot/commands/ElevatorToTop.java → src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTop.java

@@ -1,8 +1,9 @@
-package com.stuypulse.robot.commands;
+package com.stuypulse.robot.commands.elevator;
 
 import static com.stuypulse.robot.constants.Settings.Elevator.*;
 
 public class ElevatorToTop extends ElevatorToHeight {
+
     public ElevatorToTop() {
         super(MAX_HEIGHT);
     }

src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToTrap.java

@@ -0,0 +1,10 @@
+package com.stuypulse.robot.commands.elevator;
+
+import static com.stuypulse.robot.constants.Settings.Elevator.*;
+
+
+public class ElevatorToTrap extends ElevatorToHeight {
+    public ElevatorToTrap() {
+        super(TRAP_HEIGHT);
+    }
+}

src/main/java/com/stuypulse/robot/constants/Ports.java

@@ -14,8 +14,7 @@ public interface Gamepad {
     }
 
     public interface Elevator {
-        int LEFT = 0;
-        int RIGHT = 0;
+        int MOTOR = 0;
 
         int TOP_LIMIT_SWITCH = 0;
         int BOTTOM_LIMIT_SWITCH = 0;

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -7,6 +7,8 @@
 
 import com.stuypulse.stuylib.network.SmartNumber;
 
+import edu.wpi.first.math.util.Units;
+
 /*-
  * File containing tunable settings for every subsystem on the robot.
  *
@@ -22,6 +24,8 @@ public interface Elevator {
 
         double MIN_HEIGHT = 0;
         double MAX_HEIGHT = 1.8475325; // meters 
+        double TRAP_HEIGHT = 1.5; // placeholder
+        double AMP_HEIGHT = 1.25; // placeholder
 
         double MAX_HEIGHT_ERROR = 0.03;
 
@@ -30,6 +34,9 @@ public interface Elevator {
 
         public interface Encoder {
             double ENCODER_MULTIPLIER = 1;
+            double GEAR_RATIO = 1.0 / 20.0; //added
+            double WINCH_CIRCUMFERENCE = Math.PI * Units.inchesToMeters(1.25);
+            double ENCODER_RATIO = GEAR_RATIO * WINCH_CIRCUMFERENCE;
         }
     }
 

src/main/java/com/stuypulse/robot/subsystems/ElevatorImpl.java

@@ -4,6 +4,7 @@
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.constants.Settings.Elevator.Encoder;
 import com.revrobotics.RelativeEncoder;
 
 import edu.wpi.first.wpilibj.DigitalInput;
@@ -15,21 +16,24 @@ public class ElevatorImpl extends Elevator {
     // hardware
     public CANSparkMax leftMotor;
     public CANSparkMax rightMotor;
+    public CANSparkMax motor;
 
     public RelativeEncoder leftEncoder;
     public RelativeEncoder rightEncoder;
+    public RelativeEncoder encoder;
 
     public DigitalInput topLimit;
     public DigitalInput bottomLimit;
 
     public ElevatorImpl() {
+
         // motors
-        leftMotor = new CANSparkMax(Ports.Elevator.LEFT, MotorType.kBrushless);
-        rightMotor = new CANSparkMax(Ports.Elevator.RIGHT, MotorType.kBrushless);
+        motor = new CANSparkMax(Ports.Elevator.MOTOR, MotorType.kBrushless);
+
+        encoder = motor.getEncoder();
+        encoder.setPositionConversionFactor(Encoder.ENCODER_RATIO);
+        encoder.setVelocityConversionFactor(Encoder.ENCODER_RATIO / 60.0);
 
-        // encoders
-        leftEncoder = leftMotor.getEncoder();
-        rightEncoder = rightMotor.getEncoder();
 
         // limit switches
         topLimit = new DigitalInput(Ports.Elevator.TOP_LIMIT_SWITCH);
@@ -46,12 +50,12 @@ public boolean atBottom() {
 
     @Override
     public double getVelocity() { // average of the two? 
-        return (rightEncoder.getVelocity() + leftEncoder.getVelocity()) / 2 * ENCODER_MULTIPLIER;
+        return encoder.getVelocity() * ENCODER_MULTIPLIER;
     }
 
     @Override
     public double getHeight() {
-        return (leftEncoder.getPosition() + rightEncoder.getPosition()) / 2 * ENCODER_MULTIPLIER;
+        return encoder.getPosition() * ENCODER_MULTIPLIER;
     }
 
     @Override
@@ -61,19 +65,16 @@ public void setVoltage(double voltage) {
 
             voltage = 0.0;
 
-            leftEncoder.setPosition(MAX_HEIGHT);
-            rightEncoder.setPosition(MAX_HEIGHT);
+            encoder.setPosition(MAX_HEIGHT);
         } else if (atBottom() && voltage < 0) {
             DriverStation.reportWarning("Bottom Limit Reached", false);
 
             voltage = 0.0;
-
-            leftEncoder.setPosition(MIN_HEIGHT);
-            rightEncoder.setPosition(MIN_HEIGHT);
+
+            encoder.setPosition(MIN_HEIGHT);
         }
 
-        rightMotor.setVoltage(voltage);
-        leftMotor.setVoltage(voltage);
+        motor.setVoltage(voltage);
     }
 
     public void addTargetHeight(double delta) {