practice day changes

Author: Yuriana101

…loy/pathplanner/autos/1 Coral - RC .auto …ploy/pathplanner/autos/1 Coral - RC.autosrc/main/deploy/pathplanner/autos/1 Coral - RC .auto renamed to src/main/deploy/pathplanner/autos/1 Coral - RC.auto src/main/deploy/pathplanner/autos/1 Coral - RC .auto renamed to src/main/deploy/pathplanner/autos/1 Coral - RC.auto

@@ -32,7 +32,7 @@ public final class AlgaeCoralerConstants {
 	//Speeds
 	public static final double ALGAE_IN_SPEED = -1; 
 	public static final double ALGAE_OUT_SPEED = 1;
-	public static final double CORAL_OUT_SPEED = -0.23;
+	public static final double CORAL_OUT_SPEED = -0.25;
 	public static final double HOLDING_SPEED = 0;	
 	public static final double AUTO_SPEED = -0.43;
 

…loy/pathplanner/autos/2 Coral - RC .auto …ploy/pathplanner/autos/2 Coral - RC.autosrc/main/deploy/pathplanner/autos/2 Coral - RC .auto renamed to src/main/deploy/pathplanner/autos/2 Coral - RC.auto src/main/deploy/pathplanner/autos/2 Coral - RC .auto renamed to src/main/deploy/pathplanner/autos/2 Coral - RC.auto

@@ -2,6 +2,7 @@
 
 import static frc.robot.GlobalConstants.*;
 import static frc.robot.GlobalConstants.Controllers.DRIVER_CONTROLLER;
+import static frc.robot.GlobalConstants.Controllers.OPERATOR_CONTROLLER;
 import static frc.robot.Subsystems.Climber.ClimberConstants.*;
 
 import org.littletonrobotics.junction.Logger;
@@ -27,9 +28,9 @@ public Climber() {
 	public void runState() {
 		io.updateInputs(inputs);
 
-		if (Math.abs(DRIVER_CONTROLLER.getLeftTriggerAxis()) > 0.5) {
+		if (Math.abs(DRIVER_CONTROLLER.getRightTriggerAxis()) > 0.5 || Math.abs(OPERATOR_CONTROLLER.getRightTriggerAxis()) > 0.5) {
 			io.setSpeed(ClimberStates.IN.getSpeed());
-		} else if (Math.abs(DRIVER_CONTROLLER.getRightTriggerAxis()) >= 0.5) {
+		} else if (Math.abs(DRIVER_CONTROLLER.getLeftTriggerAxis()) > 0.5 || Math.abs(OPERATOR_CONTROLLER.getLeftTriggerAxis()) > 0.5) {
 			io.setSpeed(ClimberStates.OUT.getSpeed());
 		} else {
 			io.setSpeed(ClimberStates.IDLE.getSpeed());

…loy/pathplanner/autos/6 Coral - RC .auto …ploy/pathplanner/autos/6 Coral - RC.autosrc/main/deploy/pathplanner/autos/6 Coral - RC .auto renamed to src/main/deploy/pathplanner/autos/6 Coral - RC.auto src/main/deploy/pathplanner/autos/6 Coral - RC .auto renamed to src/main/deploy/pathplanner/autos/6 Coral - RC.auto

@@ -14,6 +14,7 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
@@ -44,6 +45,7 @@ public class Drive extends Subsystem<DriveStates> {
 	// Swerve:
 	private SwerveInputStream swerveInputs;
 	private SwerveDrive swerveDrive;
+	private boolean fieldRelative;
 
 	// PID Controllers:
 	private ProfiledPIDController xPID;
@@ -54,6 +56,7 @@ public class Drive extends Subsystem<DriveStates> {
 	private Pose2d target;
 	private Command pathfindingCommand;
 	private boolean first;
+	private boolean slow; 
 	// INSTANCE:
 	private static Drive instance;
 
@@ -70,6 +73,7 @@ public Drive() {
 
 		// Create field:
 		field = new Field2d();
+		fieldRelative = true;
 
 		//Logging for pathplanner
 		PathPlannerLogging.setLogActivePathCallback(poses -> {
@@ -95,6 +99,8 @@ public Drive() {
 		yPID.setTolerance(Y_TOLERANCE.magnitude());
 		rotationPID.setTolerance(ROTATION_TOLERANCE.in(Radians));
 
+		slow = false; 
+
 		// Setup SwerveDrive:
 		SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
 		try {
@@ -146,14 +152,14 @@ public Drive() {
 
 	private void establishTriggers() {
 		// Auto Align Activate:
-		addRunnableTrigger( () -> { this.BeginAligning(DriveStates.AUTO_ALIGNING_REEF); }, () -> Controllers.DRIVER_CONTROLLER.getPOV() == 0 && getState() == DriveStates.MANUAL );
-		addRunnableTrigger( () -> { this.BeginAligning(DriveStates.AUTO_ALIGNING_FEEDER); }, () -> Controllers.DRIVER_CONTROLLER.getPOV() == 90 && getState() == DriveStates.MANUAL );
+		//addRunnableTrigger( () -> { this.BeginAligning(DriveStates.AUTO_ALIGNING_REEF); }, () -> Controllers.DRIVER_CONTROLLER.getPOV() == 0 && getState() == DriveStates.MANUAL );
+		//addRunnableTrigger( () -> { this.BeginAligning(DriveStates.AUTO_ALIGNING_FEEDER); }, () -> Controllers.DRIVER_CONTROLLER.getPOV() == 90 && getState() == DriveStates.MANUAL );
 		//addRunnableTrigger( () -> { this.BeginAligning(DriveStates.AUTO_ALIGNING_CAGES); }, () -> Controllers.DRIVER_CONTROLLER.getPOV() == 180 && getState() == DriveStates.MANUAL );
 		addRunnableTrigger(() -> swerveDrive.lockPose(), () -> DRIVER_CONTROLLER.getAButton());
 		// Auto Align Cancel:
 
-		addRunnableTrigger( () -> { this.EndAligning(); }, () -> atSetpoint() && getState() != DriveStates.MANUAL );
-		addRunnableTrigger( () -> { this.EndAligning(); }, Controllers.DRIVER_CONTROLLER::getXButtonPressed );
+		//addRunnableTrigger( () -> { this.EndAligning(); }, () -> atSetpoint() && getState() != DriveStates.MANUAL );
+		//addRunnableTrigger( () -> { this.EndAligning(); }, Controllers.DRIVER_CONTROLLER::getXButtonPressed );
 	}
 
 	// Aligner Transition Methods:
@@ -228,18 +234,35 @@ public void runState() {
 		} else {
 			// Manual Control
 			// Slow Mode:
-			if (Controllers.DRIVER_CONTROLLER.getLeftBumperButton()) {
-				swerveInputs.scaleTranslation(0.5);
-				swerveInputs.scaleRotation(0.5);
+			if (slow) {
+				if(Controllers.DRIVER_CONTROLLER.getLeftBumperButtonPressed()) {
+					slow = false;
+				}
+				swerveInputs.scaleTranslation(0.33);
+				swerveInputs.scaleRotation(0.33); 
 			} else {
+				if(Controllers.DRIVER_CONTROLLER.getLeftBumperButtonPressed()) {
+					slow = true;
+				}
 				swerveInputs.scaleTranslation(1);
 				swerveInputs.scaleRotation(1);
 			}
 			// Lock Pose:
 			if (Controllers.DRIVER_CONTROLLER.getRightBumperButton()) {
 				swerveDrive.lockPose();
 			}
-			swerveDrive.driveFieldOriented(swerveInputs.get());
+
+			if (fieldRelative) {
+				if (DRIVER_CONTROLLER.getBackButtonPressed()) {
+					fieldRelative = false;
+				}
+				swerveDrive.driveFieldOriented(swerveInputs.get());
+			} else {
+				if (DRIVER_CONTROLLER.getBackButtonPressed()) {
+					fieldRelative = true;
+				}
+				swerveDrive.drive(swerveInputs.get());
+			}
 		}
 		// Update Alignment Targets:
 		if (!DriverStation.getAlliance().isEmpty()) {
@@ -254,6 +277,8 @@ public void runState() {
 		}
 		field.getObject("ROBOT").setPose(swerveDrive.getPose());
 		// Logging:
+		SmartDashboard.putBoolean("SLOW", slow);
+		SmartDashboard.putBoolean("FIELD RELATIVE", fieldRelative);
 		SmartDashboard.putData("Field", field);
 		SmartDashboard.putString("DRIVE_STATE", getState().getStateString());
 		Logger.recordOutput("Pose", swerveDrive.getPose());