Revert "Implement shooting while moving vertically"

This reverts commit d1055a4f27.

elastic-layout.json

@@ -8,10 +8,10 @@
         "layouts": [
           {
             "title": "Tag Processed",
-            "x": 896.0,
+            "x": 384.0,
             "y": 0.0,
             "width": 256.0,
-            "height": 384.0,
+            "height": 256.0,
             "type": "List Layout",
             "properties": {
               "label_position": "TOP"
@@ -56,10 +56,26 @@
         ],
         "containers": [
           {
-            "title": "RetractedLimit",
+            "title": "MatchTime",
-            "x": 512.0,
+            "x": 0.0,
             "y": 0.0,
-            "width": 256.0,
+            "width": 384.0,
+            "height": 256.0,
+            "type": "Match Time",
+            "properties": {
+              "topic": "/AdvantageKit/DriverStation/MatchTime",
+              "period": 0.06,
+              "data_type": "double",
+              "time_display_mode": "Minutes and Seconds",
+              "red_start_time": 15,
+              "yellow_start_time": 30
+            }
+          },
+          {
+            "title": "RetractedLimit",
+            "x": 0.0,
+            "y": 384.0,
+            "width": 128.0,
             "height": 128.0,
             "type": "Boolean Box",
             "properties": {
@@ -74,9 +90,9 @@
           },
           {
             "title": "Auto Chooser",
-            "x": 896.0,
+            "x": 1024.0,
-            "y": 384.0,
+            "y": 256.0,
-            "width": 256.0,
+            "width": 384.0,
             "height": 128.0,
             "type": "ComboBox Chooser",
             "properties": {
@@ -87,9 +103,9 @@
           },
           {
             "title": "Roller Percent Output",
-            "x": 512.0,
+            "x": 0.0,
             "y": 256.0,
-            "width": 384.0,
+            "width": 256.0,
             "height": 128.0,
             "type": "Number Slider",
             "properties": {
@@ -104,8 +120,8 @@
           },
           {
             "title": "Shooter OVERRIDE Velocity",
-            "x": 768.0,
+            "x": 128.0,
-            "y": 0.0,
+            "y": 384.0,
             "width": 128.0,
             "height": 128.0,
             "type": "Text Display",
@@ -118,8 +134,8 @@
           },
           {
             "title": "TRIM SHOOTER SPEED",
-            "x": 512.0,
+            "x": 256.0,
-            "y": 384.0,
+            "y": 256.0,
             "width": 384.0,
             "height": 128.0,
             "type": "Number Slider",
@@ -135,8 +151,8 @@
           },
           {
             "title": "Mode",
-            "x": 512.0,
+            "x": 256.0,
-            "y": 128.0,
+            "y": 384.0,
             "width": 384.0,
             "height": 128.0,
             "type": "Text Display",
@@ -148,49 +164,30 @@
             }
           },
           {
-            "title": "IsActive",
+            "title": "Time to rev",
-            "x": 0.0,
+            "x": 768.0,
-            "y": 0.0,
+            "y": 128.0,
-            "width": 384.0,
+            "width": 256.0,
-            "height": 256.0,
-            "type": "Boolean Box",
-            "properties": {
-              "topic": "/AdvantageKit/RealOutputs/HubShift/IsActive",
-              "period": 0.06,
-              "data_type": "boolean",
-              "true_color": 4283215696,
-              "false_color": 4294198070,
-              "true_icon": "None",
-              "false_icon": "None"
-            }
-          },
-          {
-            "title": "RemainingInShift",
-            "x": 0.0,
-            "y": 256.0,
-            "width": 384.0,
-            "height": 128.0,
-            "type": "Match Time",
-            "properties": {
-              "topic": "/AdvantageKit/RealOutputs/HubShift/RemainingInShift",
-              "period": 0.06,
-              "data_type": "double",
-              "time_display_mode": "Minutes and Seconds",
-              "red_start_time": 15,
-              "yellow_start_time": 30
-            }
-          },
-          {
-            "title": "Phase",
-            "x": 0.0,
-            "y": 384.0,
-            "width": 384.0,
             "height": 128.0,
             "type": "Large Text Display",
             "properties": {
-              "topic": "/AdvantageKit/RealOutputs/HubShift/Phase",
+              "topic": "/AdvantageKit/RealOutputs/Time to rev",
               "period": 0.06,
-              "data_type": "string"
+              "data_type": "double"
+            }
+          },
+          {
+            "title": "Arm angle extended",
+            "x": 1152.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Arm angle extended",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
             }
           }
         ]
@@ -530,8 +527,8 @@
           },
           {
             "title": "Shooter Idle max current",
-            "x": 1024.0,
+            "x": 384.0,
-            "y": 384.0,
+            "y": 512.0,
             "width": 256.0,
             "height": 128.0,
             "type": "Text Display",
@@ -558,8 +555,8 @@
           },
           {
             "title": "Shooter OVERRIDE Velocity",
-            "x": 1024.0,
+            "x": 640.0,
-            "y": 256.0,
+            "y": 512.0,
             "width": 256.0,
             "height": 128.0,
             "type": "Text Display",
@@ -579,59 +576,16 @@
         "layouts": [],
         "containers": [
           {
-            "title": "Hub Dist",
+            "title": "Auto Chooser",
-            "x": 640.0,
-            "y": 128.0,
-            "width": 256.0,
-            "height": 256.0,
-            "type": "Text Display",
-            "properties": {
-              "topic": "/AdvantageKit/RealOutputs/Hub Dist",
-              "period": 0.06,
-              "data_type": "double",
-              "show_submit_button": false
-            }
-          },
-          {
-            "title": "Distance Tolerence",
-            "x": 256.0,
-            "y": 128.0,
-            "width": 256.0,
-            "height": 256.0,
-            "type": "Text Display",
-            "properties": {
-              "topic": "/SmartDashboard/Distance Tolerence",
-              "period": 0.06,
-              "data_type": "double",
-              "show_submit_button": true
-            }
-          },
-          {
-            "title": "Chassis positive speed offset",
-            "x": 0.0,
-            "y": 256.0,
-            "width": 256.0,
-            "height": 256.0,
-            "type": "Text Display",
-            "properties": {
-              "topic": "/SmartDashboard/Chassis positive speed offset",
-              "period": 0.06,
-              "data_type": "double",
-              "show_submit_button": true
-            }
-          },
-          {
-            "title": "Chassis negative speed offset",
             "x": 0.0,
             "y": 0.0,
-            "width": 256.0,
+            "width": 640.0,
-            "height": 256.0,
+            "height": 768.0,
-            "type": "Text Display",
+            "type": "ComboBox Chooser",
             "properties": {
-              "topic": "/SmartDashboard/Chassis negative speed offset",
+              "topic": "/SmartDashboard/Auto Chooser",
               "period": 0.06,
-              "data_type": "double",
+              "sort_options": false
-              "show_submit_button": true
             }
           }
         ]

src/main/java/frc4388/robot/RobotContainer.java

@@ -125,7 +125,7 @@ public class RobotContainer {
         // );
     
         private Command RobotRev = new SequentialCommandGroup(
-            new InstantCommand(() -> m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()), m_robotShooter),
+            new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter),
             IntakeExtended,
             new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake)
         );
@@ -315,7 +315,7 @@ public class RobotContainer {
             .onTrue(new InstantCommand(() -> {
                 m_robotIntake.setMode(IntakeMode.Idle);
                 m_robotIntake.rollerStop();
-                m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds());
+                m_robotShooter.spinUpShooting();
             }))
             .onFalse(new InstantCommand(() -> {
                 m_robotShooter.spinUpIdle();
@@ -352,7 +352,7 @@ public class RobotContainer {
         //         m_robotClimber.toggleDeployed();
         //     }));
 
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> {
                 m_robotShooter.indexerStalled();
             }))

src/main/java/frc4388/robot/subsystems/shooter/Shooter.java

@@ -33,8 +33,6 @@ public class Shooter extends SubsystemBase {
     public boolean badShooterVelocity;
     public double distanceToHub = 5;
 
-    public double chassisXSpeed = 0;
-
 
     public Shooter(
         ShooterIO io,
@@ -68,8 +66,7 @@ public class Shooter extends SubsystemBase {
     private ShooterMode mode = ShooterMode.Idle;
     private boolean shooterButtonReady = false;
 
-    public void spinUpShooting(double chassisXSpeed) {
+    public void spinUpShooting() {
-        this.chassisXSpeed = chassisXSpeed;
         this.mode = ShooterMode.Shooting;
     }
 
@@ -160,14 +157,14 @@ public class Shooter extends SubsystemBase {
 
         //revtime calculations
         // double shooterAcceleration = 
-        double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub,chassisXSpeed).in(RotationsPerSecond);
+        double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub).in(RotationsPerSecond);
         double revTime = (Math.abs(shooterSpeed - shooterSpeedTargetPretend)/((7 - shooterSpeedTargetPretend)/ShooterConstants.T_CONSTANT));
         // double revTimeExp = ShooterConstants.T_CONSTANT * Math.log(1 - Math.abs(shooterSpeed/shooterSpeedTargetPretend));
         Logger.recordOutput("Time to rev", revTime);
 
         switch (mode) {
             case Shooting:
-                io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed));
+                io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
 
                 int bitmask = (
                     (shooterButtonReady ? 1 : 0) +

src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java

@@ -36,8 +36,7 @@ public class    ShooterConstants {
 
     
     public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0);
-    public static final ConfigurableDouble CHASSIS_POS_OFFSET = new ConfigurableDouble("Chassis positive speed offset", 1.0);
+
-    public static final ConfigurableDouble CHASSIS_NEG_OFFSET = new ConfigurableDouble("Chassis negative speed offset", 1.0);
     // Shoot mode tolerances
     public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8);
     public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8);
@@ -50,29 +49,16 @@ public class    ShooterConstants {
     public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3); 
 
     // 
-    public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) {
+    public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) {
         // Model derived from points
         // double speed = 
         //     1.11576*hubDistMeters*hubDistMeters +
         //     0.318464*hubDistMeters +
         //     30.6293;
-            double speed;
+        double speed = 
-        if (Math.abs(chassisXSpeed) < 0.1) {
-            speed = 
             0.0593402*hubDistMeters*hubDistMeters +
             4.90561*hubDistMeters +
             30.35696 + MODEL_TRIM.get();
-        } else if (chassisXSpeed > 0) { // positive = further from hub
-            speed = 
-            0.0593402*hubDistMeters*hubDistMeters +
-            4.90561*hubDistMeters +
-            30.35696 + (chassisXSpeed * CHASSIS_POS_OFFSET.get()) + MODEL_TRIM.get();
-        } else { // negative = moving towards hub
-            speed = 
-            0.0593402*hubDistMeters*hubDistMeters +
-            4.90561*hubDistMeters +
-            30.35696 + (chassisXSpeed * CHASSIS_NEG_OFFSET.get()) + MODEL_TRIM.get();
-        }
 
         // double speed =
         //     0.00610938*hubDistMeters*hubDistMeters
@@ -96,8 +82,8 @@ public class    ShooterConstants {
     // Motor Configuration
     public static Slot0Configs SHOOTER_PID = new Slot0Configs()
         .withKV(0.0)
-        .withKP(0.02)
+        .withKP(0.05)
-        .withKI(0.15)
+        .withKI(0.20)
         .withKD(0.0);
 
     
@@ -105,7 +91,7 @@ public class    ShooterConstants {
     
     
     
-    public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.02);
+    public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08);
     public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.15);
     public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0);
 

src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java

@@ -412,7 +412,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){
             double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity));
             fieldPos =  new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset);
-            Logger.recordOutput("Offset Value", aimOffset);
+            Logger.recordOutput("Offset Simple", aimOffset);
 
         }
 
@@ -512,14 +512,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         ); // stop the modules without breaking
     }
 
-    public double chassisXSpeeds(){
-        if (TimesNegativeOne.isRed) {
-            return chassisSpeeds.vxMetersPerSecond;
-        } else {
-            return -chassisSpeeds.vxMetersPerSecond;
-        }
-    }
-
     public void stopModules() {
         // stopped = true;
         // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());