From 8fc4c6f2c043bb06507966d98ddc0780d2cf7444 Mon Sep 17 00:00:00 2001 From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Sat, 31 Jan 2026 14:35:23 -0800 Subject: [PATCH] a --- .../robot/commands/gpm/SimpleAutoShoot.java | 131 +++++++++++++++++- .../controls/PS5ControllerDriverConfig.java | 10 +- .../frc/robot/subsystems/shooter/Shooter.java | 4 +- .../frc/robot/subsystems/turret/Turret.java | 28 ++-- 4 files changed, 154 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java index 649a86a..f2e5494 100644 --- a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java +++ b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java @@ -1,7 +1,136 @@ package frc.robot.commands.gpm; +import java.lang.reflect.Field; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Robot; +import frc.robot.constants.FieldConstants; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.turret.Turret; +import frc.robot.util.FieldZone; +import frc.robot.util.ShootingTarget; +import frc.robot.util.Vision.TurretVision; public class SimpleAutoShoot extends Command { - + private Turret turret; + private Drivetrain drivetrain; + private TurretVision turretVision; + + private double fieldAngleRad; + private double turretSetpoint; + private double adjustedSetpoint; + private double yawToTagCamera; + private double yawToTag; + + private boolean turretVisionEnabled = false; + private boolean SOTM = true; + private Translation2d drivepose; + public SimpleAutoShoot(Turret turret, Drivetrain drivetrain) { + this.turret = turret; + this.drivetrain = drivetrain; + drivepose = drivetrain.getPose().getTranslation(); + + addRequirements(turret); + } + + public void updateTurretSetpoint(Translation2d drivepose) { + + //FieldZone currentZone = getZone(drivepose); + Translation2d target = FieldConstants.getHubTranslation().toTranslation2d(); + + double D_y; + double D_x; + // TODO: Change time to goal on actual comp bot + double timeToGoal = 1.0; + + // If the robot is moving, adjust the target position based on velocity + if (SOTM) { + ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); + ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw()); + double xVel = fieldRelVel.vxMetersPerSecond; + double yVel = fieldRelVel.vyMetersPerSecond; + + D_y = target.getY() - drivepose.getY() - timeToGoal * yVel; + D_x = target.getX() - drivepose.getX() - timeToGoal * xVel; + } else { + D_y = target.getY() - drivepose.getY(); + D_x = target.getX() - drivepose.getX(); + } + + // Calculate the field-relative angle + fieldAngleRad = Math.atan2(D_y, D_x); + + // Calculate robot heading and adjust for reverse drive + double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians() + Math.PI)); // Reverse drive adjustment + + // Calculate turret setpoint (angle relative to robot heading) + turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(fieldAngleRad - robotHeading), -180.0, 180.0); + + // System.out.println("Aligning the turn to degree angle: " + turretSetpoint); + } + + public void adjustWithTurretCam() { + if(turretVision.canSeeTag(26) || turretVision.canSeeTag(10)){ + // Adjust turret setpoint based on vision input + if(Robot.getAlliance() == Alliance.Blue){ + yawToTagCamera = -1 * Units.radiansToDegrees(turretVision.getYawToTagRad(26).get()); + } + else{ + yawToTagCamera = -1 * Units.radiansToDegrees(turretVision.getYawToTagRad(10).get()); + } + double error = yawToTagCamera - yawToTag; + adjustedSetpoint = turretSetpoint + error; + } + } + + public void updateYawToTag(){ + // Calculate the yaw offset to the tag + double D_y = FieldConstants.getHubTranslation().getY() - drivetrain.getPose().getY(); + double D_x = FieldConstants.getHubTranslation().getX() - drivetrain.getPose().getX(); + double angleToTag = Units.radiansToDegrees(Math.atan(D_y / D_x)); + yawToTag = angleToTag - Units.radiansToDegrees(fieldAngleRad); + } + + @Override + public void initialize() { + // Initialize setpoint calculation and set the initial goal for the turret + updateTurretSetpoint(drivepose); + // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), drivetrain.getAngularRate(2)); + turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), 0); + } + + @Override + public void execute() { + // Continuously update setpoints and adjust based on vision if available + drivepose = drivetrain.getPose().getTranslation(); + updateTurretSetpoint(drivepose); + updateYawToTag(); + + turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2) * 1.0); + } + + @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); + } + + public boolean leftSide(Translation2d drivepose) { + if (drivepose.getY() > (FieldConstants.FIELD_WIDTH / 2)) { + return true; + } else { + return false; + } + } + + public FieldZone getZone(Translation2d drivepose) { + return FieldConstants.getZone(drivepose); + } } + diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index cf47b34..5fe646b 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -14,6 +14,7 @@ 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.constants.Constants; @@ -37,6 +38,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { private Pose2d alignmentPose = null; private Command turretAutoShoot; + private Command simpleTurretAutoShoot; private TurretJoyStickAim turretJoyStickAim; public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret) { @@ -80,11 +82,11 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { driver.get(PS5Button.SQUARE).onTrue( new InstantCommand(()->{ - if (turretAutoShoot != null && turretAutoShoot.isScheduled()){ - turretAutoShoot.cancel(); + if (simpleTurretAutoShoot != null && simpleTurretAutoShoot.isScheduled()){ + simpleTurretAutoShoot.cancel(); } else{ - turretAutoShoot = new TurretAutoShoot(turret, getDrivetrain()); - CommandScheduler.getInstance().schedule(turretAutoShoot); + simpleTurretAutoShoot = new SimpleAutoShoot(turret, getDrivetrain()); + CommandScheduler.getInstance().schedule(simpleTurretAutoShoot); } }) ); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index cb8dd6c..2247f83 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -70,8 +70,8 @@ public class Shooter extends SubsystemBase implements ShooterIO { shooterMotorLeft.setControl(voltageRequest.withVelocity(shooterTargetSpeed)); shooterMotorRight.setControl(voltageRequest.withVelocity(shooterTargetSpeed)); - // shooterMotorLeft.set(-1); - // shooterMotorRight.set(-1); + shooterMotorLeft.set(-1); + shooterMotorRight.set(-1); feederMotor.set(feederPower); } diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 660cc09..499b1c6 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -45,7 +45,9 @@ public class Turret extends SubsystemBase { private static final double TURRET_RATIO = 140.0 / 10.0; private static final double GEAR_RATIO = VERSA_RATIO * TURRET_RATIO; - private static final PIDController velocityPid = new PIDController(15, 0, 0.2); + private static final PIDController velocityPid = new PIDController(15, 0, 0.25); + + private double lastFrameVelocity = 0; /* ---------------- Hardware ---------------- */ @@ -81,7 +83,9 @@ public class Turret extends SubsystemBase { private final MechanismRoot2d root = mech.getRoot("turret", 50, 50); private final MechanismLigament2d ligament = root.append(new MechanismLigament2d("barrel", 30, 0)); - private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0); + // private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0.010); + private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0); + /* ---------------- Constructor ---------------- */ @@ -95,12 +99,12 @@ public class Turret extends SubsystemBase { // .withKV(1) // .withKA(1)); - // motor.getConfigurator().apply( - // new com.ctre.phoenix6.configs.TalonFXConfiguration() { - // { - // MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - // } - // }); + motor.getConfigurator().apply( + new com.ctre.phoenix6.configs.TalonFXConfiguration() { + { + MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + } + }); // motor.getConfigurator().apply( // new TalonFXConfiguration() { @@ -155,8 +159,6 @@ public class Turret extends SubsystemBase { @Override public void periodic() { - - double robotRelativeGoal = goalAngle.getRadians(); // --- MA-style continuous wrap selection --- @@ -196,9 +198,11 @@ public class Turret extends SubsystemBase { motor.getPosition().getValue().in(edu.wpi.first.units.Units.Radians), setpoint.position * GEAR_RATIO); - targetVelocity += Units.radiansToRotations(motorVelRotPerSec); + targetVelocity += Units.rotationsToRadians(motorVelRotPerSec); + // double voltage = feedForward.calculateWithVelocities(lastFrameVelocity, targetVelocity); double voltage = feedForward.calculate(targetVelocity); + lastFrameVelocity = targetVelocity; motor.setVoltage(voltage); @@ -224,7 +228,7 @@ public class Turret extends SubsystemBase { SmartDashboard.putNumber("Turret targetVelocity", Units.radiansToDegrees(targetVelocity)); SmartDashboard.putNumber("Turret Position Deg", - Units.radiansToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO); + Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO); // SmartDashboard.putData("Spin to 90 deg", new // InstantCommand(()->{setFieldRelativeTarget(new Rotation2d(Math.PI), 0);})); -- 2.39.5