From c7effba91dde108e495709ddccb31358dd43ba75 Mon Sep 17 00:00:00 2001 From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Sat, 24 Jan 2026 15:40:42 -0800 Subject: [PATCH] 9hu --- src/main/java/frc/robot/RobotContainer.java | 8 ++- .../frc/robot/commands/gpm/AimAtPose.java | 63 +++++++++++++++++++ .../robot/commands/gpm/TurretAutoShoot.java | 4 +- .../frc/robot/constants/FieldConstants.java | 2 +- .../frc/robot/constants/VisionConstants.java | 18 +++--- .../controls/PS5ControllerDriverConfig.java | 28 +++++---- .../frc/robot/subsystems/shooter/Shooter.java | 2 + .../subsystems/shooter/ShooterConstants.java | 6 +- .../frc/robot/subsystems/turret/Turret.java | 31 +++++++-- .../subsystems/turret/TurretConstants.java | 6 +- .../java/frc/robot/util/Vision/Vision.java | 6 ++ 11 files changed, 140 insertions(+), 34 deletions(-) create mode 100644 src/main/java/frc/robot/commands/gpm/AimAtPose.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0019740..b79bffb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,7 +9,9 @@ import org.littletonrobotics.junction.Logger; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.livewindow.LiveWindow; @@ -17,9 +19,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.commands.DoNothing; import frc.robot.commands.drive_comm.DefaultDriveCommand; +import frc.robot.commands.gpm.AimAtPose; import frc.robot.commands.vision.ShutdownAllPis; import frc.robot.constants.AutoConstants; import frc.robot.constants.Constants; +import frc.robot.constants.FieldConstants; import frc.robot.constants.VisionConstants; import frc.robot.controls.BaseDriverConfig; import frc.robot.controls.Operator; @@ -71,12 +75,13 @@ public class RobotContainer { case SwerveCompetition: case BetaBot: - vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS); + // fall-through case Vivace: case Phil: case Vertigo: + vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS); turret = new Turret(); shooter = new Shooter(); @@ -115,6 +120,7 @@ public class RobotContainer { LiveWindow.setEnabled(false); SmartDashboard.putData("Shutdown Orange Pis", new ShutdownAllPis()); + SmartDashboard.putData("Aim at thingy", new AimAtPose(drive, turret, new Pose2d(FieldConstants.field.getTagPose(26).get().getTranslation().toTranslation2d(), Rotation2d.kZero))); } /** diff --git a/src/main/java/frc/robot/commands/gpm/AimAtPose.java b/src/main/java/frc/robot/commands/gpm/AimAtPose.java new file mode 100644 index 0000000..6d58328 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/AimAtPose.java @@ -0,0 +1,63 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.FieldConstants; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.turret.Turret; + +/** + * Aims the robot at the closest April tag + */ +public class AimAtPose extends Command { + private Turret turret; + private Drivetrain drive; + private Pose2d target; + + /** + * Aims the robot at the closest April tag + * @param drive The drivetrain + */ + public AimAtPose(Drivetrain drive, Turret turret, Pose2d target){ + this.turret = turret; + this.drive = drive; + this.target = target; + addRequirements(turret); + } + + /** + * Gets the closest tag and sets the setpoint to aim at it + */ + @Override + public void initialize(){} + + @Override + public void execute() { + //Logger.recordOutput("TurretPose", new Pose2d(drive.getPose().getTranslation(), turret.getPosition())); + turret.setSetpoint(-target.getTranslation().minus(drive.getPose().getTranslation()).getAngle().getDegrees(), 0); + } + + /** + * Stops the drivetrain + * @param interrupted If the command is interrupted + */ + @Override + public void end(boolean interrupted) { + + } + + /** + * Returns if the command is finished + * @return If the PID is at the setpoint + */ + @Override + public boolean isFinished() { + // return turret.atSetPoint(); + return false; + } +} + diff --git a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java index 0fa34f0..29ae979 100644 --- a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java +++ b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java @@ -79,9 +79,9 @@ public class TurretAutoShoot extends Command { updateYawToTag(); if(turretVision != null && turretVisionEnabled && turret.atSetPoint()){ adjustWithTurretCam(); - turret.setSetpoint(adjustedSetpoint, drivetrain.getAngularRate(2)); + turret.setSetpoint(adjustedSetpoint, -drivetrain.getAngularRate(2)); } else{ - turret.setSetpoint(turretSetpoint, drivetrain.getAngularRate(2)); + turret.setSetpoint(turretSetpoint, -drivetrain.getAngularRate(2)); } } diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index ba1f454..2bdf678 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -18,7 +18,7 @@ public class FieldConstants { /** Location of hub target */ public static final Translation3d HUB_BLUE = - new Translation3d(Units.inchesToMeters(156.8), 4.035, Units.inchesToMeters(72)); + new Translation3d(Units.inchesToMeters(156.8 + 20), 4.035 + .67, Units.inchesToMeters(72)); public static Translation3d getHubTranslation() { if (Robot.getAlliance() == Alliance.Blue) { diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 6b1a80a..000c225 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -65,7 +65,7 @@ public class VisionConstants { /** * The maximum distance to the tag to use */ - public static final double MAX_DISTANCE = 2; + public static final double MAX_DISTANCE = 4; /** If vision should use manual calculations */ public static final boolean USE_MANUAL_CALCULATIONS = true; @@ -128,7 +128,7 @@ public class VisionConstants { *

