From: mixxlto Date: Tue, 10 Feb 2026 00:55:09 +0000 (-0800) Subject: a X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=802333925e0f18f1dd708d0f861a6b82a5b71931;p=FRC2026.git a --- diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 06d2f12..d6400f2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -84,7 +84,7 @@ public class RobotContainer { climb = new Climb(); spindexer = new Spindexer(); - case WaffleHouse: + case WaffleHouse: // AKA Betabot turret = new Turret(); shooter = new Shooter(); hood = new Hood(); @@ -101,7 +101,7 @@ public class RobotContainer { case Vertigo: // AKA "French Toast" drive = new Drivetrain(vision, new GyroIOPigeon2()); - driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood); + driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer); operator = new Operator(drive); // Detected objects need access to the drivetrain diff --git a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java deleted file mode 100644 index 6f92ef6..0000000 --- a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java +++ /dev/null @@ -1,124 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.LinearFilter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.Constants; -import frc.robot.constants.FieldConstants; -import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.subsystems.turret.ShotInterpolation; -import frc.robot.subsystems.turret.Turret; -import frc.robot.subsystems.turret.TurretConstants; -// import frc.robot.util.FieldZone; - -public class TurretAutoShoot extends Command { - private Turret turret; - private Drivetrain drivetrain; - - private double turretSetpoint; - - private Pose2d drivepose; - - private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); - private Rotation2d lastTurretAngle; - private Rotation2d turretAngle; - private double turretVelocity; - private final double phaseDelay = 0.03; - - public TurretAutoShoot(Turret turret, Drivetrain drivetrain) { - this.turret = turret; - this.drivetrain = drivetrain; - drivepose = drivetrain.getPose(); - - addRequirements(turret); - } - - public void updateTurretSetpoint(Pose2d drivepose) { - - Translation2d target = FieldConstants.getHubTranslation().toTranslation2d(); - Pose2d turretPosition = drivepose.transformBy(new Transform2d((new Translation2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX(),TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY())), new Rotation2d())); - double turretToTargetDistance = target.getDistance(turretPosition.getTranslation()); - - // If the robot is moving, adjust the target position based on velocity - ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); - ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw()); - - double turretVelocityX = - fieldRelVel.vxMetersPerSecond - + fieldRelVel.omegaRadiansPerSecond - * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.cos(drivepose.getRotation().getRadians()) - - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.sin(drivepose.getRotation().getRadians())); - double turretVelocityY = - fieldRelVel.vyMetersPerSecond - + fieldRelVel.omegaRadiansPerSecond - * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.cos(drivepose.getRotation().getRadians()) - - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.sin(drivepose.getRotation().getRadians())); - - // Account for imparted velocity by robot (turret) to offset - double timeOfFlight; - Pose2d lookaheadPose = turretPosition; - double lookaheadTurretToTargetDistance = turretToTargetDistance; - - // Loop (20) until lookahreadTurretToTargetDistance converges - for (int i = 0; i < 20; i++) { - timeOfFlight = ShotInterpolation.timeOfFlightMap.get(lookaheadTurretToTargetDistance); - double offsetX = turretVelocityX * timeOfFlight; - double offsetY = turretVelocityY * timeOfFlight; - lookaheadPose = - new Pose2d( - turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)), - turretPosition.getRotation()); - lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); - } - turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle(); - if (lastTurretAngle == null) { - lastTurretAngle = turretAngle; - } - turretVelocity = - turretAngleFilter.calculate( - turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME); - lastTurretAngle = turretAngle; - - // Add 180 since drivetrain is backwards - double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() + Math.PI); - turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint), -180.0, 180.0); - } - - public void updateDrivePose(){ - ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); - drivepose = drivetrain.getPose().exp( - new Twist2d( - robotRelVel.vxMetersPerSecond * phaseDelay, - robotRelVel.vyMetersPerSecond * phaseDelay, - robotRelVel.omegaRadiansPerSecond * phaseDelay)); - } - - @Override - public void initialize() { - updateDrivePose(); - updateTurretSetpoint(drivepose); - turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2)); - } - - @Override - public void execute() { - updateDrivePose(); - updateTurretSetpoint(drivepose); - turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2)); - } - - @Override - public void end(boolean interrupted) { - // Set the turret to a safe position when the command ends - turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0); - } - -} - diff --git a/src/main/java/frc/robot/commands/gpm/TurretJoyStickAim.java b/src/main/java/frc/robot/commands/gpm/TurretJoyStickAim.java deleted file mode 100644 index 06e42cb..0000000 --- a/src/main/java/frc/robot/commands/gpm/TurretJoyStickAim.java +++ /dev/null @@ -1,27 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.controls.PS5ControllerDriverConfig; -import frc.robot.subsystems.turret.Turret; - -public class TurretJoyStickAim extends Command{ - private Turret turret; - private PS5ControllerDriverConfig driver; - - public TurretJoyStickAim(Turret turret, PS5ControllerDriverConfig driver){ - this.turret = turret; - this.driver = driver; - } - - Rotation2d rotation2d = new Rotation2d(driver.getRawSideTranslation(), driver.getRawForwardTranslation()); - double angle = Units.radiansToDegrees(MathUtil.angleModulus(rotation2d.getDegrees())); - - @Override - public void execute() { - //turret.setSetpoint(angle, 0); - } - -} diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 7ae55d1..2635199 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -4,28 +4,19 @@ import java.util.function.BooleanSupplier; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Robot; -import frc.robot.commands.gpm.SimpleAutoShoot; -import frc.robot.commands.gpm.TurretAutoShoot; -import frc.robot.commands.gpm.TurretJoyStickAim; +import frc.robot.commands.gpm.AutoShootCommand; import frc.robot.constants.Constants; -import frc.robot.constants.FieldConstants; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.spindexer.Spindexer; import frc.robot.subsystems.turret.Turret; import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.hood.HoodConstants; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeConstants; import lib.controllers.PS5Controller; @@ -46,16 +37,17 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { private Pose2d alignmentPose = null; private Command turretAutoShoot; - private Command simpleTurretAutoShoot; - private TurretJoyStickAim turretJoyStickAim; + private Command autoShoot; private boolean intakeBoolean = true; - public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret, Hood hood) { + public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret, Hood hood, Intake intake, Spindexer spindexer) { super(drive); this.shooter = shooter; this.turret = turret; this.hood = hood; + this.intake = intake; + this.spindexer = spindexer; } public void configureControls() { @@ -93,73 +85,17 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { })); } - // driver.get(PS5Button.LB).onTrue( - // new SequentialCommandGroup( - // new InstantCommand(()-> shooter.setShooter(-ShooterConstants.SHOOTER_VELOCITY)), - // new WaitCommand(0.8), - // new InstantCommand(()-> shooter.setFeeder(ShooterConstants.FEEDER_RUN_POWER)) - // ) - // ).onFalse( - // new InstantCommand(() -> { - // shooter.setFeeder(0); - // shooter.setShooter(0); - // })); - //driver.get(PS5Button.TRIANGLE).onTrue(new InstantCommand(() -> shooter.setShooter(ShooterConstants.SHOOTER_VELOCITY))).onFalse(new InstantCommand(() -> shooter.setShooter(0))); - - // driver.get(PS5Button.SQUARE).onTrue( - // new InstantCommand(()->{ - // if (simpleTurretAutoShoot != null && simpleTurretAutoShoot.isScheduled()){ - // simpleTurretAutoShoot.cancel(); - // } else{ - // simpleTurretAutoShoot = new SimpleAutoShoot(turret, getDrivetrain(), shooter); - // CommandScheduler.getInstance().schedule(simpleTurretAutoShoot); - // } - // }) - // ); - - - driver.get(PS5Button.SQUARE).onTrue(new InstantCommand(() -> hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0))); - driver.get(PS5Button.TRIANGLE).onTrue(new InstantCommand(() -> hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MIN_ANGLE)), 0))); - driver.get(PS5Button.LB).onTrue(new InstantCommand(() -> hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)), 0))); - - - // driver.get(PS5Button.CIRCLE).onTrue( - // new InstantCommand(()->{ - // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(180)), 0); - // }) - // ).onFalse( - // new InstantCommand(()->{ - // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(-170)), 0); - // }) - // ); - - // driver.get(PS5Button.CROSS).onTrue( - // new InstantCommand(()->{ - // if(turretJoyStickAim == null || !turretJoyStickAim.isScheduled()){ - // turretJoyStickAim = new TurretJoyStickAim(turret, this); - // turretJoyStickAim.schedule(); - // } - // }) - // ).onFalse( - // new InstantCommand(()->{ - // if(turretJoyStickAim.isScheduled()){ - // turretJoyStickAim.cancel(); - // } - // }) - // ); - - } - - public void setAlignmentPose(){ - Translation2d drivepose = getDrivetrain().getPose().getTranslation(); - // uses tag #?? - int tagNumber = 17; - Translation2d tagpose = FieldConstants.field.getTagPose(tagNumber).get().toPose2d().getTranslation(); - double YDifference = tagpose.getY() - drivepose.getY(); - double XDifference = tagpose.getX() - drivepose.getX(); - double angle = Math.atan(YDifference/XDifference); - alignmentPose = new Pose2d(drivepose.getX(), drivepose.getY(), new Rotation2d(angle)); - System.out.println("Alignment angle: " + Units.radiansToDegrees(angle)); + driver.get(PS5Button.SQUARE).onTrue( + new InstantCommand(()->{ + if (autoShoot != null && autoShoot.isScheduled()){ + autoShoot.cancel(); + } else{ + autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer); + CommandScheduler.getInstance().schedule(autoShoot); + } + }) + ); + } @Override