From: Wesley28w Date: Mon, 19 Jan 2026 00:20:24 +0000 (-0800) Subject: Added controls on PS5 and also Turret with cameras X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=58680d60b3b1751aa99139aa599b97b13ed07c23;p=FRC2026.git Added controls on PS5 and also Turret with cameras --- diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 6ef2c09..29be150 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -2,14 +2,22 @@ package frc.robot.controls; 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.CommandScheduler; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Robot; 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.turret.Turret; import lib.controllers.PS5Controller; import lib.controllers.PS5Controller.PS5Axis; import lib.controllers.PS5Controller.PS5Button; @@ -20,12 +28,17 @@ import lib.controllers.PS5Controller.PS5Button; public class PS5ControllerDriverConfig extends BaseDriverConfig { private final PS5Controller driver = new PS5Controller(Constants.DRIVER_JOY); private final BooleanSupplier slowModeSupplier = ()->false; + private Shooter shooter; + private Turret turret; + private Pose2d alignmentPose = null; - public PS5ControllerDriverConfig(Drivetrain drive) { + public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter) { super(drive); + this.shooter = shooter; + this.turret = turret; } - public void configureControls() { + public void configureControls() { // Reset the yaw. Mainly useful for testing/driver practice driver.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI) @@ -44,8 +57,25 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { getDrivetrain()::alignWheels, interrupted->getDrivetrain().setStateDeadband(true), ()->false, getDrivetrain()).withTimeout(2)); + + driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> shooter.setFeeder(ShooterConstants.FEEDER_RUN_POWER))).onFalse(new InstantCommand(() -> shooter.setFeeder(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(() -> turret.turnOnAlignment())).onFalse(new InstantCommand(() -> turret.turnOffAlignment())); } + public void setAlignmentPose(){ + Translation2d drivepose = getDrivetrain().getPose().getTranslation(); + // uses tag #?? + int tagNumber = 17; + Translation2d tagpose = FieldConstants.APRIL_TAGS.get(tagNumber - 1).pose.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)); + } + @Override public double getRawSideTranslation() { return driver.get(PS5Axis.LEFT_X); diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 01f49d6..be1915a 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; @@ -22,17 +23,21 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; +import frc.robot.constants.FieldConstants; import frc.robot.constants.IdConstants; +import frc.robot.subsystems.drivetrain.Drivetrain; public class Turret extends SubsystemBase { - double D_x = 1; - double D_y = 1; + // double D_x = 1; + // double D_y = 1; final private TalonFX motor; - + private boolean infiniteRotation = false; private double versaPlanetaryGearRatio = 5.0; private double turretGearRatio = 140.0/10.0; private final double gearRatio = versaPlanetaryGearRatio * turretGearRatio; + private boolean alignOn = false; + private double position; private double velocity; double power; @@ -48,7 +53,10 @@ public class Turret extends SubsystemBase { MechanismRoot2d mechanismRoot = mechanism2d.getRoot("pivot", 50, 50); MechanismLigament2d ligament2d = mechanismRoot.append(new MechanismLigament2d("turret_motor", 25, 0)); - public Turret() { + private Drivetrain drivetrain; + + public Turret(Drivetrain drivetrain) { + this.drivetrain = drivetrain; motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.CANIVORE_CAN); // switch of course if (RobotBase.isSimulation()) { @@ -110,45 +118,74 @@ public class Turret extends SubsystemBase { SmartDashboard.putData("Set to 180 degrees", new InstantCommand(() -> setSetpoint(180))); SmartDashboard.putData("Set to 270 degrees", new InstantCommand(() -> setSetpoint(270))); - SmartDashboard.putData("Set to 1,1", new InstantCommand(() -> setTarget(1,1))); - SmartDashboard.putData("Set to -1,1", new InstantCommand(( )-> setTarget(-1,1))); - SmartDashboard.putData("Set to -1,-1", new InstantCommand(() -> setTarget(-1,-1))); - SmartDashboard.putData("Set to 1,-1", new InstantCommand(() -> setTarget(1,-1))); + // SmartDashboard.putData("Set to 1,1", new InstantCommand(() -> setTarget(1,1))); + // SmartDashboard.putData("Set to -1,1", new InstantCommand(( )-> setTarget(-1,1))); + // SmartDashboard.putData("Set to -1,-1", new InstantCommand(() -> setTarget(-1,-1))); + // SmartDashboard.putData("Set to 1,-1", new InstantCommand(() -> setTarget(1,-1))); - SmartDashboard.putData("Print out target position", new InstantCommand(()-> System.out.println(getTargetPosition()))); + // SmartDashboard.putData("Print out target position", new InstantCommand(()-> System.out.println(getTargetPosition()))); } + + // public void toggleAlignOn() { + // if (alignOn) { + // alignOn = false + // } else { + // alignOn = true + // } + // } + + // public void setTarget(double x, double y) { + // D_x = x; + // D_y = y; + // } - public void setTarget(double x, double y) { - D_x = x; - D_y = y; + // public double[] getTargetPosition() { + // System.out.println("Distance X value is: "+ D_x + "and the Distance Y valye is: " + D_y); + // double[] target = {D_x, D_y}; + // return target; + // } + + public void turnOnAlignment() { + alignOn = true; } - public double[] getTargetPosition() { - System.out.println("Distance X value is: "+ D_x + "and the Distance Y valye is: " + D_y); - double[] target = {D_x, D_y}; - return target; + public void turnOffAlignment() { + alignOn = false; } public double getPosition() { return motor.getPosition().getValueAsDouble() / gearRatio; // Gear ratio } - // public void setSetpoint(double setpointDegrees) { - // double clampedSetpoint = MathUtil.inputModulus(setpointDegrees, 0, 360); - // this.setpoint = clampedSetpoint; - - // // Convert mechanism degrees to motor rotations: (Degrees / 360) * GearRatio - // double motorTargetRotations = (clampedSetpoint / 360.0) * gearRatio; - // motor.setControl(voltageRequest.withPosition(motorTargetRotations)); - // } - public void setSetpoint(double setpointDegrees) { - double motorTargetRotations = (setpointDegrees / 360.0) * gearRatio; - motor.setControl(voltageRequest.withPosition(motorTargetRotations)); + if (infiniteRotation) { + // 1. Get current position in degrees + double currentDegrees = (motor.getPosition().getValueAsDouble() / gearRatio) * 360.0; + // 2. Calculate the error + double error = setpointDegrees - currentDegrees; + // 3. Wrap the error to [-180, 180] + // This finds the "remainder" of the distance relative to a full circle + double optimizedError = Math.IEEEremainder(error, 360.0); + // 4. Calculate new target in degrees + double optimizedSetpointDegrees = currentDegrees + optimizedError; + double motorTargetRotations = (optimizedSetpointDegrees / 360.0) * gearRatio; + motor.setControl(voltageRequest.withPosition(motorTargetRotations)); + } else { + // normal limited 0,360 + double motorTargetRotations = (setpointDegrees / 360.0) * gearRatio; + motor.setControl(voltageRequest.withPosition(motorTargetRotations)); + } } public void align() { + Translation2d drivepose = drivetrain.getPose().getTranslation(); + // Using tag #?? + int tagNumber = 17; + Translation2d tagpose = FieldConstants.APRIL_TAGS.get(tagNumber - 1).pose.toPose2d().getTranslation(); + double D_y = tagpose.getY() - drivepose.getY(); + double D_x = tagpose.getX() - drivepose.getX(); double angleRad = Math.atan2(D_y, D_x); + System.out.println("Aligning the turn to degree angle: " + Units.radiansToDegrees(angleRad)); setSetpoint(Units.radiansToDegrees(angleRad)); } @@ -158,7 +195,10 @@ public class Turret extends SubsystemBase { velocity = Units.rotationsPerMinuteToRadiansPerSecond(motor.getVelocity().getValueAsDouble() * 60); // SmartDashboard.putNumber("Turret Position", position); - // align(); + if (alignOn) { + align(); + } + ligament2d.setAngle(position); } @@ -177,7 +217,7 @@ public class Turret extends SubsystemBase { double simAngle = turretSim.getAngleRads(); double simRotations = Units.radiansToRotations(simAngle); - double motorRotations = simRotations * gearRatio; // gear ratio is 1 + double motorRotations = simRotations * gearRatio; encoderSim.setRawRotorPosition(motorRotations); // MUST set position encoderSim.setRotorVelocity(turretSim.getVelocityRadPerSec() * Units.radiansToRotations(1) * gearRatio); // Gear Ratio