a

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 39367b7610f07b16a6d23f6b3e1026d31e3a097d..a77b29ba899185ad77454578446cff656de7bec8 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -28,6 +28,7 @@ import frc.robot.commands.vision.ShutdownAllPis;
 import frc.robot.constants.AutoConstants;
 import frc.robot.constants.Constants;
 import frc.robot.constants.IntakeConstants;
+import frc.robot.constants.VisionConstants;
 import frc.robot.controls.BaseDriverConfig;
 import frc.robot.controls.Operator;
 import frc.robot.controls.PS5ControllerDriverConfig;
@@ -104,18 +105,18 @@ public class RobotContainer {
         intake = new Intake();
 
       case WaffleHouse: // AKA Betabot
-        // turret = new Turret();
+        turret = new Turret();
         shooter = new Shooter();
         hood = new Hood();
 
       case SwerveCompetition: // AKA "Vantage"
 
       case BetaBot: // AKA "Pancake"
-        // vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS);
+        vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS);
         // fall-through
 
       case Vivace:
-        // linearClimb = new LinearClimb();
+        //linearClimb = new LinearClimb();
 
       case Phil: // AKA "IHOP"
 

diff --git a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
index 60cb84e80266b69838c462b3d15d71ccc873dd8b..124e3ea537d33880ba06ae6e4488473e4850b584 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
+++ b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
@@ -23,14 +23,15 @@ public class ReverseMotors extends Command {
 
     @Override
     public void execute(){
+        intake.extend();
         intake.spinReverse();
-        spindexer.reverseSpindexer();
+        //spindexer.reverseSpindexer();
     }
 
     @Override
     public void end(boolean interrupted){
-        intake.spinStart();
-        spindexer.maxSpindexer();
+        intake.spinStop();
+        //spindexer.maxSpindexer();
     }
 
 }

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index e721a3920ec0aced5dd15e7042d5b8f059913777..f5707b098589dd3434c36cfdc020fafc5d79d14d 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -217,7 +217,7 @@ public class Superstructure extends Command {
             } else{
                 hood.setFieldRelativeTarget(Rotation2d.fromDegrees(hoodSetpoint), hoodVelocity);
             }
-            
+
             shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
 
             if (phaseManager.shouldFeed()) {
@@ -230,6 +230,8 @@ public class Superstructure extends Command {
         SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint);
         SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint);
         SmartDashboard.putNumber("Shooter Calculate Velocity", goalState.exitVel());
+
+        SmartDashboard.putString("Phase Manager State", phaseManager.getCurrentState().toString());
     }
 
     @Override

diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java
index 578b8cbfcef99cff755cbf3d57cd7cd0e809dbc7..e09f459f7897e471190ebed7974923db468ef57f 100644 (file)
--- a/src/main/java/frc/robot/constants/IntakeConstants.java
+++ b/src/main/java/frc/robot/constants/IntakeConstants.java
@@ -4,7 +4,7 @@ import edu.wpi.first.math.util.Units;
 
 public class IntakeConstants {
     /** Intake roller motor speed in range [-1, 1] */
-    public static final double SPEED = 1.0;
+    public static final double SPEED = 0.8;
     /** 12 tooth pinion driving 36 tooth driven gear */
     public static final double GEAR_RATIO = 36.0/12.0;
     /** radius (inches) of the rack gear which is a 10 tooth pinion at 10 DP */
@@ -22,9 +22,9 @@ public class IntakeConstants {
 
 
     /** max extension in inches */
-    public static final double MAX_EXTENSION = 10.0 - 2.0; // inches
+    public static final double MAX_EXTENSION = 10.5; // inches
 
-    public static final double INTERMEDIATE_EXTENSION = 5.0 - 2.0; //inches
+    public static final double INTERMEDIATE_EXTENSION = 5.0; //inches
 
     public static final double STOW_EXTENSION = 0.2; // inches
 

diff --git a/src/main/java/frc/robot/constants/ShotInterpolation.java b/src/main/java/frc/robot/constants/ShotInterpolation.java
index b4643d68a8a83f77d1cc3f72c1b65042ba50995c..194b48bd92925153bd99221030f138f0665c6ef1 100644 (file)
--- a/src/main/java/frc/robot/constants/ShotInterpolation.java
+++ b/src/main/java/frc/robot/constants/ShotInterpolation.java
@@ -21,7 +21,8 @@ public class ShotInterpolation {
         hoodAngleMap.put(1.0, Units.degreesToRadians(90));
 
         //TODO: find actual values from video motion
-        exitVelocityMap.put(1.0, 2.0);
-        exitVelocityMap.put(2.0, 4.0);
+        exitVelocityMap.put(0.0, 0.0);
+        exitVelocityMap.put(1.0, 2.2);
+        exitVelocityMap.put(2.0, 4.4);
     }
 }

diff --git a/src/main/java/frc/robot/constants/swerve/DriveConstants.java b/src/main/java/frc/robot/constants/swerve/DriveConstants.java
index 827817afda1fefdcdb25787e633b4e600d945681..c9f65912f5e04e354f33a16505c90aea2e1586b9 100644 (file)
--- a/src/main/java/frc/robot/constants/swerve/DriveConstants.java
+++ b/src/main/java/frc/robot/constants/swerve/DriveConstants.java
@@ -25,7 +25,7 @@ public class DriveConstants {
      */
     public static final double ROBOT_WIDTH_WITH_BUMPERS = 0.832;
 
-    public static double ROBOT_MASS = Units.lbsToKilograms(108.3 + 13 + 13.4);
+    public static double ROBOT_MASS = Units.lbsToKilograms(108.3 + 13 + 13.4 + 5.0);
 
     /** Radius of the drive wheels [meters]. */
     public static final double WHEEL_RADIUS = Units.inchesToMeters(1.95);

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index d85206f1e056488ab4184b1278d2870f010a7bf0..ddc0e40c217bf44038027af07b8866ce3a7cb5ed 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -114,6 +114,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
             controller.get(PS5Button.RIGHT_TRIGGER).debounce(3.0).onTrue(new InstantCommand(() -> {
                 intake.retract();
                 intakeBoolean = true;
+                intake.spinStop();
             }));
 
             // Calibration
@@ -122,6 +123,11 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
             })).onFalse(new InstantCommand(() -> {
                 intake.stopCalibrating();
             }));
