diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2db09ab..cdd61ae 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -62,6 +62,8 @@ import frc4388.robot.commands.Shoot; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Serializer; +import frc4388.robot.subsystems.Storage; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; import frc4388.robot.subsystems.Vision; @@ -83,10 +85,20 @@ public class RobotContainer { /* RobotMap */ private final RobotMap m_robotMap = new RobotMap(); - /* Subsystems */ - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, - m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); - + // Subsystems + private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( + m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor, + m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor, + m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor, + m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor, + m_robotMap.leftFrontEncoder, + m_robotMap.rightFrontEncoder, + m_robotMap.leftBackEncoder, + m_robotMap.rightBackEncoder + ); + 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, 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); private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); private final Hood m_robotHood = new Hood(); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index f7825bd..aedf266 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -15,10 +15,13 @@ import com.ctre.phoenix.sensors.WPI_PigeonIMU; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.Constants.SerializerConstants; +import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveModule; @@ -210,4 +213,18 @@ public class RobotMap { } - \ No newline at end of file + + /* 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 a3cf25e..b194035 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) { //TODO: Only one motor lol 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 a922e27..51b0281 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -11,20 +11,22 @@ 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; } /** * If The Beam Is Broken, Run Storage * If Else, Stop Running Storage */ public void manageStorage() { - if (m_beamShooter.get()) { + if (isBeamIntakeBroken()) { //Maybe needs to be shooter runStorage(1.d); } else { runStorage(0.d); } }