diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1a60a13..4949708 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -13,6 +13,7 @@ import frc4388.robot.Constants.*; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Serializer; +import frc4388.robot.subsystems.Storage; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; @@ -42,8 +43,8 @@ public class RobotContainer { ); */ private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor); - private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, m_robotMap.serializerShooterBelt); - + private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, m_robotMap.serializerShooterBelt, m_robotMap.serializerBeam); + private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter); private final LED m_robotLED = new LED(m_robotMap.LEDController); /* Controllers */ @@ -63,12 +64,15 @@ public class RobotContainer { // getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); m_robotIntake.setDefaultCommand( - new RunCommand(() -> m_robotIntake.runWithTriggers( - getDriverController().getLeftTriggerAxis(), getDriverController().getRightTriggerAxis()),m_robotIntake)); + new RunCommand(() -> m_robotIntake.runWithTriggers( + getDriverController().getLeftTriggerAxis(), + getDriverController().getRightTriggerAxis()), + m_robotIntake)); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand( - new RunCommand(m_robotLED::updateLED, m_robotLED)); - // dri + new RunCommand(m_robotLED::updateLED, m_robotLED)); + m_robotStorage.setDefaultCommand( + new RunCommand(m_robotStorage::manageStorage, m_robotStorage)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index c9f4012..1085616 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -8,10 +8,12 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SerializerConstants; +import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import com.revrobotics.CANSparkMaxLowLevel.MotorType; @@ -97,11 +99,16 @@ public class RobotMap { } /* Serializer Subsystem */ - public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); public final CANSparkMax serializerShooterBelt = new CANSparkMax(SerializerConstants.SERIALIZER_SHOOTER_BELT, MotorType.kBrushless); + public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); /* Intake Subsytem */ public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); + + /* Storage Subsystem */ + public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); + public final DigitalInput beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); + public final DigitalInput beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); } diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java index e9ec997..f902fb0 100644 --- a/src/main/java/frc4388/robot/subsystems/Serializer.java +++ b/src/main/java/frc4388/robot/subsystems/Serializer.java @@ -12,10 +12,10 @@ public class Serializer extends SubsystemBase{ private DigitalInput m_serializerBeam; private boolean serializerState; - public Serializer(CANSparkMax serializerBelt, CANSparkMax serializerShooterBelt) { + public Serializer(CANSparkMax serializerBelt, CANSparkMax serializerShooterBelt, DigitalInput serializerBeam) { m_serializerBelt = serializerBelt; m_serializerShooterBelt = serializerShooterBelt; - m_serializerBeam = new DigitalInput(Constants.SerializerConstants.SERIALIZER_BELT_BEAM); + m_serializerBeam = serializerBeam; serializerState = false; setSerializerState(serializerState); diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 9fc2df0..2a63bbc 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -11,16 +11,18 @@ import edu.wpi.first.wpilibj.DigitalInput; import frc4388.robot.Constants.StorageConstants; public class Storage extends SubsystemBase { - public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); - private DigitalInput m_beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); - private DigitalInput m_beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); + public CANSparkMax m_storageMotor; + private DigitalInput m_beamShooter; + private DigitalInput m_beamIntake; /** Creates a new Storage. */ - public Storage() { - + public Storage(CANSparkMax storageMotor, DigitalInput beamShooter, DigitalInput beamIntake) { + m_storageMotor = storageMotor; + m_beamShooter = beamShooter; + m_beamIntake = beamIntake; } public void manageStorage() { - if (m_beamShooter.get()) { + if (isBeamIntakeBroken()) { //Maybe needs to be shooter runStorage(1.d); } else { runStorage(0.d); } } @@ -28,12 +30,12 @@ public class Storage extends SubsystemBase { m_storageMotor.set(input); } - public boolean getBeamShooter(){ - return m_beamShooter.get(); + public boolean isBeamShooterBroken(){ + return !m_beamShooter.get(); } - public boolean getBeamIntake(){ - return m_beamIntake.get(); + public boolean isBeamIntakeBroken(){ + return !m_beamIntake.get(); } @Override