Merge pull request #540 from frc-862/534-note-pass-w-interp-map

[#534] note pass w interp map

src/main/java/frc/robot/Constants.java

@@ -51,6 +51,10 @@ public class DrivetrainConstants {
 
         public static final Translation2d SPEAKER_POSE = new Translation2d(0d, 5.547393);
 
+        public static final Translation2d RED_CORNER_POSE = new Translation2d(15.6592d, 7.5d);
+        public static final Translation2d BLUE_CORNER_POSE = new Translation2d(0.8d, 7.5d); // TODO get real >:)
+
+
         public static final double ALIGNMENT_TOLERANCE = 1d;
     }
 
@@ -270,6 +274,7 @@ public class VisionConstants {
         public static final double PROCESS_LATENCY = 0.0472; // TODO test this value
 
         public static final double POINTATTAG_ALIGNMENT_TOLERANCE = 1d;
+        public static final double POINTATPOINT_ALIGNMENT_TOLERANCE = 1d;
         public static final PIDController POINT_AIM_CONTROLLER = new PIDController(0.2, 0, 0.015, 0.01);
         public static final PIDController TAG_AIM_CONTROLLER = new PIDController(0.1, 0, 0.01);
         public static final PIDController COMBO_CONTROLLER = new PIDController(0.1, 0, 0.01);
@@ -568,6 +573,28 @@ public class ShooterConstants {
             }
         };
 
+        public static final InterpolationMap NOTEPASS_ANGLE_MAP = new InterpolationMap() {
+            {
+                put(13.716, 36d);
+                put(12.192, 36d);
+                put(10.668, 37d);
+                put(9.144, 38d);
+                put(7.62, 41d);
+                put(6.096, 46d);
+            }
+        };
+
+        public static final InterpolationMap NOTEPASS_SPEED_MAP = new InterpolationMap() {
+            {
+                put(13.716, 3600d);
+                put(12.192, 3300d);
+                put(20.668, 2900d);
+                put(9.144, 2600d);
+                put(7.62, 2250d);
+                put(6.096, 1900d);
+            }
+        };
+
         public enum ShootingState {
             AIM, SHOOT, SHOT
         }
@@ -692,3 +719,4 @@ public int getPriority() {
         }
     }
 }
+

src/main/java/frc/robot/Robot.java

