Things Happened

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

@@ -88,7 +88,9 @@ public final class Constants {
     public static final int SHOOTER_SLOT_IDX = 0;
     public static final int SHOOTER_PID_LOOP_IDX = 1;
     public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5);
-    
+    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;
     
   }
 }

src/main/java/frc4388/robot/RobotMap.java

@@ -97,9 +97,8 @@ public class RobotMap {
   /*Boom Boom Subsystem*/ 
   public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
   public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
-  void ConfigureShooterMotorControllers()
-  {
-    
+  void ConfigureShooterMotorControllers() {
+
   }
 
 

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

@@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
+import edu.wpi.first.math.controller.BangBangController;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.utility.ShooterTables;
@@ -65,5 +66,13 @@ public void runDrumShooter(double speed) {
   public void runDrumShooterVelocityPID(double targetVel) {
     m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init
     m_shooterFalconRight.follow(m_shooterFalconLeft);
-    }
+    * // New BoomBoom controller stuff 
+    BangBangController controller = new BangBangController();
+    //Controls a motor with the output of the BangBang controller
+    //Controls a motor with the output of the BangBang conroller and a feedforward
+    //Shrinks the feedforward slightly to avoid over speeding the shooter
+    motor.set(controller.calculate(encoder.getRate(), setpoint) + 0.9 * feedforward.calculate(setpoint)); */
+  
+  
+  }
 }

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

@@ -3,15 +3,74 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc4388.robot.subsystems;
+import com.revrobotics.CANDigitalInput;
+import com.revrobotics.CANEncoder;
+import com.revrobotics.CANPIDController;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.ControlType;
+import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMax.SoftLimitDirection;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.robot.Constants.ShooterConstants;
+import frc4388.utility.Gains;
 
 public class Hood extends SubsystemBase {
+  public Shooter m_shooterSubsystem;
+  
+  public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
+  
+  public static Gains m_angleGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
+
+  public CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
+  public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
+
+  public boolean m_isHoodReady = false;
+  
   /** Creates a new Hood. */
-  public Hood() {}
+  public Hood() {
+    m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
+
+    m_hoodUpLimit = m_angleAdjusterMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_hoodDownLimit = m_angleAdjusterMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen);
+    m_hoodUpLimit.enableLimitSwitch(true);
+    m_hoodDownLimit.enableLimitSwitch(true);
+  }
+    
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
   }
+  public void runAngleAdjustPID(double targetAngle) 
+  {
+    //Set PID Coefficients
+    m_angleAdjustPIDController.setP(m_angleAdjusterGains.m_kP);
+    m_angleAdjustPIDController.setI(m_angleAdjusterGains.m_kI);
+    m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD);
+    m_angleAdjusterPIDController.setIZone(m_angleAdjustGains.m_IZone);
+    m_angleAdjusterPIDController.setFF(m_angleAdjustGains.m_kF);
+    m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput);
+
+    m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
+  }
+
+  public void runHood(double input) {
+    m_angleAdjusterMotor.set(input);
+  }
+
+  public void resetGyroAngleAdj(){
+    m_angleEncoder.setPosition(0);
+  }
+
+  public double getAnglePosition(){
+    return m_angleEncoder.getPosition();
+  }
+
+  public double getAnglePositionDegrees(){
+    return ((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90;
+  }
 }