Finished up shooter (still needs review), started working on Vision

src/main/java/frc4388/robot/Constants.java

@@ -82,6 +82,10 @@ public final class Constants {
     public static final int XBOX_DRIVER_ID = 0;
     public static final int XBOX_OPERATOR_ID = 1;
   }
+
+
+
+
   public static final class ShooterConstants {
     /* PID Constants Shooter */
     public static final int SHOOTER_TIMEOUT_MS = 30;
@@ -91,6 +95,10 @@ public final class Constants {
     public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0);
     public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
     public static final double SHOOTER_TURRET_MIN = -1.0;
-    
+    public static final int SHOOTER_FALCON_BALLER_ID =; //unknown value, fix later//
+    public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID =; //"//
+  
+    public static final Gains DRUM_SHOOTER_GAINS = new Gains(x,y,z,a,b);//x,y,z,a,b are not actual values, fix laterS
+
   }
 }

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

@@ -8,16 +8,42 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
+import edu.wpi.first.wpilibj.SmartDashboard.SmartDashboard;
 import edu.wpi.first.math.controller.BangBangController;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.utility.ShooterTables;
+import frc4388.utility.Gains;
+import frc4388.utility.Trims;
+import frc4388.utility.controller.IHandController;
 
 public class BoomBoom extends SubsystemBase {
-public WPI_TalonFX m_shooterFalconLeft;
-public WPI_TalonFX m_shooterFalconRight;
+public WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID);
+public WPI_TalonFX m_shooterFalconRight= new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_FOLLOWER_ID);
 public ShooterTables m_shooterTable;
+public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
+public static BoomBoom m_boomBoom;
+public static IHandController m_driverController; //not sure if driverController in 2022 = m_controller in 2020
 
+double velP;
+double input;
+
+public boolean m_isDrumReady = false;
+public double m_fireVel;
+
+public Trims shooterTrims;
+
+public Hood m_hoodSubsystem;
+public Turret m_turretSubsystem;
+
+/*
+* Creates new BoomBoom subsystem, has drum shooter and angle adjuster
+*/
+public BoomBoom(){
+//Testing purposes resetting gyros
+//resetGryoAngleADj();
+shooterTrims = new Trims(0,0);
+}
   /** Creates a new BoomBoom. */
   public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
   m_shooterFalconLeft = shooterFalconLeft;
@@ -56,12 +82,42 @@ public ShooterTables m_shooterTable;
   public void periodic() {
     // This method will be called once per scheduler run
     // Abhi was here 
+try {
+  // SmartDashboard.putNumber("Drum Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
+
+  // SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel);
+
+  // SmartDashboard.putNumber("Shooter Temp C", m_shooterFalconLeft.getTemperature()); //all these values should be "defined" elsewhere, fix this
+
+  // SmartDashboard.putNumber("Shooter Current", m_shooterFalconLeft.getSupplyCurrent());
+
+  // SmartDashboard.putNumber("Drum Ready", m_isDrumReady);
+} catch (Exception e) {
+  //TODO: handle exception
+}
+
+  }
+
+  public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) {
+    m_hoodSubsystem = subsystem0;
+    m_turretSubsystem = subsystem1;
+  }
+
+  public double addFireVel(){
+    return m_fireVel;
   }
 public void runDrumShooter(double speed) {
     m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);
     m_shooterFalconRight.follow(m_shooterFalconLeft);
   }    
 
+public void setShooterGains() {
+  m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
+  m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
+  m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
+  m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
+  m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
+}
 
   public void runDrumShooterVelocityPID(double targetVel) {
     m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init

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

@@ -6,10 +6,40 @@ package frc4388.robot.subsystems;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-public class Vision extends SubsystemBase {
-  /** Creates a new Vision. */
-  public Vision() {}
 
+
+
+public class Vision extends SubsystemBase {
+//setup
+  Turret m_turret;
+BoomBoom m_boomBoom;
+Hood m_hood;
+
+NetworkTableEntry xEntry;
+IHandController m_driverController;
+//Aiming
+double turnAmount = 0;
+double xAngle = 0;
+double yAngle = 0;
+double target = 0;
+public double distance;
+public double realDistance;
+public static double fireVel;
+public static double fireAngle;
+
+public double m_hoodTrim;
+public double m_turretTrim;
+
+LimeLight m_limeLight;
+
+public TrackTarget(ShooterAim aimSubsystem, LimeLight limeLight) {
+  m_turret = turretSubsystem;
+  m_boomBoom = m_turret.m_turretSubsystem;
+  m_hood = m_boomBoom.m_hoodSubsystem;
+  m_limeLight = limeLight;
+  addRequirements(m_turret);
+
+}
   @Override
   public void periodic() {
     // This method will be called once per scheduler run