a

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index d896b0dfd5520109e1528f1e69294ed6b0a1f006..72aa526166a981ce841286115e1c2de7b012c4da 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -62,7 +62,8 @@ public class Superstructure extends Command {
 
     private final double phaseDelay = 0.03; // Extrapolation delay due to latency
 
-    private Translation2d target = FieldConstants.getHubTranslation().toTranslation2d();
+    //TODO: Change this back
+    private Translation2d target = FieldConstants.HUB_BLUE.toTranslation2d();
 
     private PhaseManager phaseManager = new PhaseManager();
 
@@ -122,9 +123,10 @@ public class Superstructure extends Command {
             goalState = ShooterPhysics.getShotParams(
                                target3d.minus(lookahead3d),
                 target == FieldConstants.getHubTranslation().toTranslation2d() ?
-                               2.0 : 3.0);
+                               2.0 : 5.0);
 
-            timeOfFlight = goalState.timeOfFlight();
+            double TOFAdjustment = 0.75;
+            timeOfFlight = goalState.timeOfFlight() * TOFAdjustment;
             double offsetX = turretVelocityX * timeOfFlight;
             double offsetY = turretVelocityY * timeOfFlight;
             lookaheadPose =
@@ -208,7 +210,7 @@ public class Superstructure extends Command {
         SmartDashboard.putNumber("Hood Offset", hoodOffset);
         // Phase manager stuff
         phaseManager.update(drivepose, shooter, turret);
-        //target = phaseManager.getTarget();
+        target = phaseManager.getTarget();
 
         updateDrivePose();
         updateSetpoints(drivepose);
@@ -221,7 +223,7 @@ public class Superstructure extends Command {
             if(phaseManager.getCurrentState() == CurrentState.UNDER_TRENCH){
                 hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE - hoodOffset), 0.0);
             } else{
-                hood.setFieldRelativeTarget(Rotation2d.fromDegrees(hoodSetpoint), hoodVelocity);
+                hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.hoodAngleMap.get(hoodSetpoint)), hoodVelocity);
             }
 
             shooter.setShooter(-ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));

diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java
index a9f60e79593910b284e026e375e69fa42f228225..1e68d07a335a169eb6b42fb5d04f16906566bdf4 100644 (file)
--- a/src/main/java/frc/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/robot/constants/FieldConstants.java
@@ -134,13 +134,13 @@ public class FieldConstants {
         || x > FIELD_LENGTH/2 + Units.inchesToMeters(120.0) && x < RED_ALLIANCE_LINE){
           return FieldZone.TRENCH_BUMP;
         }
