diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 6879b06..531ffb3 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -32,8 +32,6 @@ double input; public boolean m_isDrumReady = false; public double m_fireVel; - - public Hood m_hoodSubsystem; public Turret m_turretSubsystem; @@ -43,7 +41,7 @@ public Turret m_turretSubsystem; public BoomBoom(){ //Testing purposes resetting gyros //resetGryoAngleADj(); - +//shooterTrims = new Trims(0,0); } /** Creates a new BoomBoom. */ public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { @@ -128,7 +126,7 @@ public void setShooterGains() { //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 - m_shooterFalconLeft.set(controller.calculate(Encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel); + m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); } diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 19ab8f3..de78848 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -3,11 +3,12 @@ // the WPILib BSD license file in the root directory of this project. package frc4388.robot.subsystems; -import com.revrobotics.CANEncoder; -import com.revrobotics.CANPIDController; + import com.revrobotics.CANSparkMax; -import com.revrobotics.ControlType; -import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.CANSparkMaxLowLevel.MotorType; @@ -20,23 +21,28 @@ import frc4388.utility.Gains; public class Hood extends SubsystemBase { public BoomBoom 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 CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + public SparkMaxLimitSwitch m_hoodUpLimitSwitch; + public SparkMaxLimitSwitch m_hoodDownLimitSwitch; + public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS; + public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder(); - public CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); - public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); - - public boolean m_isHoodReady = false; + public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); + + //public boolean m_isHoodReady = false; + +public double m_fireAngle; + + /** Creates a new Hood. */ public Hood() { - m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); + 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); + m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_hoodUpLimitSwitch.enableLimitSwitch(true); + m_hoodDownLimitSwitch.enableLimitSwitch(true); } @@ -47,15 +53,16 @@ public class Hood extends SubsystemBase { public void runAngleAdjustPID(double targetAngle) { //Set PID Coefficients - m_angleAdjustPIDController.setP(m_angleAdjusterGains.m_kP); - m_angleAdjustPIDController.setI(m_angleAdjusterGains.m_kI); + m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP); + m_angleAdjusterPIDController.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.setIZone(m_angleAdjusterGains.m_kIzone); + m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF); m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput); - m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); + m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); } + public void runHood(double input) { m_angleAdjusterMotor.set(input); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 604bdae..4a1e119 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -9,6 +9,7 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Turret; @@ -35,7 +36,7 @@ public static double fireAngle; public double m_hoodTrim; public double m_turretTrim; - +public double m_fireAngle; public Vision(Turret aimSubsystem, BoomBoom boomBoom) { m_turret = aimSubsystem; m_boomBoom = boomBoom;