Drivetrain edits - removed coral align

src/main/java/frc/robot/Robot.java

@@ -101,7 +101,7 @@ public void teleopPeriodic() {
     // m_robotContainer.getArmAngle();
     // m_robotContainer.getHeightSet();
 
-    // m_robotContainer.printPose();
+    m_robotContainer.printPose();
   }
 
   @Override
@@ -120,5 +120,6 @@ public void simulationInit() {}
 
   /** This function is called periodically whilst in simulation. */
   @Override
-  public void simulationPeriodic() {}
+  public void simulationPeriodic() {
+  }
 }

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

@@ -6,9 +6,7 @@
 
 
 import frc.robot.Constants.OIConstants;
-import frc.robot.commands.CoralStationAlign;
 import frc.robot.commands.FieldOrientedDrive;
-// import frc.robot.subsystems.ColourSensor;
 import frc.robot.subsystems.DriveSubsystem;
 
 import com.pathplanner.lib.commands.PathPlannerAuto;
@@ -24,8 +22,9 @@
 import java.util.Optional;
 
 import edu.wpi.first.math.geometry.Transform3d;
+import java.util.List;
+import org.photonvision.targeting.PhotonTrackedTarget;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
@@ -39,16 +38,14 @@ public class RobotContainer {
   private final DriveSubsystem m_driveSubsystem= new DriveSubsystem();
   private final AprilTagPoseEstimator m_poseEstimator = new AprilTagPoseEstimator();
 
-  private boolean active = false;
-
   // Replace with CommandPS4Controller or CommandJoystick if needed
   private final CommandXboxController m_driverController =
   new CommandXboxController(OIConstants.kDriverControllerPort);
-  private final CommandPS4Controller m_manualLiftController= new CommandPS4Controller(OIConstants.kManualLiftControllerPort);
   private final FieldOrientedDrive fieldOrientedDrive= new FieldOrientedDrive(m_driveSubsystem, m_driverController);
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the trigger bindings
+    configureBindings();
     m_driveSubsystem.setDefaultCommand(fieldOrientedDrive);
   }
 
