Skip to content

Commit

Permalink
Intake branch (#1)
Browse files Browse the repository at this point in the history 
wrote the inake subsystem
  • Loading branch information
@TapChap
TapChap authored Dec 6, 2022
1 parent c50fc4d commit 1bd6324
Show file tree
Hide file tree
Showing 3 changed files with 198 additions and 50 deletions.
Empty file added .idea/runConfigurations.xml
View file
Empty file.
150 changes: 100 additions & 50 deletions src/main/java/frc/robot/subsystems/Intake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
}
}
98 changes: 98 additions & 0 deletions src/main/java/frc/robot/subsystems/Superstructure.java
View file
Original file line number Diff line number Diff line change
@@ -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));
// }
}

0 comments on commit 1bd6324

Please sign in to comment.