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

@@ -89,17 +89,17 @@ public class RobotContainer {
 			)
 		);
 
-		//elevator.setDefaultCommand(
-			//elevator.runAssistedElevator(operator::getLeftY)
-	//	);
+		elevator.setDefaultCommand(
+			elevator.runAssistedElevator(operator::getLeftY)
+		);
 
 		manipulator.setDefaultCommand(
 			manipulator.defaultCommand()
 		);
 
-		//manipulatorPivot.setDefaultCommand(
-		//	manipulatorPivot.runAssistedPivot(operator::getRightY)
-		//);
+		manipulatorPivot.setDefaultCommand(
+			manipulatorPivot.runAssistedPivot(operator::getRightY)
+		);
 
 		//Driver inputs
 		driver.start().whileTrue(
@@ -192,12 +192,10 @@ public class RobotContainer {
 			.withPosition(4, 1)
 			.withWidget(BuiltInWidgets.kBooleanBox);
 
-		/* 
 		sensorTab.addBoolean("Algae Sensor", manipulator::getAlgaePhotoSwitch)
 			.withSize(1, 1)
 			.withPosition(4, 0)
 			.withWidget(BuiltInWidgets.kBooleanBox);
-			*/
   }
 
 	public Command getAutonomousCommand() {
@@ -213,8 +211,8 @@ public class RobotContainer {
 			ElevatorConstants.kCoralIntakePosition, 
 			ManipulatorPivotConstants.kCoralIntakePosition
 		)
-			.andThen(manipulator.runUntilCollected(1));
-		}
+			.andThen(manipulator.runUntilCollected(1, true));
+	}
 
 	/**
 	 * Moves the elevator and arm to the constant setpoints and runs the manipulator until collected
@@ -227,7 +225,7 @@ public class RobotContainer {
 			l2 ? ElevatorConstants.kL2AlgaePosition : ElevatorConstants.kL3AlgaePosition, 
 			l2 ? ManipulatorPivotConstants.kL2AlgaePosition : ManipulatorPivotConstants.kL3AlgaePosition
 		)
-			.andThen(manipulator.runUntilCollected(1));
+			.andThen(manipulator.runUntilCollected(1, false));
 	}
 
 	/**

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -14,42 +14,37 @@ public class ElevatorConstants {
     public static final int kElevatorMotor1ID = 0;
     public static final int kElevatorMotor2ID = 0;
 
+    public static final int kTopLimitSwitchID = 0;
     public static final int kBottomLimitSwitchID = 0;
 
-    // 60/11 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position
-    public static final double kEncoderConversionFactor = 11/60 * (22*0.25) * 2;
+    public static final double kEncoderConversionFactor = 0;
 
-    public static final int kCurrentLimit = 40;
+    public static final int kMotorAmpsMax = 40;
 
     public static final double kPositionControllerP = 0;
     public static final double kPositionControllerI = 0;
     public static final double kPositionControllerD = 0;
-
-    public static final double kAllowedError = 0.25;
     
-    /* 
     public static final double kVelocityControllerP = 0;
     public static final double kVelocityControllerI = 0;
     public static final double kVelocityControllerD = 0;
-    */
 
     public static final double kFeedForwardS = 0;
-    public static final double kFeedForwardG = 0.53; // calculated value
-    public static final double kFeedForwardV = 4.78; // calculated value
+    public static final double kFeedForwardG = 0.41; // calculated value
+    public static final double kFeedForwardV = 1.75; // calculated value
 
     public static final double kMaxVelocity = 0;
-    public static final double kMaxAcceleration = 0;
 
     public static final double kCoralIntakePosition = 0;
     public static final double kL1Position = 0;
-    public static final double kL2Position = 14.5;
-    public static final double kL3Position = 29.0;
-    public static final double kL4Position = 53.0;
+    public static final double kL2Position = 0;
+    public static final double kL3Position = 0;
+    public static final double kL4Position = 0;
     public static final double kL2AlgaePosition = 0;
     public static final double kL3AlgaePosition = 0;
     /**The position of the top of the elevator brace */
     public static final double kBracePosition = 0;
-    public static final double kMaxHeight = 53.0;
+    public static final double kMaxHeight = 0;
 
     // 1, 7, 10 are the defaults for these, change as necessary
     public static final double kSysIDRampRate = 1;
@@ -70,19 +65,10 @@ public class ElevatorConstants {
     
     static {
         motorConfig
-            .smartCurrentLimit(kCurrentLimit)
+            .smartCurrentLimit(kMotorAmpsMax)
             .idleMode(kIdleMode);
         motorConfig.encoder
             .positionConversionFactor(kEncoderConversionFactor)
             .velocityConversionFactor(kEncoderConversionFactor / 60.0);
-        motorConfig.closedLoop
-            .p(kPositionControllerP)
-            .i(kPositionControllerI)
-            .d(kPositionControllerD)
-            .velocityFF(0.0); // keep at zero for position pid
-        motorConfig.closedLoop.maxMotion
-            .maxAcceleration(kMaxAcceleration)
-            .maxVelocity(kMaxVelocity)
-            .allowedClosedLoopError(kAllowedError);
     }
 }

src/main/java/frc/robot/constants/ManipulatorPivotConstants.java

