diff --git a/2024-Robot/src/main/java/frc/robot/commands/AutoParser.java b/2024-Robot/src/main/java/frc/robot/commands/AutoParser.java index 173b7b44..ff95f681 100644 --- a/2024-Robot/src/main/java/frc/robot/commands/AutoParser.java +++ b/2024-Robot/src/main/java/frc/robot/commands/AutoParser.java @@ -128,7 +128,7 @@ public AutoParser(Drive drive, Intake intake, Feeder feeder, Shooter shooter, Pe } } ArrayListParallelDeadlineGroup parallelDeadlineGroup = new ArrayListParallelDeadlineGroup( - new AutonomousFollower(drive, this.sampledPoints, previousHaltingTime, haltingTime, false), + new AutonomousFollower(drive, this.sampledPoints, previousHaltingTime, haltingTime, false, false), parallelCommands ); this.autoCommands.add(parallelDeadlineGroup); @@ -154,7 +154,7 @@ public AutoParser(Drive drive, Intake intake, Feeder feeder, Shooter shooter, Pe previousHaltingTime = haltingTime; } this.autoCommands.add( - new AutonomousFollower(drive, this.sampledPoints, previousHaltingTime, false) + new AutonomousFollower(drive, this.sampledPoints, previousHaltingTime, false, false) ); addCommands(this.autoCommands); diff --git a/2024-Robot/src/main/java/frc/robot/commands/AutonomousFollower.java b/2024-Robot/src/main/java/frc/robot/commands/AutonomousFollower.java index 5860c445..e35f3da7 100644 --- a/2024-Robot/src/main/java/frc/robot/commands/AutonomousFollower.java +++ b/2024-Robot/src/main/java/frc/robot/commands/AutonomousFollower.java @@ -31,26 +31,29 @@ public class AutonomousFollower extends Command { private double desiredThetaChange = 0; private boolean record; + private boolean pickupNote; private ArrayList recordedOdometry = new ArrayList(); private double pathStartTime; private double pathEndTime; - public AutonomousFollower(Drive drive, JSONArray pathPoints, double pathStartTime, double pathEndTime, boolean record) { + public AutonomousFollower(Drive drive, JSONArray pathPoints, double pathStartTime, double pathEndTime, boolean record, boolean pickupNote) { this.drive = drive; this.path = pathPoints; this.record = record; this.pathStartTime = pathStartTime; this.pathEndTime = pathEndTime; + this.pickupNote = pickupNote; addRequirements(drive); } - public AutonomousFollower(Drive drive, JSONArray pathPoints, double pathStartTime, boolean record) { + public AutonomousFollower(Drive drive, JSONArray pathPoints, double pathStartTime, boolean record, boolean pickupNote) { this.drive = drive; this.path = pathPoints; this.record = record; this.pathStartTime = pathStartTime; this.pathEndTime = path.getJSONArray(path.length() - 1).getDouble(0); + this.pickupNote = pickupNote; addRequirements(drive); } @@ -73,7 +76,7 @@ public void execute() { currentTime = Timer.getFPGATimestamp() - initTime + pathStartTime; // call PIDController function - desiredVelocityArray = drive.pidController(odometryFusedX, odometryFusedY, odometryFusedTheta, currentTime, path); + desiredVelocityArray = drive.pidController(odometryFusedX, odometryFusedY, odometryFusedTheta, currentTime, path, pickupNote); // create velocity vector and set desired theta change Vector velocityVector = new Vector(); diff --git a/2024-Robot/src/main/java/frc/robot/commands/ShootWhilePathingAndIntaking.java b/2024-Robot/src/main/java/frc/robot/commands/ShootWhilePathingAndIntaking.java index c2f518c9..46248589 100644 --- a/2024-Robot/src/main/java/frc/robot/commands/ShootWhilePathingAndIntaking.java +++ b/2024-Robot/src/main/java/frc/robot/commands/ShootWhilePathingAndIntaking.java @@ -67,8 +67,9 @@ public class ShootWhilePathingAndIntaking extends Command { private double intakeRPM; private boolean haveIntakeNote; + private boolean pickupNote; - public ShootWhilePathingAndIntaking(Drive drive, Intake intake, Feeder feeder, Shooter shooter, Peripherals peripherals, TOF tof, JSONArray pathPoints, double intakeRPM, double feederRPM, double shootDelay) { + public ShootWhilePathingAndIntaking(Drive drive, Intake intake, Feeder feeder, Shooter shooter, Peripherals peripherals, TOF tof, JSONArray pathPoints, double intakeRPM, double feederRPM, double shootDelay, boolean pickupNote) { this.drive = drive; this.intake = intake; this.feeder = feeder; @@ -79,6 +80,7 @@ public ShootWhilePathingAndIntaking(Drive drive, Intake intake, Feeder feeder, S this.intakeRPM = intakeRPM; this.feederRPM = feederRPM; this.shootDelay = shootDelay; + this.pickupNote = pickupNote; addRequirements(this.drive, this.feeder, this.shooter); } @@ -174,7 +176,7 @@ public void execute() { this.odometryY = this.drive.getFusedOdometryY(); this.odometryTheta = this.drive.getFusedOdometryTheta(); - double[] desiredVelocityArray = this.drive.pidController(this.odometryX, this.odometryY, this.odometryTheta, this.currentTime, this.path); + double[] desiredVelocityArray = this.drive.pidController(this.odometryX, this.odometryY, this.odometryTheta, this.currentTime, this.path, this.pickupNote); Vector velocityVector = new Vector(); velocityVector.setI(desiredVelocityArray[0]); velocityVector.setJ(desiredVelocityArray[1]); diff --git a/2024-Robot/src/main/java/frc/robot/commands/autos/FivePieceAuto.java b/2024-Robot/src/main/java/frc/robot/commands/autos/FivePieceAuto.java index 9f928786..d79bd99c 100644 --- a/2024-Robot/src/main/java/frc/robot/commands/autos/FivePieceAuto.java +++ b/2024-Robot/src/main/java/frc/robot/commands/autos/FivePieceAuto.java @@ -114,7 +114,7 @@ public FivePieceAuto(Drive drive, Peripherals peripherals, Intake intake, Feeder new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600, 3), new ParallelCommandGroup( new SequentialCommandGroup( - new AutonomousFollower(drive, pathJSON, 0, false), + new AutonomousFollower(drive, pathJSON, 0, false, false), new TurnToTarget(drive, peripherals) ), new AutoPrepForShot(shooter, tof, 20, 3000) @@ -125,7 +125,7 @@ public FivePieceAuto(Drive drive, Peripherals peripherals, Intake intake, Feeder new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600, 3), new ParallelCommandGroup( new SequentialCommandGroup( - new AutonomousFollower(drive, pathJSON2, 0, false), + new AutonomousFollower(drive, pathJSON2, 0, false, false), new TurnToTarget(drive, peripherals) ), new AutoPrepForShot(shooter, tof, 20, 3000) @@ -136,7 +136,7 @@ public FivePieceAuto(Drive drive, Peripherals peripherals, Intake intake, Feeder new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600, 3), new ParallelCommandGroup( new SequentialCommandGroup( - new AutonomousFollower(drive, pathJSON3, 0, false), + new AutonomousFollower(drive, pathJSON3, 0, false, false), new TurnToTarget(drive, peripherals) ), new AutoPrepForShot(shooter, tof, 20, 3000) @@ -144,7 +144,7 @@ public FivePieceAuto(Drive drive, Peripherals peripherals, Intake intake, Feeder ), new AutoShoot(drive, shooter, feeder, peripherals, lights, tof, 1200, 1), new ParallelDeadlineGroup( - new AutonomousFollower(drive, pathJSON4, 0, false), + new AutonomousFollower(drive, pathJSON4, 0, false, false), new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600), new AutoPrepForShot(shooter, tof, 20, 3000) ), diff --git a/2024-Robot/src/main/java/frc/robot/commands/autos/FourPieceCloseAuto.java b/2024-Robot/src/main/java/frc/robot/commands/autos/FourPieceCloseAuto.java index 5911061f..89ae51cd 100644 --- a/2024-Robot/src/main/java/frc/robot/commands/autos/FourPieceCloseAuto.java +++ b/2024-Robot/src/main/java/frc/robot/commands/autos/FourPieceCloseAuto.java @@ -99,7 +99,7 @@ public FourPieceCloseAuto(Drive drive, Peripherals peripherals, Intake intake, F new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600, 3), new ParallelCommandGroup( new SequentialCommandGroup( - new AutonomousFollower(drive, pathJSON, 0, false), + new AutonomousFollower(drive, pathJSON, 0, false, false), new TurnToTarget(drive, peripherals) ), new AutoPrepForShot(shooter, tof, 20, 3000) @@ -110,7 +110,7 @@ public FourPieceCloseAuto(Drive drive, Peripherals peripherals, Intake intake, F new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600, 3), new ParallelCommandGroup( new SequentialCommandGroup( - new AutonomousFollower(drive, pathJSON2, 0, false), + new AutonomousFollower(drive, pathJSON2, 0, false, false), new TurnToTarget(drive, peripherals) ), new AutoPrepForShot(shooter, tof, 20, 3000) @@ -121,7 +121,7 @@ public FourPieceCloseAuto(Drive drive, Peripherals peripherals, Intake intake, F new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600, 3), new ParallelCommandGroup( new SequentialCommandGroup( - new AutonomousFollower(drive, pathJSON3, 0, false), + new AutonomousFollower(drive, pathJSON3, 0, false, false), new TurnToTarget(drive, peripherals) ), new AutoPrepForShot(shooter, tof, 20, 3000) diff --git a/2024-Robot/src/main/java/frc/robot/commands/autos/FourPieceFarBottomAuto.java b/2024-Robot/src/main/java/frc/robot/commands/autos/FourPieceFarBottomAuto.java index c2a4efe8..37c07a91 100644 --- a/2024-Robot/src/main/java/frc/robot/commands/autos/FourPieceFarBottomAuto.java +++ b/2024-Robot/src/main/java/frc/robot/commands/autos/FourPieceFarBottomAuto.java @@ -96,19 +96,19 @@ public FourPieceFarBottomAuto(Drive drive, Peripherals peripherals, Intake intak new RunIntake(intake, Constants.SetPoints.IntakePosition.kDOWN, 1200) ), new ParallelDeadlineGroup( - new AutonomousFollower(drive, pathJSON, 0, false), + new AutonomousFollower(drive, pathJSON, 0, false, false), new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600), new AutoPrepForShot(shooter, tof, 20, 3000) ), new AutoShoot(drive, shooter, feeder, peripherals, lights, tof, 1200, 2), new ParallelDeadlineGroup( - new AutonomousFollower(drive, pathJSON2, 0, false), + new AutonomousFollower(drive, pathJSON2, 0, false, false), new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600), new AutoPrepForShot(shooter, tof, 20, 3000) ), new AutoShoot(drive, shooter, feeder, peripherals, lights, tof, 1200, 2), new ParallelDeadlineGroup( - new AutonomousFollower(drive, pathJSON3, 0, false), + new AutonomousFollower(drive, pathJSON3, 0, false, false), new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600), new AutoPrepForShot(shooter, tof, 20, 3000) ), diff --git a/2024-Robot/src/main/java/frc/robot/subsystems/Drive.java b/2024-Robot/src/main/java/frc/robot/subsystems/Drive.java index bd0867c5..dba1930c 100644 --- a/2024-Robot/src/main/java/frc/robot/subsystems/Drive.java +++ b/2024-Robot/src/main/java/frc/robot/subsystems/Drive.java @@ -916,7 +916,7 @@ public JSONArray getPathPoint(JSONArray path, double time){ } // Autonomous algorithm - public double[] pidController(double currentX, double currentY, double currentTheta, double time, JSONArray pathPoints) { + public double[] pidController(double currentX, double currentY, double currentTheta, double time, JSONArray pathPoints, boolean pickupNote) { if(time < pathPoints.getJSONArray(pathPoints.length() - 1).getDouble(0)) { JSONArray currentPoint = pathPoints.getJSONArray(0); JSONArray targetPoint = pathPoints.getJSONArray(0); @@ -965,6 +965,22 @@ public double[] pidController(double currentX, double currentY, double currentTh currentPointTheta = Math.PI - currentPointTheta; } + if (pickupNote){ + double angleToNote = (peripherals.getBackCamTargetTx()); + double differenceX = Math.abs(targetX - currentPointX); + double differenceY = Math.abs(targetY - currentPointY); + double r = (Math.sqrt((differenceX * differenceX) + (differenceY * differenceY))); + double adjustedX = r * (Math.cos((targetTheta + Math.PI) - angleToNote)); + double adjustedY = r * (Math.sin((targetTheta + Math.PI) - angleToNote)); + targetX = targetX + adjustedX; + targetY = targetY + (3 * adjustedY); + System.out.println("note: " + angleToNote); + System.out.println("x: " + targetX); + System.out.println("y: " + targetY); + targetTheta = targetTheta - angleToNote; + System.out.println("theta: " + targetTheta); + } + double feedForwardX = (targetX - currentPointX)/(targetTime - currentPointTime); double feedForwardY = (targetY - currentPointY)/(targetTime - currentPointTime); // double feedForwardTheta = -(targetTheta - currentPointTheta)/(targetTime - currentPointTime); diff --git a/2024-Robot/src/main/java/frc/robot/subsystems/Peripherals.java b/2024-Robot/src/main/java/frc/robot/subsystems/Peripherals.java index b7a939d4..ae80bd5c 100644 --- a/2024-Robot/src/main/java/frc/robot/subsystems/Peripherals.java +++ b/2024-Robot/src/main/java/frc/robot/subsystems/Peripherals.java @@ -7,16 +7,10 @@ import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; -import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.networktables.ConnectionInfo; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.PowerDistribution; -import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; -import edu.wpi.first.wpilibj.SPI.Port; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.commands.defaults.PeripheralsDefault; @@ -25,6 +19,7 @@ public class Peripherals extends SubsystemBase { private NetworkTable backCam = NetworkTableInstance.getDefault().getTable("limelight-back"); private NetworkTableEntry backCamJSON = backCam.getEntry("json"); + private NetworkTableEntry backCamTx = backCam.getEntry("tx"); private NetworkTable frontCam = NetworkTableInstance.getDefault().getTable("limelight-front"); private NetworkTableEntry frontCamJSON = frontCam.getEntry("json"); private NetworkTableEntry frontCamTy = frontCam.getEntry("ty"); @@ -114,6 +109,10 @@ public JSONObject getFrontCamLatencies(){ return latencies; } + public double getBackCamTargetTx(){ + return backCamTx.getDouble(0.0) * Math.PI / 180; + } + public void setFrontCamPipeline(int pipeline){ frontCam.getEntry("pipeline").setNumber(pipeline); }