drum pid + tentative shooting values + get voltage

src/main/java/frc4388/robot/subsystems/BoomBoom.java

@@ -37,6 +37,7 @@ public class BoomBoom extends SubsystemBase {
 
   double velP;
   double input;
+  public double pidOffset = 0;
 
   public boolean m_isDrumReady = false;
   public double m_fireVel;
@@ -56,6 +57,7 @@ public class BoomBoom extends SubsystemBase {
   public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
     m_shooterFalconLeft = shooterFalconLeft;
     m_shooterFalconRight = shooterFalconRight;
+    setShooterGains();
 
     try {
       // This is a helper class that allows us to read a CSV file into a Java array.
@@ -183,6 +185,9 @@ public class BoomBoom extends SubsystemBase {
   public void periodic() {
     // This method will be called once per scheduler run
     // speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0);
+    SmartDashboard.putNumber("Shooter Current", getCurrent());
+    SmartDashboard.putNumber("Shooter Voltage",  m_shooterFalconLeft.getMotorOutputVoltage());
+    SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
   }
 
   public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) {
@@ -211,7 +216,8 @@ public class BoomBoom extends SubsystemBase {
   }
 
   public void runDrumShooterVelocityPID(double targetVel) {
-    m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init
+    SmartDashboard.putNumber("Target Drum Velocity", 10000 + pidOffset);
+    m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel + pidOffset); // Init
     
     // New BoomBoom controller stuff
     // Controls a motor with the output of the BangBang controller
@@ -223,6 +229,10 @@ public class BoomBoom extends SubsystemBase {
     // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel));
   }
 
+  public void updateOffset(double change) {
+    pidOffset = pidOffset + change;
+  }
+
   public void increaseSpeed(double amount)
   {
     speed2 = speed2 + amount;

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

@@ -112,9 +112,9 @@ public class SwerveDrive extends SubsystemBase {
     double ySpeedMetersPerSecond = speed.getY();
     chassisSpeeds = fieldRelative
         ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
-            -rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
+            -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
         : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond,
-            -rot * SwerveDriveConstants.ROTATION_SPEED);
+            -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust);
     SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds);
     setModuleStates(states);
   }
@@ -263,4 +263,8 @@ public class SwerveDrive extends SubsystemBase {
   public double getCurrent(){
     return m_leftFront.getCurrent() + m_rightFront.getCurrent() + m_rightBack.getCurrent() + m_leftBack.getCurrent();
   }
+
+  public double getVoltage(){
+    return m_leftFront.getVoltage() + m_rightFront.getVoltage() + m_rightBack.getVoltage() + m_leftBack.getVoltage();
+  }
 }

src/main/java/frc4388/robot/subsystems/SwerveModule.java

@@ -195,4 +195,8 @@ public class SwerveModule extends SubsystemBase {
   public double getCurrent(){
     return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
   }
+
+  public double getVoltage(){
+    return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
+  }
 }

src/main/java/frc4388/robot/subsystems/Turret.java

@@ -7,7 +7,7 @@ package frc4388.robot.subsystems;
 import com.revrobotics.CANSparkMax.ControlType;
 import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMax.SoftLimitDirection;
-import com.revrobotics.SparkMaxRelativeEncoder.Type;
+
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkMaxLimitSwitch;
@@ -60,8 +60,6 @@ public class Turret extends SubsystemBase {
 
     m_boomBoomRotateMotor.setInverted(true);
 
-    // m_boomBoomRotateMotor.getAlternateEncoder(4096).setPosition(0);
-
     m_boomBoomRotatePIDController.setP(m_shooterTGains.kP);
     m_boomBoomRotatePIDController.setI(m_shooterTGains.kI);
     m_boomBoomRotatePIDController.setD(m_shooterTGains.kD);
@@ -104,12 +102,10 @@ public class Turret extends SubsystemBase {
   }
 
   public double getboomBoomRotatePosition() {
-    // return 0.0;
     return m_boomBoomRotateEncoder.getPosition();
   }
 
   public double getBoomBoomAngleDegrees() {
-    // return 0.0;
     return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360
         / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT;
   }

src/main/java/frc4388/robot/subsystems/VisionOdometry.java

@@ -112,7 +112,7 @@ public class VisionOdometry extends SubsystemBase {
     }
 
     guess = correctGuessForCenter(guess, m_shooter.getBoomBoomAngleDegrees());
-    guess = correctGuessForGyro(guess, m_drive.getRegGyro().getDegrees());
+    // guess = correctGuessForGyro(guess, m_drive.getRegGyro().getDegrees());
 
     SmartDashboard.putNumber("Vision ODO x: ", guess.x);
     SmartDashboard.putNumber("Vision ODO y: ", guess.y);