diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c7e102a..8c1aec1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -102,6 +102,21 @@ public static class ArmConstants { } public static class IntakeShooterConstants { + static { + switch (currentBot) { + case WOOD_BOT: + HAS_INTAKE = false; + break; + + case COMP_BOT: + default: + HAS_INTAKE = true; + break; + } + } + + public static final boolean HAS_INTAKE; + public static final boolean INTAKE_REVERSE = false; public static final boolean FLYWHEEL_REVERSE = false; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9ba5bee..be8d84c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ import frc.robot.commands.ArmRotateTo; import frc.robot.commands.SetLightstripColor; import frc.robot.Constants.VisionConstants; -import frc.robot.subsystems.IntakeShooter; import frc.robot.subsystems.LightStrip; import frc.robot.subsystems.SwerveDrivetrain; import frc.robot.subsystems.SwerveModule; @@ -23,6 +22,9 @@ import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.DummyArm; import frc.robot.subsystems.arm.RealArm; +import frc.robot.subsystems.intake.DummyShooter; +import frc.robot.subsystems.intake.IntakeShooter; +import frc.robot.subsystems.intake.RealShooter; import frc.robot.inputs.ChassisDriveInputs; import com.kauailabs.navx.frc.AHRS; @@ -91,11 +93,11 @@ public class RobotContainer { ArmConstants.RIGHT_ENCODER_ID, ArmConstants.ARE_MOTORS_REVERSED) : new DummyArm(); - private final IntakeShooter intakeShooter = new IntakeShooter( + private final IntakeShooter intakeShooter = Constants.IntakeShooterConstants.HAS_INTAKE ? new RealShooter( IntakeShooterConstants.FLYWHEEL_MOTOR_LEFT_ID, IntakeShooterConstants.FLYWHEEL_MOTOR_RIGHT_ID, IntakeShooterConstants.INTAKE_MOTOR_LEFT_ID, - IntakeShooterConstants.INTAKE_MOTOR_RIGHT_ID); + IntakeShooterConstants.INTAKE_MOTOR_RIGHT_ID) : new DummyShooter(); private final SwerveDrivetrain drivetrain = new SwerveDrivetrain( gyro, diff --git a/src/main/java/frc/robot/subsystems/intake/DummyShooter.java b/src/main/java/frc/robot/subsystems/intake/DummyShooter.java new file mode 100644 index 0000000..5b627ca --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/DummyShooter.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.intake; + +public class DummyShooter extends IntakeShooter { + public void setFlyWheelSpeed(double speed) { + }; + + public void startFlyWheels() { + } + + public void stopFlywheels() { + } + + public void setIntakeSpeed(double speed) { + } + + public void intake() { + } + + public void intakeReverse() { + } + + public void stopIntake() { + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeShooter.java b/src/main/java/frc/robot/subsystems/intake/IntakeShooter.java new file mode 100644 index 0000000..e4dcd22 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeShooter.java @@ -0,0 +1,27 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class IntakeShooter extends SubsystemBase { + public void setFlyWheelSpeed(double speed) { + }; + + public void startFlyWheels() { + } + + public void stopFlywheels() { + } + + public void setIntakeSpeed(double speed) { + } + + public void intake() { + } + + public void intakeReverse() { + } + + public void stopIntake() { + } + +} diff --git a/src/main/java/frc/robot/subsystems/IntakeShooter.java b/src/main/java/frc/robot/subsystems/intake/RealShooter.java similarity index 87% rename from src/main/java/frc/robot/subsystems/IntakeShooter.java rename to src/main/java/frc/robot/subsystems/intake/RealShooter.java index 494c50a..8b9bde7 100644 --- a/src/main/java/frc/robot/subsystems/IntakeShooter.java +++ b/src/main/java/frc/robot/subsystems/intake/RealShooter.java @@ -1,9 +1,8 @@ -package frc.robot.subsystems; +package frc.robot.subsystems.intake; import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj.motorcontrol.Talon; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeShooterConstants; /** @@ -17,7 +16,7 @@ * * @author Aceius E. */ -public class IntakeShooter extends SubsystemBase { +public class RealShooter extends IntakeShooter { /* * 2 motors for flywheel * 2 for intake @@ -29,13 +28,13 @@ public class IntakeShooter extends SubsystemBase { private final CANSparkMax intake1; private final CANSparkMax intake2; // might not exist - public IntakeShooter(int flywheel1id, int flywheel2id, int intake1id, int intake2id) { + public RealShooter(int flywheel1id, int flywheel2id, int intake1id, int intake2id) { this.flywheel1 = new Talon(flywheel1id); this.flywheel2 = new Talon(flywheel2id); - + flywheel1.setInverted(IntakeShooterConstants.FLYWHEEL_REVERSE); flywheel1.setInverted(IntakeShooterConstants.FLYWHEEL_REVERSE); - + this.intake1 = new CANSparkMax(intake1id, CANSparkLowLevel.MotorType.kBrushless); this.intake2 = new CANSparkMax(intake2id, CANSparkLowLevel.MotorType.kBrushless); @@ -51,7 +50,7 @@ public void setFlyWheelSpeed(double speed) { public void startFlyWheels() { setFlyWheelSpeed(1); } - + public void stopFlywheels() { setFlyWheelSpeed(0); } @@ -60,7 +59,7 @@ public void setIntakeSpeed(double speed) { intake1.set(speed); intake2.set(speed); } - + public void intake() { setIntakeSpeed(1); }