-    if(x < FieldConstants.RED_ALLIANCE_LINE) { // inside red
+    if(x > FieldConstants.RED_ALLIANCE_LINE) { // inside red
       if (Robot.getAlliance() == Alliance.Red) {
         return FieldZone.ALLIANCE;
       } else {
         return FieldZone.OPPOSITION;
       }
-    } else if (x > FieldConstants.BLUE_ALLIANCE_LINE) {
+    } else if (x < FieldConstants.BLUE_ALLIANCE_LINE) {
       if (Robot.getAlliance() == Alliance.Blue) {
         return FieldZone.ALLIANCE;
       } else {

diff --git a/src/main/java/frc/robot/constants/ShotInterpolation.java b/src/main/java/frc/robot/constants/ShotInterpolation.java
index 194b48bd92925153bd99221030f138f0665c6ef1..da323296e0707a8b2bb8e441789c4adfda350a4e 100644 (file)
--- a/src/main/java/frc/robot/constants/ShotInterpolation.java
+++ b/src/main/java/frc/robot/constants/ShotInterpolation.java
@@ -2,6 +2,7 @@ package frc.robot.constants;
 
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.math.util.Units;
+import frc.robot.subsystems.hood.HoodConstants;
 
 public class ShotInterpolation {
     public static final InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap();
@@ -17,12 +18,22 @@ public class ShotInterpolation {
         shooterPowerMap.put(0.0, 1.0);
         shooterPowerMap.put(1.0, 1.0);
 
-        hoodAngleMap.put(0.0, Units.degreesToRadians(90));
-        hoodAngleMap.put(1.0, Units.degreesToRadians(90));
+        //hoodAngleMap.put(HoodConstants.MAX_ANGLE, HoodConstants.MAX_ANGLE);
+        hoodAngleMap.put(81.3, 70.25);
+        hoodAngleMap.put(79.0, 65.9);
+        hoodAngleMap.put(58.5, 48.5);
+        //hoodAngleMap.put(1.0, Units.degreesToRadians(90));
 
-        //TODO: find actual values from video motion
         exitVelocityMap.put(0.0, 0.0);
         exitVelocityMap.put(1.0, 2.2);
         exitVelocityMap.put(2.0, 4.4);
+        exitVelocityMap.put(7.0, 12.0);
+        exitVelocityMap.put(7.8, 15.2);
+        exitVelocityMap.put(7.78, 16.8);
+        exitVelocityMap.put(7.9, 17.1);
+        exitVelocityMap.put(8.0, 17.9);
+        exitVelocityMap.put(8.08, 19.0);
+        exitVelocityMap.put(25.0, 25.0* 3.2);
+        //exitVelocityMap.put(null, null);
     }
 }

diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
index 854aaf918037bf50808c6d427b9c2c728310de0a..6a4802312a96cb0919d95fb83792ae5190637fff 100644 (file)
--- a/src/main/java/frc/robot/constants/VisionConstants.java
+++ b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -155,8 +155,8 @@ public class VisionConstants {
                 new Pair<String, Transform3d>(
                         "CameraFrontLeft",
                         new Transform3d(
-                                new Translation3d(Units.inchesToMeters(8.47), 
-                                        Units.inchesToMeters(-11.54),
+                                new Translation3d(Units.inchesToMeters(-8.47), 
+                                        Units.inchesToMeters(11.54),
                                         Units.inchesToMeters(19.7)),
                                 new Rotation3d(0, Units.degreesToRadians(-22.0),
                                         Units.degreesToRadians(55.0)))),
@@ -171,8 +171,8 @@ public class VisionConstants {
                 new Pair<String, Transform3d>(
                         "CameraBackLeft",
                         new Transform3d(
-                                new Translation3d(Units.inchesToMeters(10.91), 
-                                        Units.inchesToMeters(-12),
+                                new Translation3d(Units.inchesToMeters(-10.91), 
+                                        Units.inchesToMeters(12),
                                         Units.inchesToMeters(19.66)),
                                 new Rotation3d(0, Units.degreesToRadians(-22.0),
                                         Units.degreesToRadians(145.0)))),

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 64825eb5aba937a9cee9e8411488a7a2524376ed..6041c96653712cfa0606aec100e49f00f7353986 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -176,11 +176,11 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
         // Hood
         if (hood != null) {
             // Calibration
-            controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
-                hood.calibrate();
-            })).onFalse(new InstantCommand(() -> {
-                hood.stopCalibrating();
-            }));
+            // controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
+            //     hood.calibrate();
+            // })).onFalse(new InstantCommand(() -> {
+            //     hood.stopCalibrating();
+            // }));
 
             // Set the hood down -- for safety measures under trench
             controller.get(DPad.RIGHT).whileTrue(new InstantCommand(()->{

diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java
index 1a7e5e36ecf751dfdd7fe922bca543fcae729be3..ca0a84075cc803fe6064ee25ef12a2329042be77 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java
@@ -183,12 +183,12 @@ public class Intake extends SubsystemBase implements IntakeIO{
         robotExtension.setLength(inchExtension);
 
         if(calibrating){
-            leftMotor.set(0.1);
+            leftMotor.set(-0.1);
             rightMotor.set(-0.1);
             boolean atHardStop = Math.abs((leftMotor.getStatorCurrent().getValueAsDouble() + rightMotor.getStatorCurrent().getValueAsDouble()) / 2) >= IntakeConstants.CALIBRATING_CURRENT_THRESHOLD;
-            if(calibrationDebouncer.calculate(atHardStop)){
-                stopCalibrating();
-            }
+            // if(calibrationDebouncer.calculate(atHardStop)){
+            //     stopCalibrating();
+            // }
         }
 
         updateInputs();
@@ -230,6 +230,15 @@ public class Intake extends SubsystemBase implements IntakeIO{
      * @param setpoint in inches
      */
     public void setPosition(double setpoint) {
+        leftMotor.getConfigurator().apply(
+            new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)
+            .withNeutralMode(NeutralModeValue.Coast)
+        );
+
+        rightMotor.getConfigurator().apply(
+            new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast)
+        );
+
         double motorRotations = inchesToRotations(setpoint);
         rightMotor.setControl(voltageRequest.withPosition(motorRotations));
         leftMotor.setControl(voltageRequest.withPosition(motorRotations));

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 1dbf65fb13f42cfd66d46042628e5b50965a6e77..e5896220ce86c6d563e44ecec9ab3913ef5feb71 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -122,7 +122,9 @@ public class Turret extends SubsystemBase implements TurretIO{
                //Sets the initial motor position
                motor.setPosition(motorRotations);
 
-               motor.setPosition(Units.degreesToRotations(238.86) * TurretConstants.GEAR_RATIO);
+               //motor.setPosition(Units.degreesToRotations(238.86) * TurretConstants.GEAR_RATIO);
+
+               motor.setPosition(0.0);
 
                SmartDashboard.putData("Turn to 0", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0);}));
                SmartDashboard.putData("Turn to -90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0);}));