+
+            // Stop intake roller
+            controller.get(DPad.DOWN).onTrue(new InstantCommand(()->{
+                intake.spinStop();
+            }));
         }
 
         // Spindexer
@@ -140,7 +146,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
         // Climb
         if (climb != null) {
             // Calibration
-            controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
+            controller.get(PS5Button.OPTIONS).onTrue(new InstantCommand(() -> {
                 climb.hardstopCalibration();
             })).onFalse(new InstantCommand(() -> {
                 climb.stopCalibrating();

diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
index 784aaf69637163190ca8984fa40cc3bd5f360eed..9667a77814ae93101c79c9c07e7c4f95c892cb5e 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
+++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
@@ -36,7 +36,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
     private Debouncer calibrationDebouncer = new Debouncer(0.5, DebounceType.kRising);
 
     // logging information
-    private LinearClimbIOInputs inputs = new LinearClimbIOInputs();
+    private LinearClimbIOInputsAutoLogged inputs = new LinearClimbIOInputsAutoLogged();
 
     private final PIDController pid = new PIDController(
             ClimbConstants.PID_P,
@@ -73,6 +73,9 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
      * @param setpoint in rotations
      */
     public void setSetpoint(double setpoint) {
+        TalonFXConfiguration config = new TalonFXConfiguration();
+        config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+        motor.getConfigurator().apply(config);
         pid.setSetpoint(setpoint);
     }
 
@@ -145,6 +148,8 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
         motor.set(power);
         
         Logger.recordOutput("LinearClimb/setpointMeters", Units.rotationsToRadians(pid.getSetpoint()) * ClimbConstants.WHEEL_RADIUS / ClimbConstants.CLIMB_GEAR_RATIO);
+        updateInputs();
+        Logger.processInputs("LinearClimb", inputs);
     }
     /**
      * converts motor rotations to meters

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 4b033f76b249d1d21eaeb6c29239f016a6547662..75eaeca717eef87bc1d74fe23dcc742fb37e1406 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -69,7 +69,7 @@ public class Turret extends SubsystemBase implements TurretIO{
                motor.setNeutralMode(NeutralModeValue.Brake);
 
                TalonFXConfiguration config = new TalonFXConfiguration();
-               config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+               config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
     
                config.Slot0.kP = 12.0; 
                config.Slot0.kS = 0.1; // Static friction compensation
@@ -130,6 +130,14 @@ public class Turret extends SubsystemBase implements TurretIO{
                //Sets the initial motor position
                motor.setPosition(motorRotations);
 
+               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);}));
+               SmartDashboard.putData("Turn to 90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0);}));
+               SmartDashboard.putData("Turn to 200", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0);}));
+               SmartDashboard.putData("Turn to -200", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0);}));
+
        }
 
        /* ---------------- Public API ---------------- */

diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
index 83dd1c3953237a46a2e744831d2d55a6478e0364..9b3dd6b43bebf0220f502353598358f02d7654bd 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
@@ -22,8 +22,8 @@ public class TurretConstants {
     public static int RIGHT_ENCODER_TEETH = 22; // read above
     public static int ENCODER_COUNT_TOTAL = 8192; // how many intervals it can have, like clicks on a clock chat gpt explained to me
 
-    public static double LEFT_ENCODER_OFFSET = 0.463379; //  rot
-    public static double RIGHT_ENCODER_OFFSET = 0.197266; //  rot
+    public static double LEFT_ENCODER_OFFSET = 0.364502; //  rot
+    public static double RIGHT_ENCODER_OFFSET = 0.718506; //  rot
 
     // Turret is in center of robot, but make use of the height in shooter physics
     public static Translation3d DISTANCE_FROM_ROBOT_CENTER = new Translation3d(0,0, Units.inchesToMeters(22.172)); //meters
@@ -34,7 +34,7 @@ public class TurretConstants {
 
        public static final double FEEDFORWARD_KV = 0.185;
 
-    public static final double NORMAL_CURRENT_LIMIT = 50.0; // A
+    public static final double NORMAL_CURRENT_LIMIT = 20.0; // A
     public static final double CALIBRATION_CURRENT_LIMIT = 10.0; // A
     public static final double CALIBRATION_CURRENT_THRESHOLD = 9.0; // A