diff --git a/src/main/java/frc/robot/Commands/AmpHandoff.java b/src/main/java/frc/robot/Commands/AmpHandoff.java deleted file mode 100644 index 60da463..0000000 --- a/src/main/java/frc/robot/Commands/AmpHandoff.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc.robot.Commands; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.robot.subsystems.Indexer; -import frc.robot.subsystems.Shooter; - -public class AmpHandoff extends ParallelCommandGroup{ - - AmpHandoff(Indexer indexer, Shooter shooter){ - //addCommands(indexer.shootNote(() -> 1), shooter.ampHandoff()); - } - -} diff --git a/src/main/java/frc/robot/Commands/SpeakerShot.java b/src/main/java/frc/robot/Commands/SpeakerShot.java deleted file mode 100644 index 2b0a1e4..0000000 --- a/src/main/java/frc/robot/Commands/SpeakerShot.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc.robot.Commands; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.robot.constants.ShooterConstants; -import frc.robot.subsystems.Indexer; -import frc.robot.subsystems.Shooter; - -public class SpeakerShot extends ParallelCommandGroup{ - - SpeakerShot(Indexer indexer, Shooter shooter){ - //addCommands(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0, 0), indexer.shootNote(() -> 1.0)); - } -} diff --git a/src/main/java/frc/robot/Commands/autoIntaking.java b/src/main/java/frc/robot/Commands/autoIntaking.java deleted file mode 100644 index bcdd402..0000000 --- a/src/main/java/frc/robot/Commands/autoIntaking.java +++ /dev/null @@ -1,32 +0,0 @@ -package frc.robot.Commands; - -import java.util.function.BooleanSupplier; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.IntakeConstants; -import frc.robot.subsystems.Intake; - -public class autoIntaking extends Command{ - private Intake m_intake; - private BooleanSupplier m_beamBreak; - - public autoIntaking(Intake intake, BooleanSupplier beamBreak){ - m_intake = intake; - m_beamBreak = beamBreak; - - addRequirements(intake); - } - - @Override - public void execute(){ - m_intake.intakeControl(() -> IntakeConstants.kDownAngle, () -> 1.0); - } - - @Override - public boolean isFinished(){ - if(!m_beamBreak.getAsBoolean()){ - return true; - }else {return false;} - } - -} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ab6cda3..3e3b995 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,7 +33,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Commands.autoIntaking; -import frc.robot.constants.IntakeConstants; +import frc.robot.constants.IntakeRollerConstants; import frc.robot.constants.OIConstants; import frc.robot.constants.PhotonConstants; import frc.robot.constants.ShooterConstants; @@ -160,7 +160,7 @@ public class RobotContainer { NamedCommands.registerCommand( "Auto Intake", intake.intakeControl( - () -> IntakeConstants.kDownAngle, + () -> IntakeRollerConstants.kDownAngle, () -> 1.0 ).alongWith( indexer.advanceNote() @@ -237,9 +237,9 @@ public class RobotContainer { intake.intakeControl( () -> { if (intakeDown && indexer.getBeamBreak()) { - return IntakeConstants.kDownAngle; + return IntakeRollerConstants.kDownAngle; } else { - return IntakeConstants.kUpAngle; + return IntakeRollerConstants.kUpAngle; } }, () -> { @@ -420,7 +420,7 @@ public class RobotContainer { driver.povDown().whileTrue(indexer.shootNote(() -> -1.0)); - operator.leftTrigger().whileTrue(Commands.parallel( indexer.shootNote(() -> -1), intake.intakeControl(() -> IntakeConstants.kDownAngle, () -> -1.0))); + operator.leftTrigger().whileTrue(Commands.parallel( indexer.shootNote(() -> -1), intake.intakeControl(() -> IntakeRollerConstants.kDownAngle, () -> -1.0))); operator.start().toggleOnTrue(intake.manualPivot(() -> -operator.getRightY()*0.2, () -> driver.getLeftTriggerAxis())); diff --git a/src/main/java/frc/robot/constants/ClimberConstants.java b/src/main/java/frc/robot/constants/ClimberConstants.java index 0fa8496..6702240 100644 --- a/src/main/java/frc/robot/constants/ClimberConstants.java +++ b/src/main/java/frc/robot/constants/ClimberConstants.java @@ -1,6 +1,6 @@ package frc.robot.constants; public class ClimberConstants { - public static final int kClimberMotor1CANID = 14; - public static final int kClimberMotor2CANID = 15; + public static final int kMotor1CANID = 14; + public static final int kMotor2CANID = 15; } diff --git a/src/main/java/frc/robot/constants/HoodConstants.java b/src/main/java/frc/robot/constants/HoodConstants.java new file mode 100644 index 0000000..f4be7dd --- /dev/null +++ b/src/main/java/frc/robot/constants/HoodConstants.java @@ -0,0 +1,42 @@ +package frc.robot.constants; + +public class HoodConstants { + public enum HoodPositions { + kLoadAngle(Math.toRadians(-20.0)), + KAmpAngle(Math.toRadians(105.0)); + + private double angle; + + private HoodPositions(double angle) { + this.angle = angle; + } + + public double getAngle() { + return angle; + } + } + + + public static final int kMotorCANID = 16; + + public static final double kP = 3.0; + public static final double kI = 0.0; + public static final double kD = 0.0; + + public static final double kLoadAngle = Math.toRadians(-20.0); + public static final double KAmpAngle = Math.toRadians(105.0); + + public static final double kEncoderStepConversion = 1/(40.0*(28.0/15.0)); + + public static final double kSFF = 0.0; + public static final double kGFF = 0.33; + public static final double kVFF = 1.44; + + public static final double kMaxVelocity = 0.0; + public static final double kMaxAcceleration = 0.0; + + public static final int kMotorCurrentLimit = 50; + + public static final int kEncoderChannelA = 0; + public static final int kEncoderChannelB = 1; +} diff --git a/src/main/java/frc/robot/constants/IndexerConstants.java b/src/main/java/frc/robot/constants/IndexerConstants.java index c280aa0..321b6c1 100644 --- a/src/main/java/frc/robot/constants/IndexerConstants.java +++ b/src/main/java/frc/robot/constants/IndexerConstants.java @@ -1,7 +1,9 @@ package frc.robot.constants; public class IndexerConstants { - public static final int kIndexerID = 13; + public static final int kMotorCANID = 13; - public static final int kIndexerBeamBreakChannel = 2; + public static final int kMotorCurrentLimit = 40; + + public static final int kBeamBreakChannel = 2; } diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakePivotConstants.java similarity index 89% rename from src/main/java/frc/robot/constants/IntakeConstants.java rename to src/main/java/frc/robot/constants/IntakePivotConstants.java index 5d6f7ce..2c4f01f 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakePivotConstants.java @@ -1,8 +1,7 @@ package frc.robot.constants; -public class IntakeConstants { +public class IntakePivotConstants { public static final int kIntakePivotID = 10; - public static final int kIntakeRollerID = 12; public static final double kIntakePivotConversionFactor = (20.0*(28.0/15.0)); @@ -19,5 +18,4 @@ public class IntakeConstants { public static final double kStartingAngle = Math.toRadians(95.0); public static final double kUpAngle = Math.toRadians(90.0); public static final double kDownAngle = Math.toRadians(-34.0); - } diff --git a/src/main/java/frc/robot/constants/IntakeRollerConstants.java b/src/main/java/frc/robot/constants/IntakeRollerConstants.java new file mode 100644 index 0000000..6f1cca8 --- /dev/null +++ b/src/main/java/frc/robot/constants/IntakeRollerConstants.java @@ -0,0 +1,5 @@ +package frc.robot.constants; + +public class IntakeRollerConstants { + public static final int kMotorCANID = 12; +} diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index eafe46e..33038ac 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -1,34 +1,10 @@ package frc.robot.constants; public class ShooterConstants { - - public static final int kTopShooterID = 9; - public static final int kBottomShooterID = 11; - public static final int kShooterPivotID = 16; + public static final int kTopMotorCANID = 9; + public static final int kBottomMotorCANID = 11; - public static final double kShooterP = 0.0; - public static final double kShooterI = 0.0; - public static final double kShooterD = 0.0; + public static final int kMotorCurrentLimit = 40; - public static final double kShooterFF = 0.0; - - public static final double kShooterPivotP = 3.0; - public static final double kShooterPivotI = 0.0; - public static final double kShooterPivotD = 0.0; - - public static final double kShooterLoadAngle = Math.toRadians(-20.0); - public static final double kShooterAmpAngle = Math.toRadians(105.0); - - public static final double kPivotConversion = 1/(40.0*(28.0/15.0)); - - public static final double kSShooterPivotFF = 0.0; - public static final double kGShooterPivotFF = 0.33; - public static final double kVShooterPivotFF = 1.44; - - public static final double kMaxPivotSpeed = 0.0; - public static final double kMaxPivotAcceleration = 0.0; - - public static final int kShooterEncoderChannelA = 0; - public static final int kShooterEncoderChannelB = 1; - public static final int kShooterBeamBreakChannel = 3; + public static final int kBeamBreakChannel = 3; } diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index c88641c..4430293 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -8,34 +8,23 @@ import com.ctre.phoenix.motorcontrol.can.VictorSPX; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ClimberConstants; -import frc.robot.constants.ShooterConstants; public class Climber extends SubsystemBase { private VictorSPX motor1; private VictorSPX motor2; - private DoubleSupplier shooterAngle; - - public Climber(DoubleSupplier shooterAngle) { - motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID); - motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID); + public Climber() { + motor1 = new VictorSPX(ClimberConstants.kMotor1CANID); + motor2 = new VictorSPX(ClimberConstants.kMotor2CANID); - motor2.follow(motor1); motor1.setInverted(true); motor2.setInverted(true); - this.shooterAngle = shooterAngle; } public Command setSpeedWithSupplier(DoubleSupplier speed) { return run(() -> { - if(shooterAngle.getAsDouble() > ShooterConstants.kShooterLoadAngle){ - - motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble()); - - }else{ - motor1.set(VictorSPXControlMode.PercentOutput, 0.0); - } + motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble()); }); } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 13c8c3f..f132d90 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -20,10 +20,8 @@ import java.util.function.DoubleSupplier; import com.kauailabs.navx.frc.AHRS; import com.pathplanner.lib.auto.AutoBuilder; -import edu.wpi.first.wpilibj.ADXRS450_Gyro; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.SPI; -import edu.wpi.first.wpilibj.interfaces.Gyro; import frc.robot.constants.AutoConstants; import frc.robot.constants.DrivetrainConstants; import frc.robot.utilities.MAXSwerveModule; diff --git a/src/main/java/frc/robot/subsystems/Hood.java b/src/main/java/frc/robot/subsystems/Hood.java new file mode 100644 index 0000000..eedcdd6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Hood.java @@ -0,0 +1,109 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkLowLevel.MotorType; + +import java.util.function.DoubleSupplier; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkBase.IdleMode; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.HoodConstants; +import frc.robot.constants.HoodConstants.HoodPositions; + +public class Hood extends SubsystemBase { + private CANSparkMax shooterPivot; + + private Encoder pivotEncoder; + + private PIDController shooterPivotPID; + private ArmFeedforward shooterPivotFF; + + private HoodPositions targetPosition; + + private boolean pidControl; + + public Hood() { + shooterPivot = new CANSparkMax(HoodConstants.kMotorCANID, MotorType.kBrushless); + shooterPivot.setInverted(true); + + shooterPivot.setSmartCurrentLimit(HoodConstants.kMotorCurrentLimit); + + pivotEncoder = new Encoder(0, 1); + pivotEncoder.setDistancePerPulse(HoodConstants.kEncoderStepConversion); + + shooterPivot.burnFlash(); + + shooterPivotPID = new PIDController( + HoodConstants.kP, + HoodConstants.kI, + HoodConstants.kD + ); + + shooterPivotFF = new ArmFeedforward( + HoodConstants.kSFF, + HoodConstants.kGFF, + HoodConstants.kVFF + ); + + shooterPivot.setIdleMode(IdleMode.kBrake); + + targetPosition = HoodPositions.kLoadAngle; + + pidControl = true; + } + + @Override + public void periodic() { + if(pidControl) { + shooterPivot.setVoltage( + shooterPivotPID.calculate( + getShooterAngle(), + targetPosition.getAngle() + ) + shooterPivotFF.calculate( + targetPosition.getAngle(), + 0.0 + ) + ); + } + } + + public Command setHoodAngle(HoodPositions newPosition) { + return runOnce(() -> { + pidControl = true; + + targetPosition = newPosition; + }); + } + + public Command setSpeed(DoubleSupplier speed) { + return runOnce(() -> { pidControl = false; }).andThen(run(() -> { + shooterPivot.set(speed.getAsDouble()); + })); + } + + public Command climbState() { + return runOnce(() -> { + pidControl = false; + shooterPivot.setIdleMode(IdleMode.kCoast); + }).andThen(run(() -> { + shooterPivot.set(0); + })).finallyDo(() -> { + // finallyDo will reset break mode if we back out of climbing and interrupt + // the climbState command with something else + shooterPivot.setIdleMode(IdleMode.kBrake); + }); + } + + public Command stop() { + return setSpeed(() -> 0); + } + + public double getShooterAngle() { + return pivotEncoder.getDistance() + HoodPositions.kLoadAngle.getAngle(); + } +} diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index 8d07d53..4da73da 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -13,48 +13,52 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.IndexerConstants; public class Indexer extends SubsystemBase{ - private CANSparkMax indexerMotor; + private DigitalInput indexerBeamBreak; - public Indexer(){ - indexerMotor = new CANSparkMax(IndexerConstants.kIndexerID, MotorType.kBrushed); + private boolean autoIndex; - indexerMotor.setSmartCurrentLimit(40); + public Indexer(){ + indexerMotor = new CANSparkMax(IndexerConstants.kMotorCANID, MotorType.kBrushed); + + indexerMotor.setSmartCurrentLimit(IndexerConstants.kMotorCurrentLimit); indexerMotor.setIdleMode(IdleMode.kBrake); indexerMotor.burnFlash(); - indexerBeamBreak = new DigitalInput(IndexerConstants.kIndexerBeamBreakChannel); + indexerBeamBreak = new DigitalInput(IndexerConstants.kBeamBreakChannel); + + autoIndex = false; } - public Command autoIndexing(){ - return run(() -> { - if(!indexerBeamBreak.get()){ - indexerMotor.set(0.75); - }else if(indexerBeamBreak.get()){ - indexerMotor.set(0.0); - } - - }); - } - - public Command advanceNote(){ - return run(() -> { - if(indexerBeamBreak.get()){ - indexerMotor.set(1); - }else{ - indexerMotor.set(0.0); + @Override + public void periodic() { + if(autoIndex) { + if(!indexerBeamBreak.get()) { + indexerMotor.set(.75); + } else { + indexerMotor.set(0); } - }); - } - - public Command shootNote(DoubleSupplier indexerSpeed){ - return run(() -> { - indexerMotor.set(indexerSpeed.getAsDouble()); - }); + } } public boolean getBeamBreak(){ return indexerBeamBreak.get(); } + + public Command autoIndex() { + return runOnce(() -> { + this.autoIndex = true; + }); + } + + public Command setSpeed(DoubleSupplier speed){ + return runOnce(() -> { autoIndex = false; }).andThen(run(() -> { + indexerMotor.set(speed.getAsDouble()); + })); + } + + public Command stop() { + return setSpeed(() -> 0); + } } diff --git a/src/main/java/frc/robot/subsystems/IntakePivot.java b/src/main/java/frc/robot/subsystems/IntakePivot.java new file mode 100644 index 0000000..a5fb0a2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakePivot.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class IntakePivot extends SubsystemBase { + +} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/IntakeRoller.java similarity index 78% rename from src/main/java/frc/robot/subsystems/Intake.java rename to src/main/java/frc/robot/subsystems/IntakeRoller.java index 05185f0..b49358f 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/IntakeRoller.java @@ -13,7 +13,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.util.function.BooleanConsumer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.IntakeConstants; +import frc.robot.constants.IntakeRollerConstants; public class Intake extends SubsystemBase{ private PIDController intakeAnglePID; @@ -30,36 +30,36 @@ public class Intake extends SubsystemBase{ public Intake(){ armOffset = 0; - intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless); + intakeRoller = new CANSparkMax(IntakeRollerConstants.kIntakeRollerID, MotorType.kBrushless); intakeRoller.setSmartCurrentLimit(60); intakeRoller.setIdleMode(IdleMode.kBrake); intakeRoller.burnFlash(); - intakePivot = new CANSparkMax(IntakeConstants.kIntakePivotID, MotorType.kBrushless); + intakePivot = new CANSparkMax(IntakeRollerConstants.kIntakePivotID, MotorType.kBrushless); intakePivot.setIdleMode(IdleMode.kBrake); - intakePivot.setSmartCurrentLimit(IntakeConstants.kPivotCurrentLimit); + intakePivot.setSmartCurrentLimit(IntakeRollerConstants.kPivotCurrentLimit); intakePivot.setInverted(true); intakePivot.burnFlash(); intakeFeedForward = new ArmFeedforward( - IntakeConstants.kSFeedForward, - IntakeConstants.kGFeedForward, - IntakeConstants.kVFeedForward + IntakeRollerConstants.kSFeedForward, + IntakeRollerConstants.kGFeedForward, + IntakeRollerConstants.kVFeedForward ); intakeEncoder = intakePivot.getEncoder(); - intakeEncoder.setPosition(Units.radiansToRotations( IntakeConstants.kStartingAngle)); + intakeEncoder.setPosition(Units.radiansToRotations( IntakeRollerConstants.kStartingAngle)); // intakeEncoder.setPositionConversionFactor(IntakeConstants.kIntakePivotConversionFactor); intakeAnglePID = new PIDController( - IntakeConstants.kPIntake, - IntakeConstants.kIIntake, - IntakeConstants.kDIntake + IntakeRollerConstants.kPIntake, + IntakeRollerConstants.kIIntake, + IntakeRollerConstants.kDIntake ); - armOffset = getIntakeAngle()-IntakeConstants.kStartingAngle; + armOffset = getIntakeAngle()-IntakeRollerConstants.kStartingAngle; } @@ -108,9 +108,9 @@ public class Intake extends SubsystemBase{ intakePivot.setVoltage( intakeAnglePID.calculate( getIntakeAngle(), - IntakeConstants.kDownAngle + IntakeRollerConstants.kDownAngle ) + intakeFeedForward.calculate( - IntakeConstants.kDownAngle, + IntakeRollerConstants.kDownAngle, 0.0 ) ); @@ -132,9 +132,9 @@ public class Intake extends SubsystemBase{ intakePivot.setVoltage( intakeAnglePID.calculate( getIntakeAngle(), - IntakeConstants.kDownAngle + IntakeRollerConstants.kDownAngle ) + intakeFeedForward.calculate( - IntakeConstants.kDownAngle, + IntakeRollerConstants.kDownAngle, 0.0 ) ); @@ -148,9 +148,9 @@ public class Intake extends SubsystemBase{ intakePivot.setVoltage( intakeAnglePID.calculate( getIntakeAngle(), - IntakeConstants.kUpAngle + IntakeRollerConstants.kUpAngle ) + intakeFeedForward.calculate( - IntakeConstants.kUpAngle, + IntakeRollerConstants.kUpAngle, 0.0 ) ); @@ -165,15 +165,15 @@ public class Intake extends SubsystemBase{ } public double getIntakeAngle(){ - return Units.rotationsToRadians(intakeEncoder.getPosition()/IntakeConstants.kIntakePivotConversionFactor)-armOffset; + return Units.rotationsToRadians(intakeEncoder.getPosition()/IntakeRollerConstants.kIntakePivotConversionFactor)-armOffset; } public double getIntakePID(){ return intakeAnglePID.calculate( getIntakeAngle(), - IntakeConstants.kDownAngle + IntakeRollerConstants.kDownAngle ) + intakeFeedForward.calculate( - IntakeConstants.kDownAngle, + IntakeRollerConstants.kDownAngle, 0.0 ); } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 90a89f6..58c7f54 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -1,163 +1,53 @@ package frc.robot.subsystems; -import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ShooterConstants; -public class Shooter extends SubsystemBase{ +public class Shooter extends SubsystemBase { private CANSparkMax topShooter; private CANSparkMax bottomShooter; - private CANSparkMax shooterPivot; - - private Encoder pivotEncoder; - - private PIDController shooterPivotPID; - private ArmFeedforward shooterPivotFF; - private DigitalInput shooterBeamBreak; - private boolean indexerBeamBreak; - - public Shooter(BooleanSupplier indexerBeamBreak){ - topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless); + public Shooter(){ + topShooter = new CANSparkMax(ShooterConstants.kTopMotorCANID, MotorType.kBrushless); topShooter.setInverted(true); - bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless); + + bottomShooter = new CANSparkMax(ShooterConstants.kBottomMotorCANID, MotorType.kBrushless); bottomShooter.setInverted(true); + + topShooter.setSmartCurrentLimit(ShooterConstants.kMotorCurrentLimit); + bottomShooter.setSmartCurrentLimit(ShooterConstants.kMotorCurrentLimit); - shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless); - shooterPivot.setInverted(true); - - /* - topPID = topShooter.getPIDController(); - topPID.setFeedbackDevice(topEncoder); - bottomPID = bottomShooter.getPIDController(); - bottomPID.setFeedbackDevice(bottomEncoder); - */ - - shooterPivot.setSmartCurrentLimit(50); - topShooter.setSmartCurrentLimit(40); - bottomShooter.setSmartCurrentLimit(40); - - pivotEncoder = new Encoder(0, 1); - pivotEncoder.setDistancePerPulse(ShooterConstants.kPivotConversion); - - shooterBeamBreak = new DigitalInput(ShooterConstants.kShooterBeamBreakChannel); + shooterBeamBreak = new DigitalInput(ShooterConstants.kBeamBreakChannel); topShooter.setIdleMode(IdleMode.kCoast); bottomShooter.setIdleMode(IdleMode.kCoast); - bottomShooter.burnFlash(); - shooterPivot.burnFlash(); topShooter.burnFlash(); - - shooterPivotPID = new PIDController( - ShooterConstants.kShooterPivotP, - ShooterConstants.kShooterPivotI, - ShooterConstants.kShooterPivotD - ); - shooterPivotFF = new ArmFeedforward(ShooterConstants.kSShooterPivotFF, ShooterConstants.kGShooterPivotFF, ShooterConstants.kVShooterPivotFF); - + bottomShooter.burnFlash(); } - - /* - public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){ - return run(()-> { - angleAndSpeedControl(setpointAngle, topRPM, bottomRPM); - }); - }*/ - - public Command angleSpeedsSetpoints(DoubleSupplier setpointAngle, DoubleSupplier speed){ - return run(()-> { - angleAndSpeedControl(setpointAngle.getAsDouble(), speed.getAsDouble(), speed.getAsDouble()); - }); - } - - public Command angleSpeedsSetpoints(DoubleSupplier setpointAngle, double topRPM, double bottomRPM){ - return run(()-> { - angleAndSpeedControl(setpointAngle.getAsDouble(), topRPM, bottomRPM); - }); - } - - private void angleAndSpeedControl(double setpointAngle, double topRPM, double bottomRPM){ - shooterPivot.setIdleMode(IdleMode.kBrake); - - shooterPivot.setVoltage( - shooterPivotPID.calculate(getShooterAngle(), setpointAngle) + - shooterPivotFF.calculate(setpointAngle, 0.0)); - - //topPID.setReference(topRPM, CANSparkMax.ControlType.kVelocity); - //bottomPID.setReference(bottomRPM, CANSparkMax.ControlType.kVelocity); - topShooter.set(topRPM); - bottomShooter.set(bottomRPM); - } - - public Command ampHandoff(DoubleSupplier shootNoteSpeed, BooleanSupplier ampAngleOrNah){ - return run(() -> { - - shooterPivot.setIdleMode(IdleMode.kBrake); - - shooterPivot.setVoltage( - shooterPivotPID.calculate(getShooterAngle(), ShooterConstants.kShooterLoadAngle) + - shooterPivotFF.calculate(ShooterConstants.kShooterLoadAngle, 0.0)); - - if(shooterBeamBreak.get() || indexerBeamBreak){ - angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 0.25, 0.25); - }else{ - angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, shootNoteSpeed.getAsDouble(), shootNoteSpeed.getAsDouble()); - } - }); - } - - public Command climbState(){ - return run(() -> { - shooterPivot.setIdleMode(IdleMode.kCoast); - shooterPivot.set(0.0); - }); - } - - public double getShooterAngle(){ - return pivotEncoder.getDistance() + ShooterConstants.kShooterLoadAngle; - } - - public Command zeroEncoder(){ - return run(() -> { - pivotEncoder.reset(); - }); - } - - public Boolean getBeamBreak(){ + + public boolean getBeamBreak(){ return shooterBeamBreak.get(); } - public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier spinny){ - return run(() ->{ - shooterPivot.setIdleMode(IdleMode.kBrake); - if(getShooterAngle() >= ShooterConstants.kShooterLoadAngle && getShooterAngle() <= ShooterConstants.kShooterAmpAngle){ - shooterPivot.set(pivotPower.getAsDouble()); - }else if(getShooterAngle() > ShooterConstants.kShooterAmpAngle){ - shooterPivot.set(MathUtil.clamp(pivotPower.getAsDouble(), -1.0, 0.0)); - }else if(getShooterAngle() < ShooterConstants.kShooterLoadAngle){ - shooterPivot.set(MathUtil.clamp(pivotPower.getAsDouble(), 0.0, 1.0)); - } - - topShooter.set(spinny.getAsDouble()); - bottomShooter.set(spinny.getAsDouble()); - + public Command setSpeed(DoubleSupplier speed) { + return run(() -> { + topShooter.set(speed.getAsDouble()); + bottomShooter.set(speed.getAsDouble()); }); } - + public Command stop() { + return setSpeed(() -> 0); + } }