diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5eda132..80a21ee 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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; } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index e917689..225a8b4 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/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() { + } diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index e4d8825..ceb7569 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/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)); */ + + + } } diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 4cc9392..04b2348 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/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; + } }