end of southern maine code

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

@@ -109,8 +109,8 @@ public class RobotContainer {
 
 		drivetrain.setDefaultCommand(
 			drivetrain.drive(
-				() -> Math.pow(driver.getLeftY(), 3),
+				() -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
-				() -> Math.pow(driver.getLeftX(), 3),
+				() -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
 				driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
 				() -> true
 			)
@@ -143,7 +143,7 @@ public class RobotContainer {
 		);
 
 		driver.leftTrigger().whileTrue( 
-			manipulator.runUntilCollected(() -> 0.75).andThen(manipulator.retractCommand(() -> 0.25))
+			manipulator.runUntilCollected(() -> 0.75)
 		);
 
 		driver.start().and(driver.back()).onTrue(
@@ -153,6 +153,8 @@ public class RobotContainer {
 		driver.y().whileTrue(drivetrain.zeroHeading());
 		
 		driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false));
+
+		driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
 		
 		/* 
 		driver.rightBumper().whileTrue(
@@ -201,26 +203,28 @@ public class RobotContainer {
 			)
 		);
 
-		operator.start().toggleOnTrue(climberPivot.runPivot(() -> operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
+		operator.back().onTrue(elevator.homeCommand());
+
+		operator.start().toggleOnTrue(climberPivot.runPivot(() -> -operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
 
 		operator.a().onTrue(
-			safeMoveManipulator(ElevatorConstants.kL1Position, 0.0)
+			safeMoveManipulator(ElevatorConstants.kL1Position, ManipulatorPivotConstants.kStartingPosition)
 		);
 
 		operator.x().onTrue(
 			safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
-				.alongWith(manipulator.runManipulator(() -> 0.5, false))
+				.alongWith(manipulator.runManipulator(() -> 0.85, false))
 				.until(() -> driver.a().getAsBoolean())
 		);
 
 		operator.b().onTrue(
 			safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
-				.alongWith(manipulator.runManipulator(() -> 0.5, false))
+				.alongWith(manipulator.runManipulator(() -> 0.85, false))
 				.until(() -> driver.a().getAsBoolean())
 		);
 
 		operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
-			.alongWith(manipulator.runManipulator(() -> 0.5, false))
+			.alongWith(manipulator.runManipulator(() -> 0.85, false))
 			.until(() -> driver.a().getAsBoolean())
 		);
 

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

@@ -43,8 +43,8 @@ public class ElevatorConstants {
 
     public static final double kCoralIntakePosition = 0;
     public static final double kL1Position = 0;
-    public static final double kL2Position = 9;
+    public static final double kL2Position = 11;
-    public static final double kL3Position = 23.0;
+    public static final double kL3Position = 27;
     public static final double kL4Position = 50.5;
     public static final double kL4TransitionPosition = 40.0;
     public static final double kL2AlgaePosition = 23.0;

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

@@ -29,26 +29,27 @@ public class ManipulatorPivotConstants {
     public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41
     public static final double kFeedForwardV = 0.68; //calculated value 0.68
 
-    public static final double kFFGravityOffset = Units.degreesToRadians(135.0);
+    public static final double kFFGravityOffset = Units.degreesToRadians(135.0+90);
 
     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.78-0.25;
 
-    public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
+    public static final double kStartingPosition = Units.degreesToRadians(90);
-    public static final double kL1Position = Units.degreesToRadians(0.0);
+    public static final double kCoralIntakePosition = Units.degreesToRadians(175.0+90);
-    public static final double kL2Position = Units.degreesToRadians(22.0);
+    public static final double kL1Position = Units.degreesToRadians(0.0+90);
-    public static final double kL3Position = Units.degreesToRadians(22.0);
+    public static final double kL2Position = Units.degreesToRadians(22.0+90);
-    public static final double kL4Position = Units.degreesToRadians(45.0);
+    public static final double kL3Position = Units.degreesToRadians(22.0+90);
-    public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
+    public static final double kL4Position = Units.degreesToRadians(45.0+90);
-    public static final double kL3AlgaePosition = Units.degreesToRadians(175.0);
+    public static final double kL2AlgaePosition = Units.degreesToRadians(175.0+90);
-    public static final double kProcessorPosition = Units.degreesToRadians(175.0);
+    public static final double kL3AlgaePosition = Units.degreesToRadians(175.0+90);
-    public static final double kNetPosition = Units.degreesToRadians(175.0);
+    public static final double kProcessorPosition = Units.degreesToRadians(175.0+90);
+    public static final double kNetPosition = Units.degreesToRadians(175.0+90);
     /**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(71.0+90);
     /**The forward rotation limit of the pivot */
-    public static final double kRotationLimit = Units.degreesToRadians(175.0);
+    public static final double kRotationLimit = Units.degreesToRadians(175.0+90);
 
     public static final double kSysIDRampRate = 1;
     public static final double kSysIDStepVolts = 7;

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

@@ -4,7 +4,7 @@ public class OIConstants {
     public static final int kDriverControllerPort = 0;
     public static final int kOperatorControllerPort = 1;
     
-    public static final double kDriveDeadband = 0.05;
+    public static final double kDriveDeadband = Math.pow(0.05, 3);
 
     public static final String kAutoTab = "Auto Tab";
     public static final String kSensorsTab = "Sensors Tab";

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

@@ -174,6 +174,14 @@ public class Elevator extends SubsystemBase {
         
     }
 
+    public Command homeCommand(){
+        return run(() -> {
+            elevatorMotor1.setVoltage(0.5);
+        })
+        .until(() -> elevatorMotor1.getOutputCurrent() > 5)
+        .andThen(run(() -> encoder.setPosition(0)));
+    }
+
     /**
      * Moves the elevator to a target destination (setpoint). 
      * 
@@ -208,7 +216,7 @@ public class Elevator extends SubsystemBase {
                     }
                 
                 }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
-                    .andThen(runManualElevator(() -> -.1)
+                    .andThen(runManualElevator(() -> -.5)
                     .until(() -> encoder.getPosition() == 0));
 
         } else {

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

@@ -47,7 +47,7 @@ public class ManipulatorPivot extends SubsystemBase {
         );
         pidController.setSetpoint(0);
 
-        pidController.enableContinuousInput(0, 180);
+        pidController.enableContinuousInput(0, 280);
         
         feedForward = new ArmFeedforward(
             ManipulatorPivotConstants.kFeedForwardS,