@@ -38,7 +38,7 @@ public void disabledPeriodic() {
 
         RobotContainer container = (RobotContainer) getContainer();
         if (haveDriverStation) {
-            container.drivetrain.setSpeakerPose(DriverStation.getAlliance().get());
+            container.drivetrain.setAlliancePose(DriverStation.getAlliance().get());
         }
         container.pivot.resetBias();
 

src/main/java/frc/robot/RobotContainer.java

@@ -1,5 +1,7 @@
 package frc.robot;
 
+import java.util.function.BooleanSupplier;
+
 import com.ctre.phoenix6.Orchestra;
 import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
@@ -9,12 +11,15 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.StartEndCommand;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -36,6 +41,7 @@
 import frc.robot.command.ManualClimb;
 import frc.robot.command.PathFindToAuton;
 import frc.robot.command.PathToPose;
+import frc.robot.command.PointAtPoint;
 // import frc.robot.command.ReverseChasePieces;
 import frc.robot.command.Sing;
 import frc.robot.command.SmartCollect;
@@ -99,8 +105,9 @@ public class RobotContainer extends LightningContainer {
 	SwerveRequest.RobotCentric slowRobotCentric;
 	SwerveRequest.PointWheelsAt point;
 	Telemetry logger;
-
+	
 	private Boolean triggerInit;
+	private boolean notePassOnTarget = false;
 	public static double bias = 0d;
 
 	@Override
@@ -254,16 +261,15 @@ protected void configureButtonBindings() {
 						.deadlineWith(leds.enableState(LED_STATES.COLLECTING)));
 
 		// new Trigger(coPilot::getBButton)
-		// 	.whileTrue(new AutonSmartCollect(() -> 0.65, () -> 0.75, collector, indexer)
-			// .deadlineWith(leds.enableState(LED_STATES.COLLECTING).withTimeout(1)));
+		// .whileTrue(new AutonSmartCollect(() -> 0.65, () -> 0.75, collector, indexer)
+		// .deadlineWith(leds.enableState(LED_STATES.COLLECTING).withTimeout(1)));
 
 		// cand shots for the robot
 		new Trigger(coPilot::getXButton)
 				.whileTrue(new PointBlankShot(flywheel, pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING)));
 		// new Trigger(coPilot::getYButton).whileTrue(new PivotUP(pivot));
-		new Trigger(coPilot::getYButton).whileTrue(new NotePass(flywheel, pivot)
-				.deadlineWith(leds.enableState(LED_STATES.SHOOTING)));
-		// new Trigger(coPilot::getYButton).whileTrue(new Tune(flywheel, pivot)
+		new Trigger(coPilot::getYButton).whileTrue(new NotePass(drivetrain, flywheel, pivot, driver, indexer));
+		// new Trigger(coPilot::getYButton).whileTrue(new Tune(flywheel, pivot));
 		// .deadlineWith(leds.enableState(LED_STATES.SHOOTING)));
 		new Trigger(coPilot::getAButton).whileTrue(new AmpShot(flywheel, pivot)
 				.deadlineWith(leds.enableState(LED_STATES.SHOOTING)));

src/main/java/frc/robot/command/shoot/NotePass.java

@@ -4,53 +4,129 @@
 
 package frc.robot.command.shoot;
 
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.RobotContainer;
-import frc.robot.Constants.CandConstants;
+import frc.robot.Constants.DrivetrainConstants;
+import frc.robot.Constants.ShooterConstants;
+import frc.robot.Constants.VisionConstants;
 import frc.robot.subsystems.Flywheel;
+import frc.robot.subsystems.Indexer;
 import frc.robot.subsystems.Pivot;
+import frc.robot.subsystems.Swerve;
 import frc.thunder.command.TimedCommand;
+import frc.thunder.filter.XboxControllerFilter;
+import frc.thunder.shuffleboard.LightningShuffleboard;
 
 public class NotePass extends Command {
-  private final Flywheel flywheel;
+
+	private final Swerve drivetrain;
+  	private final Flywheel flywheel;
 	private final Pivot pivot;
+	private Translation2d targetPose;
+	private double currentHeading;
+	private double targetHeading;
+	private double feedForwardOutput;
+	private double pidOutput;
+	private PIDController pidController = VisionConstants.COMBO_CONTROLLER; // TAG_AIM_CONTROLLER
+    private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.25, 0.5);
+	private XboxControllerFilter driver;
+	private Indexer indexer;
 
 	/**
 	 * Creates a new NotePass.
 	 *
+	 * @param drivetrain subsystem
 	 * @param pivot    subsystem
 	 * @param flywheel subsystem
+	 * @param driver   the driver's controller, used for drive input
+	 * @param indexer  subsystem
 	 */
-	public NotePass(Flywheel flywheel, Pivot pivot) {
+	public NotePass(Swerve drivetrain, Flywheel flywheel, Pivot pivot, XboxControllerFilter driver, Indexer indexer) {
+		this.drivetrain = drivetrain;
 		this.flywheel = flywheel;
 		this.pivot = pivot;
+		this.driver = driver;
+		this.indexer = indexer;
 
-		addRequirements(flywheel, pivot);
+		addRequirements(flywheel, pivot, drivetrain, indexer);
 	}
 
 	@Override
 	public void initialize() {
-		flywheel.setAllMotorsRPM(CandConstants.NOTE_PASS_RPM + flywheel.getBias());
-		pivot.setTargetAngle(CandConstants.NOTE_PASS_ANGLE + pivot.getBias());
+		if(isBlueAlliance()){
+			targetPose = DrivetrainConstants.BLUE_CORNER_POSE;
+		} else {
+			targetPose = DrivetrainConstants.RED_CORNER_POSE;
+		}
 	}
 
 	@Override
 	public void execute() {
-		flywheel.setAllMotorsRPM(CandConstants.NOTE_PASS_RPM + pivot.getBias());
-		pivot.setTargetAngle(CandConstants.NOTE_PASS_ANGLE + flywheel.getBias());
-		if(flywheel.allMotorsOnTarget() && pivot.onTarget()) {
+		Pose2d pose = drivetrain.getPose();
+        var deltaX = targetPose.getX() - pose.getX();
+        var deltaY = targetPose.getY() - pose.getY();
+
+        currentHeading = (pose.getRotation().getDegrees() + 360) % 360;
+
+        // Calculate vector to target, add 180 to make it point backwards
+        targetHeading = Math.toDegrees(Math.atan2(deltaY, deltaX)) + 180; 
+        targetHeading = (targetHeading + 360) % 360; // Modulo 360 to keep it in the range of 0-360
+
+        pidOutput = pidController.calculate(currentHeading, targetHeading);
+        feedForwardOutput = feedforward.calculate(pidOutput);
+
+        if(inTolerance()) {
+            feedForwardOutput = 0;
+        }
+
+        drivetrain.setField(-driver.getLeftY(), -driver.getLeftX(), feedForwardOutput);
+
+		flywheel.setAllMotorsRPM(ShooterConstants.NOTEPASS_SPEED_MAP.get(drivetrain.distanceToCorner()) + flywheel.getBias());
+		pivot.setTargetAngle(ShooterConstants.NOTEPASS_ANGLE_MAP.get(drivetrain.distanceToCorner()) + pivot.getBias());
+
+		if (flywheel.allMotorsOnTarget() && pivot.onTarget() /*&& inTolerance() */) {
+			indexer.indexUp();
 			new TimedCommand(RobotContainer.hapticCopilotCommand(), 1d).schedule();
+			new TimedCommand(RobotContainer.hapticDriverCommand(), 1d).schedule();
+		}
+
+		if (!DriverStation.isFMSAttached()) {
+			LightningShuffleboard.setBool("Note-Pass", "In tloeracnce", inTolerance());
+			LightningShuffleboard.setDouble("Note-Pass", "CurrentHeading", currentHeading);
+			LightningShuffleboard.setDouble("Note-Pass", "TargetHeading", targetHeading);
+			LightningShuffleboard.setDouble("Note-Pass", "Distance to Corner", drivetrain.distanceToCorner());
 		}
 	}
 
 	@Override
 	public void end(boolean interrupted) {
 		flywheel.coast(true);
 		pivot.setTargetAngle(pivot.getStowAngle());
+		indexer.stop();
 	}
 
 	@Override
 	public boolean isFinished() {
-		return false;
+		return !indexer.hasNote();
 	}
+
+	private boolean isBlueAlliance() {
+        var alliance = DriverStation.getAlliance();
+        return alliance.isPresent() && alliance.get() == Alliance.Blue;
+    }
+
+	private boolean inTolerance() {
+        double difference = Math.abs(currentHeading - targetHeading);
+        difference = difference > 180 ? 360 - difference : difference;
+        return difference <= VisionConstants.POINTATPOINT_ALIGNMENT_TOLERANCE; // TODO not has Target but if the correct filter is set
+    }
 }

src/main/java/frc/robot/subsystems/Swerve.java

@@ -57,6 +57,7 @@ public class Swerve extends SwerveDrivetrain implements Subsystem {
     private LinearFilter xFilter = LinearFilter.singlePoleIIR(2, 0.01);
     private LinearFilter yFilter = LinearFilter.singlePoleIIR(2, 0.01);
     private LinearFilter rotFilter = LinearFilter.singlePoleIIR(2, 0.01);
+    private Translation2d cornerPose = DrivetrainConstants.BLUE_CORNER_POSE;
     private Translation2d speakerPose = PoseConstants.BLUE_SPEAKER_LOCATION.toTranslation2d();
 
     private DoubleLogEntry timerLog;
@@ -387,16 +388,21 @@ public void enableVision() {
         System.out.println("Vision Enabled");
     }
 
-    public void setSpeakerPose(Alliance alliance) {
+    public void setAlliancePose(Alliance alliance) {
         if (alliance == Alliance.Red) {
             speakerPose = PoseConstants.RED_SPEAKER_LOCATION.toTranslation2d();
+            cornerPose = DrivetrainConstants.RED_CORNER_POSE;
         }
     }
 
     public double distanceToSpeaker() {
         return speakerPose.getDistance(getPose().getTranslation());
     }
 
+    public double distanceToCorner() {
+        return cornerPose.getDistance(getPose().getTranslation());
+    }
+
     public void setDrivetrainPose(Pose2d newPose) {
         seedFieldRelative(newPose);
     }