@@ -74,7 +71,6 @@ private void configureBindings() {
     m_driverController.x().onTrue( // Reset gyro whenever necessary
       new InstantCommand(() -> m_driveSubsystem.resetGyro(), m_driveSubsystem)
     );
-    m_driverController.leftBumper().onTrue(new CoralStationAlign(m_driveSubsystem, m_driverController));
   }
 
   /**
@@ -112,7 +108,6 @@ public Pose2d getPose(){
     // return new AprilTagAlignment(m_driveSubsystem, new AprilTagPoseEstimator(), 3, 0.5);
   }
 
-  // TODO: Delete
   public void printPose() {
     Optional<Transform3d> opt = m_poseEstimator.getRobotToSeenTag();
     if(opt.isPresent()) {
@@ -123,6 +118,4 @@ public void printPose() {
       SmartDashboard.putNumber("robot2tag/r/yaw", r2t.getRotation().getZ());
     }
   }
-
 }
-

src/main/java/frc/robot/commands/FieldOrientedDrive.java

@@ -7,6 +7,7 @@
 import frc.robot.Constants.FieldOrientedDriveConstants;
 import frc.robot.Constants.TestingConstants;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -66,45 +67,29 @@ public void initialize() {
     // Called every time the scheduler runs while the command is scheduled.
     @Override
     public void execute() {
-        // toggle slow mode on press of A
-        if (xboxController.a().getAsBoolean() && !isAPressed){
-            slowModeActive = !slowModeActive; // toggle
-            isAPressed = true;
-        }
-        // we ignore anything while its pressed, to avoid rapid switching, by having a "hasToggled" flag while A is pressed
-        if (!xboxController.a().getAsBoolean() && isAPressed){ // we only turn isAPressed off if it's on (duh)
-            isAPressed = false;
-        }
-
-        // ditto for field-centric-turning with B press
-        if (xboxController.b().getAsBoolean() && !isBPressed) {
-            fieldCentricTurning = !fieldCentricTurning;
-            isBPressed = true;
-        }
-        if (!xboxController.b().getAsBoolean() && isBPressed) {
-            isBPressed = false;
-        }
-
+        xboxController.a().onTrue(Commands.runOnce(() -> {slowModeActive =! slowModeActive;}));
         SmartDashboard.putBoolean("Slow Mode Active", slowModeActive);
-        SmartDashboard.putBoolean("Field Centric Driving", fieldCentricTurning);
 
-        SmartDashboard.putBoolean("A is on?", isAPressed);
-        SmartDashboard.putBoolean("B is on?", isBPressed);
+        maximumAcceleration= (slowModeActive ? AccelerationLimiterConstants.maximumAccelerationReduced 
+        : AccelerationLimiterConstants.maximumAcceleration);
 
-        maximumAcceleration=(slowModeActive ? AccelerationLimiterConstants.maximumAccelerationReduced : AccelerationLimiterConstants.maximumAcceleration);
+        // SmartDashboard.putNumber("Goal bearing", goalBearing);
 
-        //Both joysticks assumes the right to be bearing 0 and then works clockwise from there. To have bearing 0 be in front, the bearing
-        //has to be moved back by 90 degrees/ 1/2 PI
-        //If right joystick is not being moved retain previous bearing
+        // Both joysticks assumes the right to be bearing 0 and then works clockwise from there. 
+        // To have bearing 0 be in front, the bearing has to be moved back by 90 degrees/ 1/2 PI
+        // If right joystick is not being moved retain previous bearing
         if (Math.hypot(xboxController.getRightY(), xboxController.getRightX()) > 0.9) {
             joystickTurnBearing = Math.atan2(xboxController.getRightY(), xboxController.getRightX()) + Math.PI/2;
         }
-        //error tolerance of 2 degrees
-        if (Math.abs(joystickTurnBearing - goalBearing) > Math.PI/180 * FieldOrientedDriveConstants.bearingTolerance){
+        // SmartDashboard.putNumber("Turn: Right Joystick bearing", joystickTurnBearing);
+
+        // error tolerance of 2 degrees
+        if (Math.abs(joystickTurnBearing-goalBearing) > Math.PI/180 * FieldOrientedDriveConstants.bearingTolerance){
             goalBearing = -joystickTurnBearing;
             bearingPIDController.reset();
             bearingPIDController.setSetpoint(goalBearing);
         }
+
         robotBearing = driveSubsystem.getGyroAngle();
         if (robotBearing < 0){
             //converting to within range 0 to 360 degrees
@@ -113,7 +98,7 @@ public void execute() {
         //converting to radians
         robotBearing = robotBearing / 180 * Math.PI;
 
-        //it looks cooked but that's because the controller is mapped kinda funny
+        // it looks cooked but that's because the controller is mapped kinda funny
         joystickMoveBearing = Math.atan2(xboxController.getLeftX(), -xboxController.getLeftY());
         joystickMoveBearing = joystickMoveBearing + robotBearing - 2*Math.PI;
 
@@ -140,10 +125,12 @@ else if (ySpeed < previousYSpeed - maximumAcceleration){
 
         // depending on how you want to control the robot, toggled by button B
         if (fieldCentricTurning) {
-            // Datis and Ella prefers this
+            // Ollie Pinfield and Joe Heap prefer this
+            // Joystick controls absolute heading (relative to starting position)
             rotSpeed = bearingPIDController.calculate(robotBearing);
         } else {
-            // William and Sam prefers this
+            // William and Sam prefer this
+            // Joystick controls rotational movement
             rotSpeed = -xboxController.getRightX();
         }
 
@@ -167,7 +154,6 @@ else if (ySpeed < previousYSpeed - maximumAcceleration){
     public void end(boolean interrupted) {
         driveSubsystem.drive(0.0, 0.0, 0.0, false);
     }
-
     @Override
     public boolean isFinished() {
         return false;

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

@@ -296,6 +296,6 @@ public void resetGyro(){
    * Gets angle of the gyro in degrees
    */
   public double getGyroAngle(){
-    return m_gyro.getAngle()%360;
+    return m_gyro.getAngle() % 360;
   }
 }

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

@@ -10,9 +10,9 @@
 
 import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.PersistMode;
 import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.RelativeEncoder;
@@ -101,6 +101,7 @@ public void setDesiredState(SwerveModuleState desiredState) {
     correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition()));
 
     // Command driving and turning SPARKS towards their respective setpoints.
+
     m_drivingClosedLoopController.setSetpoint(correctedDesiredState.speedMetersPerSecond, ControlType.kVelocity);
     m_turningClosedLoopController.setSetpoint(correctedDesiredState.angle.getRadians(), ControlType.kPosition);