Skip to content

Commit

Permalink
2-ball Internals (#20)
Browse files Browse the repository at this point in the history 
* 2-ball internals code, turret enum renaming

Needs testing.

* Fix motor conditions

* Testing code

Testing on Monday or Tuesday!

* Fix infinite storage state updates

* more print statements + failsafes, will probably rewrite but just to save

* internals, but now on transition timers

* auton 1-ball start & total ball count

Co-authored-by: eggaskin <[email protected]>
  • Loading branch information
@ky28059 @eggaskin @e3l
2 people authored and e3l committed Mar 5, 2022
1 parent 9bb6997 commit 741c00f
Show file tree
Hide file tree
Showing 4 changed files with 128 additions and 72 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -141,7 +141,7 @@ private void controllerBindings() {
mechAButton.whenPressed(new RequestShotCommand(internalSubsystem));
mechXButton.whenPressed(new InstantCommand(() -> internalSubsystem.setPower(0)));

mechYButton.whenPressed(new InstantCommand(() -> turretSubsystem.setFlywheelPower(0.8)));
mechYButton.whenPressed(new InstantCommand(() -> turretSubsystem.setFlywheelPower(0.2)));
mechBButton.whenPressed(new InstantCommand(() -> turretSubsystem.setFlywheelPower(0)));

if (tankSubsystem != null) {
Expand Down
12 changes: 9 additions & 3 deletions src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ public class IntakeSubsystem extends GRTSubsystem {
* counterclockwise angle from straight upwards. In degrees.
*/
public enum IntakePosition {
RAISED(0), DEPLOYED(465469);
RAISED(0), DEPLOYED(200000);

public final double value;

Expand Down Expand Up @@ -83,6 +83,14 @@ public IntakeSubsystem(InternalSubsystem internalSubsystem /* , JetsonConnection
deploy.config_kI(0, kI);
deploy.config_kD(0, kD);

// Soft limit deploy between RAISED and DEPLOYED
/*
deploy.configForwardSoftLimitEnable(true);
deploy.configReverseSoftLimitEnable(true);
deploy.configForwardSoftLimitThreshold(IntakePosition.RAISED.value);
deploy.configReverseSoftLimitThreshold(IntakePosition.DEPLOYED.value);
*/

// Initialize Shuffleboard entries
shuffleboardTab = Shuffleboard.getTab("Intake");
shuffleboardPEntry = shuffleboardTab.add("kP", kP).getEntry();
Expand All @@ -98,11 +106,9 @@ public IntakeSubsystem(InternalSubsystem internalSubsystem /* , JetsonConnection
@Override
public void periodic() {
// Get PID constants from Shuffleboard for testing

deploy.config_kP(0, shuffleboardPEntry.getDouble(kP));
deploy.config_kI(0, shuffleboardIEntry.getDouble(kI));
deploy.config_kD(0, shuffleboardDEntry.getDouble(kD));


// If the jetson detects a ball or the driver is running the intake, the intake
// is deployed,
Expand Down
32 changes: 17 additions & 15 deletions src/main/java/frc/robot/subsystems/TurretSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,13 +44,15 @@ public enum TurretMode {

/**
* Represents the state of a turret module (flywheel, turntable, hood).
* If a module is READY, it is completely ready to shoot.
* If a module is ALMOST, it is nearly ready and will become ready after a robot stop.
* If a module is in HIGH_TOLERANCE, it is completely ready to shoot.
* If a module is in LOW_TOLERANCE, it is nearly ready and will become ready after a robot stop.
* This state is for rejecting, which doesn't need a perfectly lined up shot but needs better than completely
* unaligned. It is also for drivers to know when the turret is almost ready
* If a module is UNALIGNED, it is not ready; it will require more than a second to get it to READY status.
* For turntable, this includes being in the blind spot.
*/
public enum ModuleState {
READY, ALMOST, UNALIGNED
HIGH_TOLERANCE, LOW_TOLERANCE, UNALIGNED
}

public boolean spin = false;
Expand Down Expand Up @@ -262,7 +264,7 @@ public void periodic() {
// hood.set(ControlMode.Position, desiredHoodAngle);
// turntable.set(ControlMode.Position, Math.max(Math.min(desiredTurntablePosition, TURNTABLE_MAX_POS), TURNTABLE_MIN_POS));

System.out.println("Turret RPM: " + flywheelEncoder.getVelocity());
//System.out.println("Turret RPM: " + flywheelEncoder.getVelocity());
}

/**
Expand All @@ -283,8 +285,8 @@ public void setReject(boolean reject) {
private ModuleState flywheelReady() {
// TODO: test thresholding values
double diff = Math.abs(flywheelEncoder.getVelocity() - desiredFlywheelSpeed);
return diff < 10 ? ModuleState.READY
: diff < 20 ? ModuleState.ALMOST
return diff < 10 ? ModuleState.HIGH_TOLERANCE
: diff < 20 ? ModuleState.LOW_TOLERANCE
: ModuleState.UNALIGNED;
}

Expand All @@ -293,14 +295,14 @@ private ModuleState flywheelReady() {
* @return The state of the turntable.
*/
private ModuleState turntableAligned() {
// If the calculated theta is in the blind spot, return the RED state
// If the calculated theta is in the blind spot, return UNALIGNED
if (desiredTurntablePosition < TURNTABLE_MIN_POS || desiredTurntablePosition > TURNTABLE_MAX_POS)
return ModuleState.UNALIGNED;

// TODO: test thresholding values
double diff = Math.abs(turntable.getSelectedSensorPosition() - desiredTurntablePosition);
return diff < 10 ? ModuleState.READY
: diff < 20 ? ModuleState.ALMOST
return diff < 10 ? ModuleState.HIGH_TOLERANCE
: diff < 20 ? ModuleState.LOW_TOLERANCE
: ModuleState.UNALIGNED;
}

Expand All @@ -311,15 +313,15 @@ private ModuleState turntableAligned() {
private ModuleState hoodReady() {
// TODO: test thresholding values
double diff = Math.abs(hood.getSelectedSensorPosition() - desiredHoodAngle);
return diff < 5 ? ModuleState.READY
: diff < 10 ? ModuleState.ALMOST
return diff < 5 ? ModuleState.HIGH_TOLERANCE
: diff < 10 ? ModuleState.LOW_TOLERANCE
: ModuleState.UNALIGNED;
}

/**
* Gets the current state of the turret by taking the lowest state of all of its modules.
* Ex: UNALIGNED, ALMOST, READY -> UNALIGNED
* Ex. ALMOST, ALMOST, READY -> ALMOST
* Ex: UNALIGNED, LOW_TOLERANCE, HIGH_TOLERANCE -> UNALIGNED
* Ex. LOW_TOLERANCE, LOW_TOLERANCE, HIGH_TOLERANCE -> LOW_TOLERANCE
* @return The state of the turret.
*/
public ModuleState getState() {
Expand All @@ -329,8 +331,8 @@ public ModuleState getState() {

// TODO: is there a better way to implement this?
return flywheelState == ModuleState.UNALIGNED || turntableState == ModuleState.UNALIGNED || hoodState == ModuleState.UNALIGNED ? ModuleState.UNALIGNED
: flywheelState == ModuleState.ALMOST || turntableState == ModuleState.ALMOST || hoodState == ModuleState.ALMOST ? ModuleState.ALMOST
: ModuleState.READY;
: flywheelState == ModuleState.LOW_TOLERANCE || turntableState == ModuleState.LOW_TOLERANCE || hoodState == ModuleState.LOW_TOLERANCE ? ModuleState.LOW_TOLERANCE
: ModuleState.HIGH_TOLERANCE;
}

/**
Expand Down
154 changes: 101 additions & 53 deletions src/main/java/frc/robot/subsystems/internals/InternalSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,13 +40,23 @@ public class InternalSubsystem extends GRTSubsystem {

private final ColorMatch colorMatcher;
private final Timer exitTimer;
private final Timer entranceTimer;
private final Timer storageTimer;

// Subsystem ball states
private int entranceStorageBallCount = 0;
private int storageStagingBallCount = 0;
private int stagingExitBallCount = 0;
private int totalBallCount = 0;

private boolean prevEntranceDetected = false;
// private boolean prevStorageDetected = false;
// private boolean prevStagingDetected = false;

private boolean shotRequested = false;
private boolean rejecting = false;
private boolean rejectingChecked = false;
private int ballCount = 0;

private boolean prevEntranceDetected = false;
private boolean driverOverride = false;

public InternalSubsystem(TurretSubsystem turretSubsystem) {
Expand Down Expand Up @@ -81,93 +91,131 @@ public InternalSubsystem(TurretSubsystem turretSubsystem) {
colorMatcher.setConfidenceThreshold(0.8);

exitTimer = new Timer();
entranceTimer = new Timer();
storageTimer = new Timer();

// Set alliance color from FMS
switch (DriverStation.getAlliance()) {
case Red: ALLIANCE_COLOR = RED; break;
case Blue: ALLIANCE_COLOR = BLUE; break;
default: ALLIANCE_COLOR = RED; break;
}

// if we are in auton we start with 1 ball
if (DriverStation.isAutonomous()) {
totalBallCount = 1;
}
}

@Override
public void periodic() {
// Get last detected storage color from the sensor thread and check IR sensors for balls.

boolean entranceDetected = entrance.get() >= 0.4;
if (!prevEntranceDetected && entranceDetected) totalBallCount++;
prevEntranceDetected = entranceDetected;

Color storageColor = matchColor(colorSensorThread.getLastStorage());
//boolean entranceDetected = entrance.get() >= 0.4;
//boolean stagingDetected = staging.get() >= 0.2;
boolean storageDetected = isBall(storageColor);

double e = entrance.get();
double s = staging.get();
boolean entranceDetected = e >= 0.4;
boolean stagingDetected = s >= 0.2;
boolean stagingDetected = staging.get() >= 0.13;

/*
System.out.println("Entrance: " + e);
System.out.println("Staging: " + s);
// Testing prints, Y = ball detected, X = no ball
System.out.println("Entrance: " + (prevEntranceDetected ? "Y" : "X") +
", Storage: " + (storageDetected ? "Y" : "X") +
", Staging: " + (stagingDetected ? "Y" : "X"));

System.out.println("STORAGE matched " + (
storageColor == RED ? "RED" :
storageColor == BLUE ? "BLUE" :
storageColor == EMPTY ? "EMPTY" :
"null"));
*/

if (driverOverride) return;

// Check the entrance sensor for incoming balls and update the ball count accordingly
if (!prevEntranceDetected && entranceDetected) ballCount++;
prevEntranceDetected = entranceDetected;
// If there is a ball in the entrance, run the bottom motor.
//old condition: (entranceStorageBallCount > 0 && storageStagingBallCount == 0)
if (prevEntranceDetected && !storageDetected) {
// Spin the bottom motor on a timer
entranceTimer.start();
motorBottom.set(0.3);
}
// If 1.5 second has elapsed, stop motor
// TODO this may need more logic to make sure we actually get the ball in,
// could use storage color sensor to detect if it got caught
if (entranceTimer.hasElapsed(1.5)) {
entranceTimer.stop();
entranceTimer.reset();
motorBottom.set(0);

// If a ball has entered internals, start the bottom motor
if (entranceDetected) motorBottom.set(0.3);
prevEntranceDetected = false;
//entranceStorageBallCount--;
//storageStagingBallCount++;
System.out.println("ball moved from entrance to storage");
}

// If there is a ball between storage and staging and staging is empty, run the top and bottom motors
//old condition: (storageStagingBallCount > 0 && stagingExitBallCount == 0)
if (storageDetected && !stagingDetected && !shotRequested) {
// Spin the bottom and top motors on a timer
storageTimer.start();
motorTop.set(0.5);
motorBottom.set(0.3);

// If there is a ball in storage, start the top motor
if (isBall(storageColor)) {
// If we haven't already checked rejection logic, reject the ball if it doesn't match alliance color
if (!rejectingChecked) {
rejecting = storageColor != ALLIANCE_COLOR;
rejectingChecked = true;
}
motorTop.set(0.5);
//System.out.println("STORAGE detected, running top motor");
}
// If 0.5 seconds have elapsed, stop the motors
if (storageTimer.hasElapsed(0.5)) {
storageTimer.stop();
storageTimer.reset();
motorTop.set(0);
motorBottom.set(0);

//storageStagingBallCount--;
//stagingExitBallCount++;
System.out.println("ball moved from storage to staging");
}

// If there is a ball in staging, stop the bottom and top motors
// If there is a ball in staging, we don't want to push it into turret, especially if there is a shot going
if (stagingDetected) {
motorBottom.set(0);
motorTop.set(0);
//System.out.println("STAGING detected, stopping both motors");
}

if (turretSubsystem != null) {
// If a shot was requested and the turret is ready, load a ball into the turret.
// If rejecting, the turret can be in a semi-aligned state; otherwise, require it to be fully lined up.
turretSubsystem.setReject(rejecting);
if (shotRequested /* && turretSubsystem.getState() == TurretSubsystem.ModuleState.READY
|| rejecting && turretSubsystem.getState() == TurretSubsystem.ModuleState.ALMOST */) {
if (shotRequested /* && turretSubsystem.getState() == TurretSubsystem.ModuleState.HIGH_TOLERANCE
|| rejecting && turretSubsystem.getState() == TurretSubsystem.ModuleState.LOW_TOLERANCE */) {
// Spin the top motor on a timer
exitTimer.start();
motorTop.set(0.5);
//turretSubsystem.setFlywheelPower(0.2);
//System.out.println("Shot requested, processing");

// If 1.5 seconds have elapsed, mark the shot as finished and decrement the ball count.
if (exitTimer.hasElapsed(1.5)) {
exitTimer.stop();
exitTimer.reset();

exitTimer.start();
motorTop.set(0.5);

// If 1.5 seconds have elapsed, mark the shot as finished
if (exitTimer.hasElapsed(1)) {
exitTimer.stop();
exitTimer.reset();
motorTop.set(0);

// Reset states
shotRequested = false;
rejectingChecked = false;
//stagingExitBallCount--;
totalBallCount--;
System.out.println("ball exited turret");
}
}
}

motorTop.set(0);
//turretSubsystem.setFlywheelPower(0);
entranceStorageBallCount = prevEntranceDetected ? 1 : 0;
storageStagingBallCount = storageDetected ? 1 : 0;
stagingExitBallCount = stagingDetected ? 1 : 0;

shotRequested = false;
rejectingChecked = false;
ballCount--;
// This is really just a different version of our other ball/no ball print so
//it's not *really* needed unless we want to implement ball-counting, though in that
//case we just change that to this.
System.out.println("Entrance: " + entranceStorageBallCount +
" Storage: " + storageStagingBallCount +
" Staging: " + stagingExitBallCount);

//System.out.println("Shot finished, terminating loop");
}
}
}
}

/**
Expand All @@ -188,11 +236,11 @@ public void requestShot() {
}

/**
* Gets how many balls are inside internals.
* Gets how many total balls are inside internals.
* @return The current ball count.
*/
public int getBallCount() {
return ballCount;
return entranceStorageBallCount + storageStagingBallCount + stagingExitBallCount;
}

/**
Expand Down

0 comments on commit 741c00f

Please sign in to comment.