From 1bd632449d190e5a96f71d710e3b5eb63e004ece Mon Sep 17 00:00:00 2001 From: Shai Grossman <72968565+korozya-bit@users.noreply.github.com> Date: Tue, 6 Dec 2022 10:20:57 +0200 Subject: [PATCH] Intake branch (#1) wrote the inake subsystem --- .idea/runConfigurations.xml | 0 .../java/frc/robot/subsystems/Intake.java | 150 ++++++++++++------ .../frc/robot/subsystems/Superstructure.java | 98 ++++++++++++ 3 files changed, 198 insertions(+), 50 deletions(-) create mode 100644 .idea/runConfigurations.xml create mode 100644 src/main/java/frc/robot/subsystems/Superstructure.java diff --git a/.idea/runConfigurations.xml b/.idea/runConfigurations.xml new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index cc89400..c7bbc25 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -5,56 +5,106 @@ import com.revrobotics.ColorSensorV3; import edu.wpi.first.wpilibj.*; import edu.wpi.first.wpilibj2.command.*; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -import static frc.robot.Constants.IntakeConstants; +import frc.robot.Constants; public class Intake extends SubsystemBase { - private int ballCount; - private final CANSparkMax frontMotor = new CANSparkMax(IntakeConstants.INTAKE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); - private final CANSparkMax backMotor = new CANSparkMax(IntakeConstants.UPPER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); - private final DoubleSolenoid piston = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, IntakeConstants.FWD_CHANNEL, IntakeConstants.REV_CHANNEL); - private final ColorSensorV3 colorSensor = new ColorSensorV3(I2C.Port.kMXP); - private final Ultrasonic ultrasonic = new Ultrasonic(IntakeConstants.UPPER_PING, IntakeConstants.UPPER_ECHO); - - public Intake() { - frontMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); - backMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); - - frontMotor.setInverted(true); - backMotor.setInverted(true); - - ultrasonic.setEnabled(true); - ballCount = 1; - } - - private Command incrementBallCount(int inc){ - return new InstantCommand(()-> this.ballCount = ballCount + inc); - } - - public Command pullIntoIntake() { - return new FunctionalCommand( - ()-> piston.set(DoubleSolenoid.Value.kForward), - ()-> frontMotor.set(IntakeConstants.INTAKE_SPEED), - (__)-> frontMotor.set(0), - ()-> colorSensor.getProximity() < IntakeConstants.COLOR_LIMIT) - .andThen(incrementBallCount(1)); - } - - public Command pullIntoUpper(){ - return new FunctionalCommand( - () -> {}, - ()-> backMotor.set(IntakeConstants.TRANSPORT_SPEED), - (__)-> backMotor.set(0), - ()-> ultrasonic.getRangeMM() < IntakeConstants.SONIC_LIMIT); - } - - public Command pullIntoShooter(Trigger ballShotTrigger){ - return new FunctionalCommand( - ()-> {}, - ()-> backMotor.set(IntakeConstants.TRANSPORT_SPEED), - (__) -> backMotor.set(0), - ballShotTrigger) - .andThen(incrementBallCount(-1)); - } + private int ballsQuantity; + private final CANSparkMax frontMotor = new CANSparkMax( + Constants.IntakeConstants.INTAKE_MOTOR_ID, + CANSparkMaxLowLevel.MotorType.kBrushless); + private final CANSparkMax backMotor = new CANSparkMax( + Constants.IntakeConstants.UPPER_MOTOR_ID, + CANSparkMaxLowLevel.MotorType.kBrushless); + private final DoubleSolenoid piston = new DoubleSolenoid( + PneumaticsModuleType.CTREPCM, + Constants.IntakeConstants.FWD_CHANNEL, + Constants.IntakeConstants.REV_CHANNEL); + private final ColorSensorV3 colorSensor = new ColorSensorV3(I2C.Port.kMXP); + private final Ultrasonic ultrasonic = new Ultrasonic( + Constants.IntakeConstants.UPPER_PING, + Constants.IntakeConstants.UPPER_ECHO); + + public Intake() { + frontMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); + backMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); + + frontMotor.setInverted(true); + backMotor.setInverted(true); + + ultrasonic.setEnabled(true); + ballsQuantity = 1; + } + + public void openClosePiston(boolean needToOpen) { + if (needToOpen) piston.set(DoubleSolenoid.Value.kForward); + else piston.set(DoubleSolenoid.Value.kReverse); + } + + public void setFrontMotorSpeed(double speed) { + frontMotor.set(speed); + } + + public void setBackMotorSpeed(double speed) { + backMotor.set(speed); + } + + private double getColorDis() { + return colorSensor.getProximity(); + } + + public boolean isColorDist() { + return getColorDis() > Constants.IntakeConstants.COLOR_LIMIT; + } + + private double getUltrasonicDis() { + return ultrasonic.getRangeMM(); + } + + public boolean isUltraDist() { + return this.ultrasonic.getRangeMM() < Constants.IntakeConstants.SONIC_LIMIT; + } + + public Command closeIntake() { + return new InstantCommand( + () -> openClosePiston(false), this) + .andThen(stopFrontMotor()); + } + + public Command pullToUltrasonic() { + return new RunCommand(() -> setFrontMotorSpeed(0.5), this) + .until(this::isUltraDist) + .andThen(stopFrontMotor()); + } + + public boolean isOurColor() { + switch (DriverStation.getAlliance()) { + case Red: + return colorSensor.getRed() > colorSensor.getBlue(); + case Blue: + return colorSensor.getBlue() > colorSensor.getRed(); + default: + DriverStation.reportError("No alliance has been found", false); + return false; + } + } + + public FunctionalCommand ejectFromColorCommand() { + return new FunctionalCommand( + () -> {}, + () -> this.setFrontMotorSpeed(-0.3), + (__) -> new WaitCommand(0.5).andThen(stopFrontMotor()), + ()-> !isColorDist()); + } + + public boolean intakeFull() { + return isColorDist() && isUltraDist() && isOurColor(); + } + + public Command stopFrontMotor() { + return new InstantCommand(() -> frontMotor.set(0)); + } + + public Command stopBackMotor() { + return new InstantCommand(() -> backMotor.set(0)); + } } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java new file mode 100644 index 0000000..829ce98 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -0,0 +1,98 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.*; +import lib.RepeatingCommand; + +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicReference; +import java.util.function.BooleanSupplier; + +public class Superstructure { + private final Intake intake = new Intake(); + +// option a - David Arownowitz +// public Command intakeBallsCommand() { +// return new InstantCommand( +// () -> intake.openClosePiston(true)) +// .andThen( +// new RepeatingCommand( +// new ConditionalCommand( +// // deciding by whether the ball is our color +// new ConditionalCommand( +// // whether the ultrasonic detects a ball +// intake.closeIntake() +// .andThen(new RunCommand(()-> {})), +// intake.pullToUltrasonic(), +// intake::isUltraDist), +// new ConditionalCommand( +// // whether we want to eject from intake or from shooter +// intake.ejectFromColorCommand(), +// new RunCommand(() -> { +// intake.setFrontMotorSpeed(0.3); +// // TODO call the low shooter eject command +// }) +// .until(() -> intake.isUltraDist()) +// .andThen(() -> intake.stopBackMotor()) +// /*TODO .andThen(stopShootMotorCommand*/, +// intake::isUltraDist), +// intake::isOurColor))); +// } + + // option b - Yoav Cohen +// public Command intakeBallsCommand() { +// AtomicBoolean intakeFull = new AtomicBoolean(false); +// return new InstantCommand( +// () -> intake.openClosePiston(true)) +// .andThen( +// new RepeatingCommand( +// new ConditionalCommand( +// // deciding by whether the ball is our color +// new ConditionalCommand( +// // whether the ultrasonic detects a ball +// intake.closeIntake() +// .andThen(new InstantCommand(()-> intakeFull.set(true))), +// intake.pullToUltrasonic(), +// intake::isUltraDist), +// new ConditionalCommand( +// // whether we want to eject from intake or from shooter +// intake.ejectFromColorCommand(), +// new RunCommand(() -> { +// intake.setFrontMotorSpeed(0.3); +// // TODO call the low shooter eject command +// }) +// .until(() -> intake.isUltraDist()) +// .andThen(() -> intake.stopBackMotor()) +// /*TODO .andThen(stopShootMotorCommand*/, +// intake::isUltraDist), +// intake::isOurColor)).until(intakeFull::get)); +// } + + // option c +// public Command intakeBallsCommand() { +// return new InstantCommand( +// () -> intake.openClosePiston(true)) +// .andThen( +// new RepeatingCommand( +// new ConditionalCommand( +// // deciding by whether the ball is our color +// new ConditionalCommand( +// // whether the ultrasonic detects a ball +// intake.closeIntake() +// .andThen(new RunCommand(()-> {})), +// intake.pullToUltrasonic(), +// intake::isUltraDist), +// new ConditionalCommand( +// // whether we want to eject from intake or from shooter +// intake.ejectFromColorCommand(), +// new RunCommand(() -> { +// intake.setFrontMotorSpeed(0.3); +// // TODO call the low shooter eject command +// }) +// .until(() -> intake.isUltraDist()) +// .andThen(() -> intake.stopBackMotor()) +// /* TODO .andThen(stopShootMotorCommand*/, +// intake::isUltraDist), +// intake::isOurColor)) +// .until(intake::intakeFull)); +// } +}