Final changes from comp

redshiftrobotics · Mar 20, 2024 · 82767bd · 82767bd
1 parent 75d5339
commit 82767bd
Show file tree

Hide file tree

Showing 5 changed files with 13 additions and 25 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -44,8 +44,8 @@ public static enum Bot {
 				currentBot = Bot.SIM_BOT;
 				break;
 
-			case "03282B00": // Wood Bot Serial Number
-				currentBot = Bot.WOOD_BOT;
+			case "03282B00": // Wood Bot Serial Number (switched rios during comp)
+				currentBot = Bot.COMP_BOT;
 				break;
 
 			case "03238024": // Practice (Now comp) Bot Serial Number
@@ -226,7 +226,7 @@ public static class SwerveModuleConstants {
 					VELOCITY_MOTOR_ID_BR = 42;
 					ANGULAR_MOTOR_ID_BR = 6;
 					ANGULAR_MOTOR_ENCODER_ID_BR = 3;
-					ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.046142578125  + 0.5;
+					ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.046142578125 + 0.5;
 					break;
 
 				case COMP_BOT:

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -13,7 +13,6 @@
 import frc.robot.commands.HangControl;
 import frc.robot.commands.AimAtAngle;
 import frc.robot.commands.ArmRotateTo;
-import frc.robot.commands.AutoRotateTo;
 import frc.robot.commands.SetLightstripColorFor;
 import frc.robot.subsystems.ChassisDriveInputs;
 import frc.robot.subsystems.LightStrip;
@@ -239,10 +238,9 @@ public void setUpDriveController() {
 
 			xbox.y().onTrue(Commands.runOnce(inputs::toggleFieldRelative));
 
-			xbox.rightBumper().onTrue(new AutoRotateTo(drivetrain,
-			Rotation2d.fromDegrees(0), true));
-			xbox.leftBumper().onTrue(new AutoRotateTo(drivetrain,
-			Rotation2d.fromDegrees(-90 * flip), true));
+			xbox.rightBumper().whileTrue(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(0)));
+			xbox.leftBumper().whileTrue(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(-90 * flip)));
+			xbox.a().whileTrue(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(-180)));
 
 			if (vision != null)
 				xbox.x().onTrue(Commands.runOnce(vision::toggleUsing, vision));

diff --git a/src/main/java/frc/robot/commands/AimAtAngle.java b/src/main/java/frc/robot/commands/AimAtAngle.java
@@ -32,25 +32,21 @@ public AimAtAngle(SwerveDrivetrain drivetrain, ChassisDriveInputs chassisDriveIn
 				RobotMovementConstants.ROTATION_PID_I,
 				RobotMovementConstants.ROTATION_PID_D);
 		rotatePID.enableContinuousInput(-Math.PI, Math.PI);
-		rotatePID.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
 		rotatePID.setSetpoint(direction.getRadians());
 
-		addRequirements(drivetrain, chassisDriveInputs);
+		addRequirements(drivetrain);
 	}
 
-
 	@Override
 	public void initialize() {
+		rotatePID.reset();
 		drivetrain.toDefaultStates();
 	}
 
 	@Override
 	public void execute() {
 
-		double rotationSpeed = 0;
-		if (!rotatePID.atSetpoint()) {
-			rotationSpeed = rotatePID.calculate(drivetrain.getHeading().getRadians());
-		}
+		double rotationSpeed = rotatePID.calculate(drivetrain.getHeading().getRadians());
 
 		double xSpeed = 0;
 		double ySpeed = 0;
@@ -61,22 +57,17 @@ public void execute() {
 			fieldRelative = chassisDriveInputs.isFieldRelative();
 		}
 
-		ChassisSpeeds desiredSpeeds = new ChassisSpeeds(xSpeed, ySpeed, 0);
-		if (fieldRelative) desiredSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(desiredSpeeds, drivetrain.getHeading());
-		desiredSpeeds.omegaRadiansPerSecond = rotationSpeed;
-
-		drivetrain.setDesiredState(desiredSpeeds, false);
-
+		drivetrain.setDesiredState(new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed), fieldRelative);
 		drivetrain.updateSmartDashboard();
 	}
 
 	@Override
 	public boolean isFinished() {
-		return (chassisDriveInputs == null) && (rotatePID.atSetpoint());
+		return false;
 	}
 
 	@Override
 	public void end(boolean interrupted) {
-		drivetrain.stop();
+		drivetrain.setDesiredState(new ChassisSpeeds());
 	}
 }
diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java
@@ -55,7 +55,6 @@ public void execute() {
 		final double currentAngle = drivetrain.getHeading().getRadians();
 
 		double turnsSeed = rotatePID.calculate(currentAngle, this.currentAngleGoal);
-		// SmartDashboard.putNumber("Turn Speed Auto", turnsSeed);
 
 		drivetrain.setDesiredState(new ChassisSpeeds(0, 0, (turnsSeed)));
 

diff --git a/src/main/java/frc/robot/subsystems/ChassisDriveInputs.java b/src/main/java/frc/robot/subsystems/ChassisDriveInputs.java
@@ -22,7 +22,7 @@ public class ChassisDriveInputs extends SubsystemBase {
 	private final int maxSpeedLevel = DriverConstants.NUMBER_OF_SPEED_OPTIONS - 1;
 
 	private int speedLevel = maxSpeedLevel / 2;
-	private boolean isFieldRelative = false;
+	private boolean isFieldRelative = true;
 
 	private boolean maxSpeedMode = false;