From c34d9ebf4581a2450fe63097371d669aeb73e40a Mon Sep 17 00:00:00 2001 From: 12sliu <12sliu@brightoncollege.net> Date: Wed, 25 Feb 2026 16:01:08 +0000 Subject: [PATCH 1/2] small refactor --- .../robot/commands/FieldOrientedDrive.java | 34 ++++++++++--------- .../frc/robot/subsystems/DriveSubsystem.java | 2 +- 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/commands/FieldOrientedDrive.java b/src/main/java/frc/robot/commands/FieldOrientedDrive.java index 07902ac..6879be4 100644 --- a/src/main/java/frc/robot/commands/FieldOrientedDrive.java +++ b/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; @@ -60,42 +61,42 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (xboxController.a().getAsBoolean()&&hasToggled==false){ - slowModeActive=!slowModeActive; - hasToggled=true; - } - if(xboxController.a().getAsBoolean()==false){ - //button no longer pressed, able to toggle slow mode now - hasToggled=false; - } + xboxController.a().onTrue(Commands.runOnce(() -> {slowModeActive =! slowModeActive;})); SmartDashboard.putBoolean("Slow Mode Active", slowModeActive); - 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 - if (Math.hypot(xboxController.getRightY(), xboxController.getRightX())> 0.9) { + // 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; } // SmartDashboard.putNumber("Turn: Right Joystick bearing", joystickTurnBearing); - //error tolerance of 2 degrees - if (Math.abs(joystickTurnBearing-goalBearing)>Math.PI/180*FieldOrientedDriveConstants.bearingTolerance){ + + // 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){ + if (robotBearing < 0){ //converting to within range 0 to 360 degrees robotBearing+=360; } //converting to radians robotBearing = robotBearing / 180 * Math.PI; // SmartDashboard.putNumber("Robot bearing", robotBearing); + //it looks cooked but that's because the controller is mapped kinda funny joystickMoveBearing = Math.atan2(xboxController.getLeftX(), -xboxController.getLeftY()); // SmartDashboard.putNumber("Drive: Left joystick bearing", joystickMoveBearing); + //gyro measures angles anticlockwise. joystickMoveBearing=joystickMoveBearing+robotBearing-2*Math.PI; @@ -146,6 +147,7 @@ else if (ySpeed Date: Thu, 26 Feb 2026 11:43:09 +0000 Subject: [PATCH 2/2] deleted deprecated coralalign --- src/main/java/frc/robot/Robot.java | 5 +- src/main/java/frc/robot/RobotContainer.java | 13 +- .../frc/robot/commands/CoralStationAlign.java | 133 ------------------ .../robot/commands/FieldOrientedDrive.java | 63 +-------- .../frc/robot/subsystems/MAXSwerveModule.java | 3 +- 5 files changed, 13 insertions(+), 204 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/CoralStationAlign.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f38e922..a3840ab 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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() { + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ec20b0d..01d9ecf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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 opt = m_poseEstimator.getRobotToSeenTag(); if(opt.isPresent()) { @@ -123,6 +118,4 @@ public void printPose() { SmartDashboard.putNumber("robot2tag/r/yaw", r2t.getRotation().getZ()); } } - } - diff --git a/src/main/java/frc/robot/commands/CoralStationAlign.java b/src/main/java/frc/robot/commands/CoralStationAlign.java deleted file mode 100644 index 98a4775..0000000 --- a/src/main/java/frc/robot/commands/CoralStationAlign.java +++ /dev/null @@ -1,133 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Constants.CoralStationAlignConstants; -import frc.robot.Constants.FieldOrientedDriveConstants; -import frc.robot.Constants.TestingConstants; -import frc.robot.subsystems.AprilTagPoseEstimator; -import frc.robot.subsystems.DriveSubsystem; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class CoralStationAlign extends Command { - private DriveSubsystem driveSubsystem; - private PIDController bearingPIDController; - private CommandXboxController controller; - private AprilTagPoseEstimator estimator; - private PIDController xPIDController; - private boolean active; - /** - * Creates a new CoralStationAlign, which locks the robot's bearing towards the left coral station - * Coral station require is decided by the general direction of left joystick - * cancel by pressing right bumper - */ - // public CoralStationAlign(DriveSubsystem driveSubsystem, CommandXboxController controller, AprilTagPoseEstimator estimator) { - // this.driveSubsystem=driveSubsystem; - // this.controller=controller; - // this.estimator=estimator; - // // Use addRequirements() here to declare subsystem dependencies. - // addRequirements(driveSubsystem, estimator); - // } - public CoralStationAlign(DriveSubsystem driveSubsystem, CommandXboxController controller) { - this.driveSubsystem=driveSubsystem; - this.controller=controller; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(driveSubsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - //remember that gyro is flipped - bearingPIDController=new PIDController( - CoralStationAlignConstants.kRotP, - CoralStationAlignConstants.kRotI, - CoralStationAlignConstants.kRotD - ); - //left coral station bearing is at 126 to 0, where 0 is forwards for the robot. - - double currentBearing=driveSubsystem.getGyroAngle(); - if (currentBearing<0){ - currentBearing+=360; - } - // SmartDashboard.putNumber("robotBearing", currentBearing); - if (currentBearing<180){ - // SmartDashboard.putBoolean("SetRightCS", false); - bearingPIDController.setSetpoint(CoralStationAlignConstants.leftCoralStationRot*Math.PI/180); - } - else{ - // SmartDashboard.putBoolean("SetRightCS", true); - bearingPIDController.setSetpoint(CoralStationAlignConstants.rightCoralStationRot*Math.PI/180); - } - bearingPIDController.setTolerance(FieldOrientedDriveConstants.bearingTolerance); - bearingPIDController.enableContinuousInput(0, 2*Math.PI); - // xPIDController=new PIDController( - // AprilTagAlignmentConstants.kMoveP, - // AprilTagAlignmentConstants.kMoveI, - // AprilTagAlignmentConstants.kMoveD - // ); - // xPIDController.setSetpoint(CoralStationAlignConstants.xDisplacement); - // xPIDController.setSetpoint(CoralStationAlignConstants.xTolerance); - active=true; -} - - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - SmartDashboard.putNumber("robotBearing", driveSubsystem.getGyroAngle()); - SmartDashboard.putBoolean("Coral Station Align Active", true); - //check to end command - if (controller.rightBumper().getAsBoolean()){ - //end command the moment rightBumper is pressed - active=false; - } - if (Math.hypot(controller.getRightY(), controller.getRightX())> 0.9) { - if (controller.getRightX()>0){ - //right stick facing right, which swaps coral station from left to Right. Bearing 240 - bearingPIDController.reset(); - bearingPIDController.setSetpoint(CoralStationAlignConstants.rightCoralStationRot*Math.PI/180); - } - else{ - bearingPIDController.reset(); - bearingPIDController.setSetpoint(CoralStationAlignConstants.leftCoralStationRot*Math.PI/180); - } - } - double xSpeed=0; - double bearing = Math.toRadians(driveSubsystem.getGyroAngle()); - double joystickMoveBearing = Math.atan2(controller.getLeftX(), -controller.getLeftY()); - double joystickMoveMagnitude = Math.hypot(controller.getLeftX(), controller.getLeftY()); - // Optional pose = estimator.getRobotToSeenTag(); - // if (pose.isEmpty()){ - // SmartDashboard.putBoolean("April Tag in view", false); - // xSpeed= joystickMoveMagnitude * Math.cos(joystickMoveBearing) * TestingConstants.maximumSpeedReduced; - // } - // else{ - // SmartDashboard.putBoolean("April Tag in view", true); - // SmartDashboard.putNumber("XDisplacement", pose.get().getX()); - // //reverse xSpeed because drivetrain must drive at positive sped to reduce xdistance - // xSpeed= -xPIDController.calculate(pose.get().getX()); - // } - xSpeed= joystickMoveMagnitude * Math.cos(joystickMoveBearing) * TestingConstants.maximumSpeedReduced; - double ySpeed= joystickMoveMagnitude * Math.sin(joystickMoveBearing) * TestingConstants.maximumSpeedReduced; - driveSubsystem.drive(xSpeed, -ySpeed, bearingPIDController.calculate(bearing), false); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - SmartDashboard.putBoolean("Coral Station Align Active", false); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return !active; - } -} diff --git a/src/main/java/frc/robot/commands/FieldOrientedDrive.java b/src/main/java/frc/robot/commands/FieldOrientedDrive.java index dfa5960..c57e901 100644 --- a/src/main/java/frc/robot/commands/FieldOrientedDrive.java +++ b/src/main/java/frc/robot/commands/FieldOrientedDrive.java @@ -67,43 +67,6 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { -<<<<<<< HEAD - // 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; - } - - 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); - - //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){ -======= xboxController.a().onTrue(Commands.runOnce(() -> {slowModeActive =! slowModeActive;})); SmartDashboard.putBoolean("Slow Mode Active", slowModeActive); @@ -122,7 +85,6 @@ public void execute() { // error tolerance of 2 degrees if (Math.abs(joystickTurnBearing-goalBearing) > Math.PI/180 * FieldOrientedDriveConstants.bearingTolerance){ ->>>>>>> c34d9ebf4581a2450fe63097371d669aeb73e40a goalBearing = -joystickTurnBearing; bearingPIDController.reset(); bearingPIDController.setSetpoint(goalBearing); @@ -135,21 +97,10 @@ public void execute() { } //converting to radians robotBearing = robotBearing / 180 * Math.PI; -<<<<<<< HEAD - //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; -======= - // SmartDashboard.putNumber("Robot bearing", robotBearing); - - //it looks cooked but that's because the controller is mapped kinda funny - joystickMoveBearing = Math.atan2(xboxController.getLeftX(), -xboxController.getLeftY()); - // SmartDashboard.putNumber("Drive: Left joystick bearing", joystickMoveBearing); - - //gyro measures angles anticlockwise. - joystickMoveBearing=joystickMoveBearing+robotBearing-2*Math.PI; ->>>>>>> c34d9ebf4581a2450fe63097371d669aeb73e40a joystickMoveMagnitude = Math.pow(Math.pow(xboxController.getLeftX(), 2) + Math.pow(xboxController.getLeftY(), 2), 0.5); @@ -174,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(); } @@ -201,12 +154,6 @@ else if (ySpeed < previousYSpeed - maximumAcceleration){ public void end(boolean interrupted) { driveSubsystem.drive(0.0, 0.0, 0.0, false); } -<<<<<<< HEAD - -======= - - // Returns true when the command should end. ->>>>>>> c34d9ebf4581a2450fe63097371d669aeb73e40a @Override public boolean isFinished() { return false; diff --git a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java index b8b1d52..97175ee 100644 --- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java +++ b/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);