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 6ef2c090bbbc04de7d5ad73979b88e3ee5b650f5..29be15070eba66713ed4ab5c25bb8ce27d1d40a2 100644 (file)
--- 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 01f49d6c07cb6f2d1b9be4f910adb72cf5648b03..be1915a3d38a65adc09980cbe6e53ec1f8fa9e22 100644 (file)
--- 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