src/main/deploy/pathplanner/autos/fein.auto

@@ -1,19 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "fein"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}

src/main/deploy/pathplanner/paths/fein.path

@@ -1,70 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 7.600204918039607,
-        "y": 6.374590163938041
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 7.600204918032786,
-        "y": 7.573360655737705
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 4.459426229508197,
-        "y": 5.978995901639344
-      },
-      "prevControl": {
-        "x": 5.370491803278689,
-        "y": 5.978995901639344
-      },
-      "nextControl": {
-        "x": 2.995158619919281,
-        "y": 5.978995901639344
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 1.6543032786885246,
-        "y": 6.254713114754098
-      },
-      "prevControl": {
-        "x": 2.613319672131147,
-        "y": 6.278688524590164
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 90.0
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": -90.0
-  },
-  "useDefaultConstraints": true
-}

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

@@ -71,7 +71,6 @@ public class RobotContainer {
 
 		configureButtonBindings();
 		//elevatorSysIDBindings();
-		//elevatorOnlyBindings();
 
 		configureNamedCommands();
 
@@ -87,13 +86,6 @@ public class RobotContainer {
 		operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse));
 	}*/
 
-	private void elevatorOnlyBindings(){
-		elevator.setDefaultCommand(elevator.maintainPosition());
-		manipulatorPivot.setDefaultCommand(manipulatorPivot.maintainPosition());
-
-		driver.a().onTrue(safeMoveManipulator(ElevatorConstants.kL2Position, ManipulatorPivotConstants.kL2Position));
-	}
-
 	private void configureButtonBindings() {
 		//Default commands
 		climberPivot.setDefaultCommand(
@@ -125,7 +117,7 @@ public class RobotContainer {
 
 		manipulator.setDefaultCommand(
 			manipulator.runUntilCollected(
-				() -> 0.0
+				() -> 0
 			)
 		);
 
@@ -136,33 +128,24 @@ public class RobotContainer {
 		);
 
 		driver.rightTrigger().whileTrue(
-			manipulator.runManipulator(() -> 0.35, true)
+			manipulator.runManipulator(() -> 1, true)
 		);
 
 		driver.leftTrigger().whileTrue( 
-			manipulator.runUntilCollected(() -> 0.35)
+			manipulator.runUntilCollected(() -> 0.75)
-		);
-
-		driver.leftBumper().whileTrue(
-			manipulator.runUntilCollected(() -> 0.5)
-		);
-
-		driver.rightBumper().whileTrue(
-			manipulator.runManipulator(() -> 1, false)
 		);
 
 		driver.start().and(driver.back()).onTrue(
 			startingConfig()
 		);
 
-		driver.y().whileTrue(drivetrain.zeroHeading());
+		driver.y().onTrue(drivetrain.zeroHeading());
 		
 		driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
 		driver.povUp().whileTrue(climberPivot.runPivot(0.5));
 
 		driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
 		driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
-		driver.a().whileTrue(manipulator.runManipulator(() -> 0.5, false));
 		
 		//Operator inputs
 		operator.povUp().onTrue(
@@ -194,15 +177,19 @@ public class RobotContainer {
 		);
 
 		operator.a().onTrue(
-			safeMoveManipulator(ElevatorConstants.kL1Position, 0.0)
+			coralIntakeRoutine()
 		);
 
 		operator.x().onTrue(
-			safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
+			algaeIntakeRoutine(true)
 		);
 
 		operator.b().onTrue(
-			safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
+			algaeIntakeRoutine(false)
+		);
+
+		operator.y().whileTrue(
+			elevator.goToSetpoint(() -> ElevatorConstants.kL2Position)
 		);
 
 	}
@@ -283,9 +270,6 @@ public class RobotContainer {
 
 		sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition);
 
-		sensorTab.addDouble("dt distance", drivetrain::driveDistance);
-
-
 
  	}
 
@@ -414,8 +398,8 @@ public class RobotContainer {
 			.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
 				 elevator.maintainPosition());*/
 		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition)
-		.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
+		.andThen(elevator.goToSetpoint(() -> elevatorPosition))
-		.raceWith(elevator.maintainPosition()));
+		.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
 	}
 
 	@SuppressWarnings("unused")

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