@@ -19,8 +19,6 @@ public class ManipulatorPivotConstants {
 
     public static final int kMotorAmpsMax = 40;
 
-    public static final double kPivotConversion = 12/60 * 20/60 * 28/12;
-
     public static final double kPivotMaxVelocity = 0;
 
     public static final double kPositionalP = 0;
@@ -83,7 +81,6 @@ public class ManipulatorPivotConstants {
         motorConfig
             .smartCurrentLimit(kMotorAmpsMax)
             .idleMode(kIdleMode);
-        motorConfig.encoder.positionConversionFactor(kPivotConversion);
     }
 
 

src/main/java/frc/robot/subsystems/Elevator.java

@@ -3,9 +3,6 @@ package frc.robot.subsystems;
 import java.util.function.DoubleSupplier;
 
 import com.revrobotics.RelativeEncoder;
-import com.revrobotics.spark.ClosedLoopSlot;
-import com.revrobotics.spark.SparkBase;
-import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
@@ -24,14 +21,12 @@ public class Elevator extends SubsystemBase {
     protected SparkMax elevatorMotor1;
     protected SparkMax elevatorMotor2;
 
-    private SparkClosedLoopController elevatorClosedLoop;
-
     protected RelativeEncoder encoder;
 
     private DigitalInput bottomLimitSwitch;
 
     private PIDController positionController;
-    //private PIDController velocityController;
+    private PIDController velocityController;
 
     private ElevatorFeedforward feedForward;
     
@@ -46,8 +41,6 @@ public class Elevator extends SubsystemBase {
             MotorType.kBrushless
         );
 
-        elevatorClosedLoop = elevatorMotor1.getClosedLoopController();
-
         elevatorMotor1.configure(
             ElevatorConstants.motorConfig, 
             ResetMode.kResetSafeParameters, 
@@ -72,13 +65,11 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kPositionControllerD
         );
 
-        /* 
         velocityController = new PIDController(
             ElevatorConstants.kVelocityControllerP, 
             ElevatorConstants.kVelocityControllerI, 
             ElevatorConstants.kVelocityControllerD
         );
-        */
 
         feedForward = new ElevatorFeedforward(
             ElevatorConstants.kFeedForwardS, 
@@ -132,7 +123,7 @@ public class Elevator extends SubsystemBase {
      * 
      * @param speed How fast the elevator moves
      * @return Sets motor voltage to move the elevator relative to the speed parameter
-     *
+     */
     public Command runAssistedElevator(DoubleSupplier speed) {
         return run(() -> {
             double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kMaxVelocity;
@@ -146,7 +137,6 @@ public class Elevator extends SubsystemBase {
         }).until(
             () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kMaxHeight);
     }
-            */
     
     /**
      * Moves the elevator to a target destination (setpoint). 
@@ -163,16 +153,6 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kMaxHeight
         );
 
-        return run(() -> {
-
-            elevatorClosedLoop.setReference(clampedSetpoint,
-                SparkBase.ControlType.kMAXMotionPositionControl,
-                ClosedLoopSlot.kSlot0,
-                feedForward.calculate(0)
-                );
-                
-        });
-/* 
         if (clampedSetpoint == 0) {
             return run(() -> {
                 double voltsOut = positionController.calculate(
@@ -201,8 +181,6 @@ public class Elevator extends SubsystemBase {
                 () -> positionController.atSetpoint() || bottomLimitSwitch.get()
             ).withTimeout(timeout);
         }
-            */
-
     }
 
     /**

src/main/java/frc/robot/subsystems/Manipulator.java

@@ -16,7 +16,7 @@ public class Manipulator extends SubsystemBase {
     private SparkMax manipulatorMotor;
 
     private DigitalInput coralBeamBreak;
-  //  private DigitalInput algaeBeamBreak; 
+    private DigitalInput algaeBeamBreak; 
 
     public Manipulator() {
         manipulatorMotor = new SparkMax(
@@ -31,7 +31,7 @@ public class Manipulator extends SubsystemBase {
         );
 
         coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
-      //  algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID);
+        algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID);
     }
 
     /**
@@ -42,8 +42,10 @@ public class Manipulator extends SubsystemBase {
      */
     public Command defaultCommand() {
         return run(() -> {
-            runUntilCollected(0.1);
-            });
+            manipulatorMotor.set(
+                algaeBeamBreak.get() ? 0.1 : 0
+            );
+        });
     }
 
     /**
@@ -68,15 +70,19 @@ public class Manipulator extends SubsystemBase {
      * @param coral Is the object a coral? (True = Coral, False = Algae)
      * @return Returns a command that sets the speed of the motor
      */
-    public Command runUntilCollected(double speed) {
+    public Command runUntilCollected(double speed, boolean coral) {
         return run(() -> {
             manipulatorMotor.set(
-                speed * -1
+                coral ? speed : speed * -1
             );
-        }).until(() -> coralBeamBreak.get());
+        }).until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
     }
 
     public boolean getCoralBeamBreak() {
         return coralBeamBreak.get();
     }
+
+    public boolean getAlgaePhotoSwitch() {
+        return algaeBeamBreak.get();
+    }
 }

src/main/java/frc/robot/subsystems/Vision.java

@@ -11,8 +11,6 @@ public class Vision{
     private DoubleSubscriber cam1ClosestTag;
     private BooleanSubscriber cam1TagDetected;
 
-    private DoubleSubscriber cam1TimeStamp;
-
     private DoubleSubscriber framerate;
 
     public Vision(){
@@ -24,6 +22,7 @@ public class Vision{
         cam1ClosestTag = visionTable.getDoubleTopic("cam1ClosestTag").subscribe(0.0);
 
         cam1TagDetected = visionTable.getBooleanTopic("cam1_visible").subscribe(false);
+      //  cam2TagDetected = visionTable.getBooleanTopic("cam2_visible").subscribe(false);
 
         framerate = visionTable.getDoubleTopic("fps").subscribe(0.0);
     }
@@ -40,10 +39,6 @@ public class Vision{
         return cam1TagDetected.get();
     }
 
-    public double getCam1TimeStamp(){
-        return cam1GlobalPose.getLastChange();
-    }
-
     public double getFPS(){
         return framerate.get();
     }