diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f634214..01694de 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,7 @@ import frc.robot.constants.PhotonConstants; import frc.robot.constants.ShooterConstants; import frc.robot.subsystems.Climber; import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Indexer; import frc.robot.utilities.PhotonVision; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; @@ -36,12 +37,13 @@ public class RobotContainer { private static final String kAutoTabName = "Autonomous"; private static final String kTeleopTabName = "Teloperated"; - private PhotonVision photonvision; + //private PhotonVision photonvision; private Drivetrain drivetrain; private Intake intake; private Shooter shooter; private Climber climber; + private Indexer indexer; private CommandXboxController driver; private CommandXboxController operator; @@ -59,7 +61,8 @@ public class RobotContainer { private int speakerTag; public RobotContainer() { - try { + /*try { + photonvision = new PhotonVision( PhotonConstants.kCameraName, PhotonConstants.kCameraTransform, @@ -68,12 +71,12 @@ public class RobotContainer { ); } catch (IOException e) { photonvision = null; - } + }*/ // TODO Need to provide a real initial pose // TODO Need to provide a real IAprilTagProvider, null means we're not using one at all // TODO Need to provide a real VisualPoseProvider, null means we're not using one at all - drivetrain = new Drivetrain(new Pose2d(), photonvision, null); + drivetrain = new Drivetrain(new Pose2d(), null, null); intake = new Intake(); @@ -81,6 +84,8 @@ public class RobotContainer { climber = new Climber(shooter.getShooterAngle()); + indexer = new Indexer(); + // An example Named Command, doesn't need to remain once we start actually adding real things // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration NamedCommands.registerCommand("Set Drivetrain X", drivetrain.setXCommand()); @@ -133,6 +138,8 @@ public class RobotContainer { climber.setDefaultCommand(climber.stop()); + indexer.setDefaultCommand(indexer.autoIndexing()); + driver.povCenter().onFalse( drivetrain.driveCardinal( driver::getLeftY, @@ -198,7 +205,7 @@ public class RobotContainer { driver.leftBumper().toggleOnTrue(intake.intakeDownCommand()); - operator.y().onTrue(climber.setSpeed(operator.getRightTriggerAxis())); + operator.y().onTrue(climber.setSpeedWithSupplier(operator::getRightTriggerAxis)); } diff --git a/src/main/java/frc/robot/constants/ClimberConstants.java b/src/main/java/frc/robot/constants/ClimberConstants.java index ca639fa..0fa8496 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 = 1; - public static final int kClimberMotor2CANID = 2; + public static final int kClimberMotor1CANID = 14; + public static final int kClimberMotor2CANID = 15; } diff --git a/src/main/java/frc/robot/constants/IndexerConstants.java b/src/main/java/frc/robot/constants/IndexerConstants.java new file mode 100644 index 0000000..2d0a952 --- /dev/null +++ b/src/main/java/frc/robot/constants/IndexerConstants.java @@ -0,0 +1,7 @@ +package frc.robot.constants; + +public class IndexerConstants { + public static final int kIndexerID = 13; + + public static final int kIndexerBeamBreakChannel = 3; +} diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index 075ee97..a1fcd40 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -2,9 +2,9 @@ package frc.robot.constants; public class ShooterConstants { - public static final int kTopShooterID = 11; - public static final int kBottomShooterID = 12; - public static final int kShooterPivotID = 13; + public static final int kTopShooterID = 9; + public static final int kBottomShooterID = 11; + public static final int kShooterPivotID = 16; public static final double kShooterP = 0.0; public static final double kShooterI = 0.0; @@ -28,4 +28,8 @@ public class ShooterConstants { 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 = 4; } diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index ecc66ce..e91bdbb 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -13,17 +13,24 @@ public class Climber extends SubsystemBase { private VictorSPX motor1; private VictorSPX motor2; + private DoubleSupplier shooterAngle; + //TODO What tells the climber to stop climbing when it achieves the target height? public Climber(DoubleSupplier shooterAngle) { - motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID); - motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID); + if(shooterAngle.getAsDouble() > Math.toRadians(-10.0)){ + motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID); + motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID); - motor2.follow(motor1); + motor2.follow(motor1); + } } public Command setSpeedWithSupplier(DoubleSupplier speed) { return run(() -> { - motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble()); + + if(shooterAngle.getAsDouble() > Math.toRadians(-10.0)){ + motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble()); + } }); } diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java new file mode 100644 index 0000000..2cd2b33 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.Command; +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); + + indexerMotor.setSmartCurrentLimit(40); + indexerMotor.setIdleMode(IdleMode.kBrake); + indexerMotor.burnFlash(); + + indexerBeamBreak = new DigitalInput(IndexerConstants.kIndexerBeamBreakChannel); + } + + public Command autoIndexing(){ + return run(() -> { + if(!indexerBeamBreak.get()){ + indexerMotor.set(0.5); + }else if(indexerBeamBreak.get()){ + indexerMotor.set(0.0); + } + + }); + } + + public Command shootNote(){ + return run(() -> { + indexerMotor.set(1.0); + }); + } +} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 12a2282..f498a2a 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -24,7 +24,12 @@ public class Intake extends SubsystemBase{ public Intake(){ intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless); + intakeRoller.setSmartCurrentLimit(60); + intakeRoller.setIdleMode(IdleMode.kCoast); + intakeRoller.burnFlash(); + intakePivot = new CANSparkMax(IntakeConstants.kIntakePivotID, MotorType.kBrushless); + intakePivot.setIdleMode(IdleMode.kBrake); intakePivot.setSmartCurrentLimit(IntakeConstants.kPivotCurrentLimit); intakePivot.burnFlash(); diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 5c8861b..98f39a9 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -4,11 +4,13 @@ import java.util.function.DoubleSupplier; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +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; @@ -34,18 +36,31 @@ public class Shooter extends SubsystemBase{ private PIDController shooterPivotPID; private ArmFeedforward shooterPivotFF; + private DigitalInput shooterBeamBreak; + public Shooter(){ topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless); bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless); + + topShooter.setSmartCurrentLimit(40); + topShooter.burnFlash(); + + bottomShooter.setSmartCurrentLimit(40); + bottomShooter.burnFlash(); topEncoder = topShooter.getEncoder(); bottomEncoder = bottomShooter.getEncoder(); shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless); + shooterPivot.setSmartCurrentLimit(50); + shooterPivot.burnFlash(); + pivotEncoder = new Encoder(0, 1); pivotEncoder.setDistancePerPulse(ShooterConstants.kPivotConversion); + shooterBeamBreak = new DigitalInput(ShooterConstants.kShooterBeamBreakChannel); + //TODO These can probably devolve into BangBang controllers and let the feed forwards maintain speed topShooterPID = new PIDController( ShooterConstants.kShooterP, @@ -76,6 +91,8 @@ public class Shooter extends SubsystemBase{ public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){ return run(()-> { + shooterPivot.setIdleMode(IdleMode.kBrake); + shooterPivot.setVoltage( shooterPivotPID.calculate(pivotEncoder.getDistance(), setpointAngle) + shooterPivotFF.calculate(setpointAngle, 0.0)); @@ -86,7 +103,32 @@ public class Shooter extends SubsystemBase{ }); } + public Command recieveNote(){ + return run(() -> { + shooterPivot.setIdleMode(IdleMode.kBrake); + + shooterPivot.setVoltage( + shooterPivotPID.calculate(pivotEncoder.getDistance(), ShooterConstants.kShooterLoadAngle) + + shooterPivotFF.calculate(ShooterConstants.kShooterLoadAngle, 0.0)); + + if(shooterBeamBreak.get()){ + topShooter.set(0.25); + bottomShooter.set(0.25); + }else{ + topShooter.set(0.); + bottomShooter.set(0.); + } + }); + } + + public Command climbState(){ + return run(() -> { + shooterPivot.setIdleMode(IdleMode.kCoast); + shooterPivot.set(0.0); + }); + } + public DoubleSupplier getShooterAngle(){ - return pivotEncoder.getDistance(); + return () -> {return pivotEncoder.getDistance();}; } }