Drive and more shooter

RegisJesuitRobotics · Feb 21, 2024 · 4bb8c2b · 4bb8c2b
1 parent 169b6f7
commit 4bb8c2b
Show file tree

Hide file tree

Showing 6 changed files with 34 additions and 22 deletions.
diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "Competition2024";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 115;
-  public static final String GIT_SHA = "8c975b34b659b8bf7369f379e803422cd3dae397";
-  public static final String GIT_DATE = "2024-02-21 08:03:57 MST";
+  public static final int GIT_REVISION = 116;
+  public static final String GIT_SHA = "169b6f70fac563f1a8d2dca9938c53e55ae751a4";
+  public static final String GIT_DATE = "2024-02-21 09:23:36 MST";
   public static final String GIT_BRANCH = "workingongettingready";
-  public static final String BUILD_DATE = "2024-02-21 09:21:03 MST";
-  public static final long BUILD_UNIX_TIME = 1708532463510L;
+  public static final String BUILD_DATE = "2024-02-21 10:39:21 MST";
+  public static final long BUILD_UNIX_TIME = 1708537161942L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -32,7 +32,7 @@ public static class IntakeConstants {
 
   public static class SlapdownConstants {
     public static final int FEEDER_MOTOR_ID = 7;
-    public static final int ROTATION_MOTOR_ID = 8;
+    public static final int ROTATION_MOTOR_ID = 9;
 
     public static final boolean FEEDER_INVERTED = true;
     public static final boolean ROTATION_INVERTED = false;
@@ -163,18 +163,18 @@ private SwerveConstants() {}
 
     public static final SwerveModuleConfiguration FRONT_LEFT_MODULE_CONFIGURATION =
         new SwerveModuleConfiguration(
-            12, 9, 17, false, true, -0.7455146908760071, false, SHARED_SWERVE_MODULE_CONFIGURATION);
+            12, 8, 17, false, true, 2.862408, false, SHARED_SWERVE_MODULE_CONFIGURATION);
     public static final SwerveModuleConfiguration FRONT_RIGHT_MODULE_CONFIGURATION =
         new SwerveModuleConfiguration(
-            13, 5, 18, false, true, 2.7857091108003242, false, SHARED_SWERVE_MODULE_CONFIGURATION);
+            13, 5, 18, false, true, -1.294680, false, SHARED_SWERVE_MODULE_CONFIGURATION);
 
     public static final SwerveModuleConfiguration BACK_LEFT_MODULE_CONFIGURATION =
         new SwerveModuleConfiguration(
-            14, 10, 19, false, true, 2.676796474860444, false, SHARED_SWERVE_MODULE_CONFIGURATION);
+            14, 10, 19, false, true, 0.368155, false, SHARED_SWERVE_MODULE_CONFIGURATION);
 
     public static final SwerveModuleConfiguration BACK_RIGHT_MODULE_CONFIGURATION =
         new SwerveModuleConfiguration(
-            15, 3, 20, false, true, -2.4605051837685683, false, SHARED_SWERVE_MODULE_CONFIGURATION);
+            15, 3, 20, false, true, 2.906894, false, SHARED_SWERVE_MODULE_CONFIGURATION);
   }
 
   public static class AutoConstants {
@@ -256,13 +256,13 @@ public static class ShooterConstants {
 
     public static TunablePIDGains SHOOTER_PID_GAINS =
         new TunablePIDGains(
-            "/shooter/pid", 0.024885 / (Math.PI * 2), 0, 0, MiscConstants.TUNING_MODE);
+            "/shooter/pid", 0.005, 0, 0, MiscConstants.TUNING_MODE);
     public static TunableFFGains SHOOTER_FF_GAINS =
         new TunableFFGains(
             "/shooter/FF",
-            0.34964 / (Math.PI * 2),
-            0.05597 / (Math.PI * 2),
-            0.01424 / (Math.PI * 2),
+                0.39383,
+            0.0089833,
+            0.0010833,
             MiscConstants.TUNING_MODE);
   }
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -197,9 +197,10 @@ private void configureOperatorBindings() {
         .onTrue(
             new AmpPlaceCommand(
                 elevatorSubsystem, wristSubsystem, shooterSubsystem, transportSubsystem));
-    operatorController.share().whileTrue(Commands.parallel(elevatorSubsystem.setElevatorPositionCommand(0), slapdownSubsystem.setFeederVoltageCommand(6), intakeSubsystem.setIntakeVoltageCommand(6), transportSubsystem.setVoltageCommand(6.0).until(transportSubsystem::atSensor).andThen(transportSubsystem.setVoltageCommand(0.0))));
+    operatorController.share().whileTrue(Commands.parallel(elevatorSubsystem.setElevatorPositionCommand(0), slapdownSubsystem.setFeederVoltageCommand(6), intakeSubsystem.setIntakeVoltageCommand(6), transportSubsystem.setVoltageCommand(4.0).until(transportSubsystem::atSensor).andThen(transportSubsystem.setVoltageCommand(0.0))));
 //    operatorController.share().onTrue(transportSubsystem.setVoltageCommand(6.0).until(transportSubsystem::atSensor));
-    operatorController.options().whileTrue(Commands.parallel(elevatorSubsystem.setElevatorPositionCommand(Units.inchesToMeters(3)), shooterSubsystem.runVelocityCommand(3000.0/60), Commands.sequence(Commands.waitUntil(() -> shooterSubsystem.getVelocity() >= Units.rotationsToRadians(3000.0/60)), transportSubsystem.setVoltageCommand(10.0))));
+    double rpm = 10000;
+    operatorController.options().whileTrue(Commands.parallel(elevatorSubsystem.setElevatorPositionCommand(Units.inchesToMeters(0)), shooterSubsystem.runVelocityCommand(rpm/60), Commands.sequence(Commands.waitUntil(shooterSubsystem::inTolerance), transportSubsystem.setVoltageCommand(12.0))));
   }
 
   private void configureDriving() {

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java
@@ -1,6 +1,6 @@
 package frc.robot.subsystems.shooter;
 
-import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.*;
 import static frc.robot.Constants.ShooterConstants.*;
 
 import com.revrobotics.CANSparkLowLevel;
@@ -27,7 +27,9 @@ public class ShooterSubsystem extends SubsystemBase {
 
   private final SysIdRoutine shooterSysId =
       new SysIdRoutine(
-          new SysIdRoutine.Config(),
+          new SysIdRoutine.Config(
+            Volts.per(Second).of(0.5), Volts.of(10), Seconds.of(12), null
+          ),
           new SysIdRoutine.Mechanism(
               (voltage) -> setFlyVoltage(voltage.in(Volts)),
               null, // No log consumer, since data is recorded by URCL
@@ -41,7 +43,7 @@ public class ShooterSubsystem extends SubsystemBase {
 
   private final TunableTelemetryPIDController pidController =
       new TunableTelemetryPIDController("/shooter/pid", SHOOTER_PID_GAINS);
-  private final SimpleMotorFeedforward feedforward = SHOOTER_FF_GAINS.createFeedforward();
+  private SimpleMotorFeedforward feedforward = SHOOTER_FF_GAINS.createFeedforward();
 
   private final DoubleTelemetryEntry flyVoltageReq =
       new DoubleTelemetryEntry("/shooter/voltageReq", true);
@@ -120,6 +122,10 @@ public double getVelocity() {
     return flywheelEncoder.getVelocity();
   }
 
+  public boolean inTolerance() {
+    return Math.abs(getVelocity() - pidController.getSetpoint()) / (pidController.getSetpoint()) < 0.01;
+  }
+
   public Command setVoltageCommand(double voltage) {
     return this.run(() -> setVoltage(voltage));
   }
@@ -147,6 +153,9 @@ public void setFlyVoltage(double voltage) {
 
   @Override
   public void periodic() {
+    if (SHOOTER_FF_GAINS.hasChanged()) {
+      feedforward = SHOOTER_FF_GAINS.createFeedforward();
+    }
     flywheelMotor.logValues();
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java
@@ -10,6 +10,7 @@
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.revrobotics.*;
+import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
@@ -310,8 +311,8 @@ private void configSteerMotor(SwerveModuleConfiguration config) {
         MiscConstants.CONFIGURATION_ATTEMPTS);
 
     ConfigurationUtils.applyCheckRecordRev(
-        () -> steerMotor.setIdleMode(CANSparkMax.IdleMode.kBrake),
-        () -> steerMotor.getIdleMode() == CANSparkMax.IdleMode.kBrake,
+        () -> steerMotor.setIdleMode(IdleMode.kCoast),
+        () -> steerMotor.getIdleMode() == IdleMode.kCoast,
         faultRecorder.run("Idle mode"),
         MiscConstants.CONFIGURATION_ATTEMPTS);
 

diff --git a/src/main/java/frc/robot/subsystems/transport/TransportSubsystem.java b/src/main/java/frc/robot/subsystems/transport/TransportSubsystem.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import frc.robot.Constants.MiscConstants;
 import frc.robot.telemetry.types.BooleanTelemetryEntry;
 import frc.robot.telemetry.types.DoubleTelemetryEntry;
 import frc.robot.telemetry.types.EventTelemetryEntry;
@@ -23,7 +24,7 @@ public class TransportSubsystem extends SubsystemBase {
   private static final DigitalInput shooterSensor = new DigitalInput(SHOOTER_SENSOR_ID);
   public final TelemetryCANSparkMax transportMotor =
       new TelemetryCANSparkMax(
-          TRANSPORT_MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless, "/transport/motor", false);
+          TRANSPORT_MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless, "/transport/motor", MiscConstants.TUNING_MODE);
 
   public void runShooterTransportVoltage(double voltage) {
     voltageReq.append(voltage);