* Only affects calculations using PhotonVision, not manual calculations. */ - public static final double HIGHEST_AMBIGUITY = 0.01; + public static final double HIGHEST_AMBIGUITY = 0.1; public static final int MAX_EMPTY_TICKS = 10; @@ -155,15 +155,15 @@ public class VisionConstants { new Pair( "CameraRight", new Transform3d( - new Translation3d(Units.inchesToMeters(0.75), Units.inchesToMeters(11.5), - Units.inchesToMeters(9.75)), - new Rotation3d(0, Units.degreesToRadians(27), Units.degreesToRadians(10)))), + new Translation3d(Units.inchesToMeters(-11.5), Units.inchesToMeters(-1), + Units.inchesToMeters(10.5)), + new Rotation3d(0, Units.degreesToRadians(-29), Units.degreesToRadians(190)))), new Pair( "CameraLeft", new Transform3d( - new Translation3d(Units.inchesToMeters(-13.5), Units.inchesToMeters(10.375), - Units.inchesToMeters(11.625)), - new Rotation3d(0, Units.degreesToRadians(27), 0))))); + new Translation3d(Units.inchesToMeters(-9.5), Units.inchesToMeters(13.5), + Units.inchesToMeters(13)), + new Rotation3d(0, Units.degreesToRadians(-29), Math.PI))))); /** * The transformations from the robot to object detection cameras @@ -174,7 +174,7 @@ public class VisionConstants { new Rotation3d(0, Units.degreesToRadians(20), 0)))); // used to cleanly shutdown the OrangePi - public static final String[] ORANGEPI_HOSTNAMES = {"photonfront.local", "photonback.local"}; + public static final String[] ORANGEPI_HOSTNAMES = {"photonvision.local"}; public static final String ORANGEPI_USERNAME = "pi"; public static final String ORANGEPI_PASSWORD = "raspberry"; } diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 7a06491..3d8eca0 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -65,17 +65,17 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { interrupted->getDrivetrain().setStateDeadband(true), ()->false, getDrivetrain()).withTimeout(2)); - 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.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( @@ -89,6 +89,12 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { }) ); + driver.get(PS5Button.LB).onTrue(new InstantCommand(() -> shooter.setFeeder(1))).onFalse( + new InstantCommand(()->{ + shooter.setFeeder(0); + }) + ); + // driver.get(PS5Button.CROSS).onTrue( // new InstantCommand(()->{ // if(turretJoyStickAim == null || !turretJoyStickAim.isScheduled()){ diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index c6051c4..fc8190d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -65,6 +65,8 @@ public class Shooter extends SubsystemBase { public void periodic(){ shooterMotorLeft.setControl(voltageRequest.withVelocity(shooterTargetSpeed)); shooterMotorRight.setControl(voltageRequest.withVelocity(shooterTargetSpeed)); + shooterMotorLeft.set(-1); + shooterMotorRight.set(-1); feederMotor.set(feederPower); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index c5fef88..188e1e4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -1,9 +1,11 @@ package frc.robot.subsystems.shooter; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + public class ShooterConstants { //TODO: find these values public static final double FEEDER_RUN_POWER = 0.3; // meters per second?? - public static final double SHOOTER_VELOCITY = 10.0; // meters per second + public static double SHOOTER_VELOCITY = 30.0; // meters per second public static final double SHOOTER_GEAR_RATIO = 36.0 / 24.0; // gear ratio from motors to shooter wheel // public static final double SHOOTER_LAUNCH_DIAMETER = 0.0762; // meters (3 inches) public static final double SHOOTER_LAUNCH_DIAMETER = 0.1016; // meters (4 inches) I think this is right. @@ -13,6 +15,8 @@ public class ShooterConstants { // in m/s public static final double EXIT_VELOCITY_TOLERANCE = 1.0; + + } // 8 velcocity is too little // 16 is too much \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 3d6e67a..fd054b2 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; +import frc.robot.subsystems.shooter.ShooterConstants; public class Turret extends SubsystemBase implements TurretIO{ final private TalonFX motor; @@ -47,6 +48,11 @@ public class Turret extends SubsystemBase implements TurretIO{ private TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); + private double dV = 2.0; + private double kP = 10.0; + private double kI = 0.0; + private double kD = 0.0; + public Turret() { motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.RIO_CAN); // switch of course @@ -74,15 +80,15 @@ public class Turret extends SubsystemBase implements TurretIO{ config.Slot0.kV = 0.0; // Velocity gain: 1 rps -> 0.12V config.Slot0.kA = 0; // Acceleration gain: 1 rps² -> 0V (should be tuned if acceleration matters) - config.Slot0.kP = 10.0; // If position error is 1 rotation, apply 10V - config.Slot0.kI = 0.0; // Integral term (usually left at 0 for MotionMagic) - config.Slot0.kD = 0.0; // Derivative term (used to dampen oscillations) + config.Slot0.kP = kP; // If position error is 1 rotation, apply 10V + config.Slot0.kI = kI; // Integral term (usually left at 0 for MotionMagic) + config.Slot0.kD = kD; // Derivative term (used to dampen oscillations) MotionMagicConfigs motionMagicConfigs = config.MotionMagic; motionMagicConfigs.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY / TurretConstants.TURRET_RADIUS) * gearRatio; // max velocity * gear ratio motionMagicConfigs.MotionMagicAcceleration = Units.radiansToRotations(TurretConstants.MAX_ACCELERATION / TurretConstants.TURRET_RADIUS) * gearRatio; // max Acceleration * gear ratio - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; motor.getConfigurator().apply(config); // config.ClosedLoopGeneral.ContinuousWrap = true; @@ -104,6 +110,7 @@ public class Turret extends SubsystemBase implements TurretIO{ SmartDashboard.putData("Set to -180 degrees", new InstantCommand(() -> setSetpoint(-180, 0))); SmartDashboard.putData("Set to -90 degrees", new InstantCommand(() -> setSetpoint(-90, 0))); SmartDashboard.putData("Reset Yaw", new InstantCommand(() -> resetYaw())); + SmartDashboard.putNumber("Shooter Velocity", ShooterConstants.SHOOTER_VELOCITY); } public double getPosition() { @@ -140,7 +147,6 @@ public class Turret extends SubsystemBase implements TurretIO{ double motorTargetRotations = (setpoint / 360.0) * gearRatio; //Tune this with rotating robot - double dV = TurretConstants.ROTATIONAL_VELOCITY_CONSTANT; motor.setControl(voltageRequest.withPosition(motorTargetRotations).withFeedForward(dV * robotRotVel)); } } @@ -150,7 +156,20 @@ public class Turret extends SubsystemBase implements TurretIO{ updateInputs(); ligament2d.setAngle(getPosition()); - SmartDashboard.putNumber("Turret Position Degrees", getPosition()); + dV = SmartDashboard.getNumber("FF Constant", dV); + kP = SmartDashboard.getNumber("kP Value", kP); + kI = SmartDashboard.getNumber("kI Value", kI); + kD = SmartDashboard.getNumber("kD Value", kD); + + SmartDashboard.putNumber("Turret Position Degrees", getPosition()); + SmartDashboard.putNumber("FF Constant", dV); + SmartDashboard.putNumber("kP Value", kP); + SmartDashboard.putNumber("kI Value", kI); + SmartDashboard.putNumber("kD Value", kD); + + + ShooterConstants.SHOOTER_VELOCITY = SmartDashboard.getNumber("Shooter Velocity", ShooterConstants.SHOOTER_VELOCITY); + } @Override diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index a7ef3c6..18de26b 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -6,11 +6,11 @@ public class TurretConstants { public static double MAX_ANGLE = 180; public static double MIN_ANGLE = -180; - public static double MAX_VELOCITY = 2.0; // m/s - public static double MAX_ACCELERATION = 10; // m/s^2 + public static double MAX_VELOCITY = 10000000; // m/s + public static double MAX_ACCELERATION = 10000000; // m/s^2 public static double TURRET_WIDTH = Units.feetToMeters(1.0); public static double TURRET_RADIUS = TURRET_WIDTH / 2; - public static double ROTATIONAL_VELOCITY_CONSTANT = 0; + public static double ROTATIONAL_VELOCITY_CONSTANT = 0.2; } diff --git a/src/main/java/frc/robot/util/Vision/Vision.java b/src/main/java/frc/robot/util/Vision/Vision.java index 08bdab2..d92b3de 100644 --- a/src/main/java/frc/robot/util/Vision/Vision.java +++ b/src/main/java/frc/robot/util/Vision/Vision.java @@ -95,6 +95,12 @@ public class Vision { } } } + + Pose3d[] tags = new Pose3d[FieldConstants.field.getTags().size()]; + for (int i = 0; i < FieldConstants.field.getTags().size(); i++) { + tags[i] = (FieldConstants.field.getTagPose(i+1).get()); + } + Logger.recordOutput("AprilTags", tags); } -- 2.39.5