diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 02032f9..ceea9f5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -51,6 +51,7 @@ import frc4388.robot.subsystems.LimeLight; import frc4388.robot.subsystems.Pneumatics; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; import frc4388.robot.subsystems.Storage; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -71,6 +72,7 @@ public class RobotContainer { private final Intake m_robotIntake = new Intake(); private final Shooter m_robotShooter = new Shooter(); private final ShooterAim m_robotShooterAim = new ShooterAim(); + private final ShooterHood m_robotShooterHood = new ShooterHood(); private final Climber m_robotClimber = new Climber(); private final Leveler m_robotLeveler = new Leveler(); private final Storage m_robotStorage = new Storage(); @@ -92,6 +94,10 @@ public class RobotContainer { /* Passing Drive and Pneumatics Subsystems */ m_robotPneumatics.passRequiredSubsystem(m_robotDrive); m_robotDrive.passRequiredSubsystem(m_robotPneumatics); + + m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim); + m_robotShooterHood.passRequiredSubsystem(m_robotShooter); + m_robotShooterAim.passRequiredSubsystem(m_robotShooter); configureButtonBindings(); @@ -195,7 +201,7 @@ public class RobotContainer { //Calibrates turret and hood new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) - .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim)); + .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood)); //Prepares storage for intaking new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.LEFT_TRIGGER) diff --git a/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java index b2a54ac..4585d04 100644 --- a/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java +++ b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java @@ -13,19 +13,23 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; public class CalibrateShooter extends CommandBase { Shooter m_shooter; ShooterAim m_shooterAim; + ShooterHood m_shooterHood; + /** * Calibrates the turret by moving the hood all the way down and moving the turret all the way right, then reseting the encoders * @param shootSub The Shooter subsystem * @param aimSub The ShooterAim subsystem */ - public CalibrateShooter(Shooter shootSub, ShooterAim aimSub) { + public CalibrateShooter(Shooter shootSub, ShooterAim aimSub, ShooterHood hoodSub) { m_shooter = shootSub; m_shooterAim = aimSub; - addRequirements(m_shooter, m_shooterAim); + m_shooterHood = hoodSub; + addRequirements(m_shooter, m_shooterHood, m_shooterAim); } // Called when the command is initially scheduled. @@ -36,10 +40,10 @@ public class CalibrateShooter extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false); - m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false); - m_shooter.m_angleEncoder.setPosition(0); - m_shooter.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED); + m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false); + m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false); + m_shooterHood.m_angleEncoder.setPosition(0); + m_shooterHood.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED); m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, false); @@ -50,8 +54,8 @@ public class CalibrateShooter extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); - m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true); m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); diff --git a/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java b/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java index a91fb49..03b8d02 100644 --- a/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java @@ -14,15 +14,17 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; import frc4388.utility.controller.IHandController; public class HoldTarget extends CommandBase { - //Setup + // Setup NetworkTableEntry xEntry; ShooterAim m_shooterAim; Shooter m_shooter; + ShooterHood m_shooterHood; IHandController m_driverController; - //Aiming + // Aiming double turnAmount = 0; double xAngle = 0; double yAngle = 0; @@ -42,6 +44,7 @@ public class HoldTarget extends CommandBase { public HoldTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) { m_shooterAim = aimSubsystem; m_shooter = shooterSubsystem; + m_shooterHood = m_shooter.m_shooterHoodSubsystem; addRequirements(m_shooterAim); NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); } @@ -99,7 +102,7 @@ public class HoldTarget extends CommandBase { m_shooter.m_fireVel = fireVel; - m_shooter.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim; + m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim; } } diff --git a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java index 405530d..368f93c 100644 --- a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java @@ -10,16 +10,17 @@ package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterHood; public class HoodPositionPID extends CommandBase { - Shooter m_shooter; double firingAngle; + private ShooterHood m_shooterHood; /** * Creates a new HoodPositionPID. */ - public HoodPositionPID(Shooter subSystem) { - m_shooter = subSystem; - //addRequirements(m_shooter); + public HoodPositionPID(ShooterHood subSystem) { + m_shooterHood = subSystem; + addRequirements(m_shooterHood); } // Called when the command is initially scheduled. @@ -35,7 +36,7 @@ public class HoodPositionPID extends CommandBase { firingAngle = (-slope*m_shooter.addFireAngle())+b;*/ //SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle()); //SmartDashboard.putNumber("Fire Angle", firingAngle); - m_shooter.runAngleAdjustPID(firingAngle); + m_shooterHood.runAngleAdjustPID(firingAngle); } // Called once the command ends or is interrupted. @@ -46,7 +47,7 @@ public class HoodPositionPID extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - double encoderPos = m_shooter.m_angleAdjustMotor.getEncoder().getPosition(); + double encoderPos = m_shooterHood.m_angleAdjustMotor.getEncoder().getPosition(); if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){ return false; } diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java index 2adcbda..daf6961 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import frc4388.robot.commands.storage.StorageRun; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; import frc4388.robot.subsystems.Storage; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -25,9 +26,11 @@ public class ShootFireGroup extends ParallelRaceGroup { * @param m_storage The Storage subsytem */ public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) { + ShooterHood m_shooterHood = m_shooter.m_shooterHoodSubsystem; + addCommands( new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())), - new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())), + new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle())), new HoldTarget(m_shooter, m_shooterAim), new StorageRun(m_storage) ); diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java index f806eb4..2cde87c 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java @@ -11,18 +11,22 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterHood; public class ShooterVelocityControlPID extends CommandBase { Shooter m_shooter; double m_targetVel; double m_actualVel; + private ShooterHood m_shooterHood; + /** * Runs the drum at a velocity * @param subsystem The Shooter subsytem */ - public ShooterVelocityControlPID(Shooter subsystem) { + public ShooterVelocityControlPID(Shooter subsystem, ShooterHood subHood) { m_shooter = subsystem; - addRequirements(m_shooter); + m_shooterHood = subHood; + addRequirements(m_shooter, m_shooterHood); } // Called when the command is initially scheduled. @@ -35,7 +39,7 @@ public class ShooterVelocityControlPID extends CommandBase { @Override public void execute() { m_shooter.runDrumShooterVelocityPID(/*m_shooter.addFireVel()*/13000); - m_shooter.runAngleAdjustPID(m_shooter.addFireAngle()); + m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()); //SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel()); //SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle()); } diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index 857e003..d5c638e 100644 --- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; import frc4388.utility.controller.IHandController; public class TrackTarget extends CommandBase { @@ -33,6 +34,7 @@ public class TrackTarget extends CommandBase { public double m_hoodTrim; public double m_turretTrim; + private ShooterHood m_shooterHood; /** * Uses the Limelight to track the target @@ -42,6 +44,7 @@ public class TrackTarget extends CommandBase { public TrackTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) { m_shooterAim = aimSubsystem; m_shooter = shooterSubsystem; + m_shooterHood = m_shooter.m_shooterHoodSubsystem; addRequirements(m_shooterAim); NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); } @@ -97,7 +100,7 @@ public class TrackTarget extends CommandBase { m_shooter.m_fireVel = fireVel; - m_shooter.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim; + m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim; } } diff --git a/src/main/java/frc4388/robot/subsystems/CANDigitalInput.java b/src/main/java/frc4388/robot/subsystems/CANDigitalInput.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index e16cabe..00e54d7 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -10,15 +10,6 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.revrobotics.CANDigitalInput; -import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; -import com.revrobotics.CANEncoder; -import com.revrobotics.CANPIDController; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMax.SoftLimitDirection; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.ControlType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -31,13 +22,8 @@ import frc4388.utility.controller.IHandController; public class Shooter extends SubsystemBase { public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); - public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); - - CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); - public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; - public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS; public static Shooter m_shooter; public static IHandController m_controller; @@ -48,10 +34,11 @@ public class Shooter extends SubsystemBase { public boolean velReached; public double m_fireVel; - public double m_fireAngle; - CANDigitalInput m_hoodUpLimit, m_hoodDownLimit; public Trims shooterTrims; + + public ShooterHood m_shooterHoodSubsystem; + public ShooterAim m_shooterAimSubsystem; /* * Creates a new Shooter subsystem, with the drum shooter and the angle adjsuter. @@ -64,7 +51,6 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.configFactoryDefault(); m_shooterFalcon.setNeutralMode(NeutralMode.Coast); m_shooterFalcon.setInverted(true); - m_angleAdjustMotor.setIdleMode(IdleMode.kBrake); m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalcon.configClosedloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); setShooterGains(); @@ -87,17 +73,6 @@ public class Shooter extends SubsystemBase { //SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10)); //SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5)); //SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30)); - - m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_hoodUpLimit.enableLimitSwitch(true); - m_hoodDownLimit.enableLimitSwitch(true); - - m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); - m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT); - m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT); - } @Override @@ -107,7 +82,6 @@ public class Shooter extends SubsystemBase { SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity()); SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel); - SmartDashboard.putNumber("Fire Angle CSV", m_fireAngle); //SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition()); } @@ -118,13 +92,18 @@ public class Shooter extends SubsystemBase { } } + /** + * Passes subsystem needed. + * @param subsystem Subsystem needed. + */ + public void passRequiredSubsystem(ShooterHood subsystem0, ShooterAim subsystem1) { + m_shooterHoodSubsystem = subsystem0; + m_shooterAimSubsystem = subsystem1; + } + public double addFireVel() { return m_fireVel; } - public double addFireAngle() { - return m_fireAngle; - } - /** * Runs drum shooter motor. @@ -145,20 +124,6 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); } - /* Angle Adjustment PID Control */ - public void runAngleAdjustPID(double targetAngle) - { - // Set PID Coefficients - m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP); - m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI); - m_angleAdjustPIDController.setD(m_angleAdjustGains.m_kD); - m_angleAdjustPIDController.setIZone(m_angleAdjustGains.m_kIzone); - m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF); - m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput); - - m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); - } - /** * Runs drum shooter velocity PID. * @param falcon Motor to use @@ -168,12 +133,4 @@ public class Shooter extends SubsystemBase { System.out.println("dddddddddddddddddddddddd" + targetVel); m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID } - - public void resetGyroAngleAdj(){ - m_angleEncoder.setPosition(0); - } - - public double getAnglePosition(){ - return m_angleEncoder.getPosition(); - } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java index 07b04f5..086103c 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -22,14 +22,15 @@ import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; public class ShooterAim extends SubsystemBase { + public Shooter m_shooterSubsystem; + public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS; CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit; - // Configure PID Controllers - CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController(); - public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder(); - + // Configure PID Controllers + CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController(); + public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder(); /** * Creates a subsytem for the turret aiming @@ -49,6 +50,19 @@ public class ShooterAim extends SubsystemBase { m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_LEFT_SOFT_LIMIT); } + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Passes subsystem needed. + * @param subsystem Subsystem needed. + */ + public void passRequiredSubsystem(Shooter subsystem) { + m_shooterSubsystem = subsystem; + } + public void runShooterWithInput(double input) { m_shooterRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER); } @@ -71,17 +85,13 @@ public class ShooterAim extends SubsystemBase { m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition); } - public void resetGyroShooterRotate() - { - m_shooterRotateEncoder.setPosition(0); - } + public void resetGyroShooterRotate() + { + m_shooterRotateEncoder.setPosition(0); + } - public double getShooterRotatePosition(){ - return m_shooterRotateMotor.getEncoder().getPosition(); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run + public double getShooterRotatePosition() + { + return m_shooterRotateMotor.getEncoder().getPosition(); } } diff --git a/src/main/java/frc4388/robot/subsystems/ShooterHood.java b/src/main/java/frc4388/robot/subsystems/ShooterHood.java new file mode 100644 index 0000000..ae0ee66 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/ShooterHood.java @@ -0,0 +1,95 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the 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.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.Gains; +import frc4388.utility.controller.IHandController; + +public class ShooterHood extends SubsystemBase { + public Shooter m_shooterSubsystem; + + public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + + public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS; + + public CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); + public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); + + public double m_fireAngle; + public CANDigitalInput m_hoodUpLimit, m_hoodDownLimit; + + /** + * Creates a new ShooterHood. + */ + public ShooterHood() { + m_angleAdjustMotor.setIdleMode(IdleMode.kBrake); + + m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_hoodUpLimit.enableLimitSwitch(true); + m_hoodDownLimit.enableLimitSwitch(true); + + m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT); + m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + SmartDashboard.putNumber("Fire Angle CSV", m_fireAngle); + } + + /** + * Passes subsystem needed. + * @param subsystem Subsystem needed. + */ + public void passRequiredSubsystem(Shooter subsystem) { + m_shooterSubsystem = subsystem; + } + + public double addFireAngle() { + return m_fireAngle; + } + + /* Angle Adjustment PID Control */ + public void runAngleAdjustPID(double targetAngle) + { + // Set PID Coefficients + m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP); + m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI); + m_angleAdjustPIDController.setD(m_angleAdjustGains.m_kD); + m_angleAdjustPIDController.setIZone(m_angleAdjustGains.m_kIzone); + m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF); + m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput); + + m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); + } + + public void resetGyroAngleAdj(){ + m_angleEncoder.setPosition(0); +} + + public double getAnglePosition(){ + return m_angleEncoder.getPosition(); + } +}