@@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 public class DrivetrainConstants {
     // Driving Parameters - Note that these are not the maximum capable speeds of
     // the robot, rather the allowed maximum speeds
-    public static final double kMaxSpeedMetersPerSecond = 5.5;
+    public static final double kMaxSpeedMetersPerSecond = 4.8;
     public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
 
     // Chassis configuration

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

@@ -41,18 +41,16 @@ public class ElevatorConstants {
 
     public static final double kCoralIntakePosition = 0;
     public static final double kL1Position = 0;
-    public static final double kL2Position = 8;
+    public static final double kL2Position = 14.5;
-    public static final double kL3Position = 25.0;
+    public static final double kL3Position = 29.0;
-    public static final double kL4Position = 50.5;
+    public static final double kL4Position = 52.0;
     public static final double kL4TransitionPosition = 40.0;
-    public static final double kL2AlgaePosition = 21.0;
+    public static final double kL2AlgaePosition = 22.0;
     public static final double kL3AlgaePosition = 39.0;
     public static final double kProcessorPosition = 4.0;
     /**The position of the top of the elevator brace */
     public static final double kBracePosition = 0;
-    public static final double kMaxHeight = 51.0; //actual is 51
+    public static final double kMaxHeight = 52.0; //actual is 52
-
-    public static final double kVoltageLimit = 7;
 
     // 1, 7, 10 are the defaults for these, change as necessary
     public static final double kSysIDRampRate = .25;

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

@@ -34,19 +34,19 @@ public class ManipulatorPivotConstants {
     public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
     public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
 
-    public static final double kEncoderOffset = 0.780;
+    public static final double kEncoderOffset = 0.7815;
 
     public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
     public static final double kL1Position = Units.degreesToRadians(0.0);
-    public static final double kL2Position = Units.degreesToRadians(22.0);
+    public static final double kL2Position = Units.degreesToRadians(60.0);
-    public static final double kL3Position = Units.degreesToRadians(22.0);
+    public static final double kL3Position = Units.degreesToRadians(60.0);
     public static final double kL4Position = Units.degreesToRadians(45.0);
     public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
     public static final double kL3AlgaePosition = Units.degreesToRadians(175.0);
     public static final double kProcesserPosition = Units.degreesToRadians(175.0);
     public static final double kNetPosition = Units.degreesToRadians(175.0);
     /**The closest position to the elevator brace without hitting it */
-    public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0);
+    public static final double kPivotSafeStowPosition = Units.degreesToRadians(67.0);
     /**The forward rotation limit of the pivot */
     public static final double kRotationLimit = Units.degreesToRadians(175.0);
 

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

@@ -22,7 +22,7 @@ public class ModuleConstants {
     public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI;
     // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15
     // teeth on the bevel pinion
-    public static final double kDrivingMotorReduction = (45.0 * 20) / (kDrivingMotorPinionTeeth * 15);
+    public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15);
     public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
         / kDrivingMotorReduction;
 

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

@@ -1,5 +1,5 @@
 package frc.robot.constants;
 
 public class NeoMotorConstants {
-    public static final double kFreeSpeedRpm = 6000; //for kraken not neo
+    public static final double kFreeSpeedRpm = 5676;
 }

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

@@ -281,8 +281,4 @@ public class Drivetrain extends SubsystemBase {
     public void addVisionMeasurement(Pose2d pose, double timestamp){
         m_estimator.addVisionMeasurement(pose, timestamp);
     }
-
-    public double driveDistance(){
-        return m_frontLeft.getTotalDist();
-    }
 }

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

@@ -76,8 +76,6 @@ public class Elevator extends SubsystemBase {
 
         pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
 
-        
-
         feedForward = new ElevatorFeedforward(
             ElevatorConstants.kFeedForwardS, 
             ElevatorConstants.kFeedForwardG, 
@@ -143,32 +141,34 @@ public class Elevator extends SubsystemBase {
     public Command maintainPosition() {
 
         return startRun(() -> {
-/* 
+            
-            pidControllerUp.reset();
+
-            pidControllerDown.reset();
-            */
         }, 
         () -> {
-/* 
+            /* 
-            double upOutput = pidControllerUp.calculate(getEncoderPosition());
+            if (!pidControllerUp.atSetpoint()) {
-            double downOutput = pidControllerDown.calculate(getEncoderPosition());
+                if(encoder.getPosition()>pidControllerUp.getSetpoint()){
-
+                    elevatorMotor1.setVoltage(
-            if(pidControllerUp.getSetpoint()>encoder.getPosition()) 
+                        pidControllerUp.calculate(
-                elevatorMotor1.setVoltage( MathUtil.clamp(
+                            encoder.getPosition()
-                    upOutput + feedForward.calculate(0), -1, 1)
+                        ) + feedForward.calculate(0)
-                );
+                    );
-            else{
+                }else{
+                    elevatorMotor1.setVoltage(
+                        pidControllerDown.calculate(
+                            encoder.getPosition()
+                        ) + feedForward.calculate(0)
+                    );
+                }
+            } else {
                 elevatorMotor1.setVoltage(
-                    MathUtil.clamp(
-                    downOutput + feedForward.calculate(0), -1, 1)
-                );
-            } 
-            */
-
-            elevatorMotor1.setVoltage(
                     feedForward.calculate(0)
                 );
+            }*/
 
+            elevatorMotor1.setVoltage(
+                feedForward.calculate(0)
+            );
         });
         
     }
@@ -182,60 +182,28 @@ public class Elevator extends SubsystemBase {
      */
     public Command goToSetpoint(DoubleSupplier setpoint) {
 
-        if (setpoint.getAsDouble() == 0) {
+        return startRun(() -> {
-            return startRun(() -> {
         
-                pidControllerUp.reset();
+        pidControllerUp.setSetpoint(setpoint.getAsDouble());
-                pidControllerDown.reset();
+        pidControllerDown.setSetpoint(setpoint.getAsDouble());
-                pidControllerUp.setSetpoint(setpoint.getAsDouble());
+        pidControllerUp.reset();
-                pidControllerDown.setSetpoint(setpoint.getAsDouble());
+        pidControllerDown.reset();
+        }, 
+        () -> {
+            double upOutput = pidControllerUp.calculate(getEncoderPosition());
+            double downOutput = pidControllerDown.calculate(getEncoderPosition());
 
-                }, 
+            if(setpoint.getAsDouble()>encoder.getPosition())
-                () -> {
+                elevatorMotor1.setVoltage(
-                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
+                    upOutput + feedForward.calculate(0)
-                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
+                );
+            else{
+                elevatorMotor1.setVoltage(
+                    downOutput + feedForward.calculate(0)
+                );
+            }
         
-                    if(setpoint.getAsDouble()>encoder.getPosition()) 
+        }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
-                        elevatorMotor1.setVoltage( MathUtil.clamp(
-                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
-                        );
-                    else{
-                        elevatorMotor1.setVoltage(
-                            MathUtil.clamp(
-                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
-                        );
-                    }
-                
-                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
-                    .andThen(runManualElevator(() -> -.1)
-                    .until(() -> encoder.getPosition() == 0));
-
-        } else {
-            return startRun(() -> {
-        
-                pidControllerUp.reset();
-                pidControllerDown.reset();
-                pidControllerUp.setSetpoint(setpoint.getAsDouble());
-                pidControllerDown.setSetpoint(setpoint.getAsDouble());
-        
-                },
-                () -> {
-                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
-                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
-        
-                    if(setpoint.getAsDouble()>encoder.getPosition()) 
-                        elevatorMotor1.setVoltage( MathUtil.clamp(
-                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
-                        );
-                    else{
-                        elevatorMotor1.setVoltage(
-                            MathUtil.clamp(
-                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
-                        );
-                    }
-                
-                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
-        }
   
             
             /*

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

@@ -138,8 +138,4 @@ public class MAXSwerveModule {
     public void resetEncoders() {
         m_drive.setPosition(0);
     }
-
-    public double getTotalDist(){
-        return m_drive.getPosition().getValueAsDouble();
-    }
 }