Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

2-ball Internals #20

Merged
merged 7 commits into from
Feb 19, 2022
Merged

2-ball Internals #20

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
04d4f47
2-ball internals code, turret enum renaming
ky28059 Feb 18, 2022
1496eef
Fix motor conditions
ky28059 Feb 18, 2022
90cd7e1
Testing code
ky28059 Feb 18, 2022
9d44eba
Fix infinite storage state updates
ky28059 Feb 18, 2022
0ce8e27
more print statements + failsafes, will probably rewrite but just to …
eggaskin Feb 19, 2022
d65ad69
internals, but now on transition timers
eggaskin Feb 19, 2022
cd2ccc2
auton 1-ball start & total ball count
eggaskin Feb 19, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
131 changes: 71 additions & 60 deletions src/main/java/frc/robot/subsystems/internals/InternalSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,12 +41,19 @@ public class InternalSubsystem extends GRTSubsystem {
private final ColorMatch colorMatcher;
private final Timer exitTimer;

// Subsystem ball states
private int entranceStorageBallCount = 0;
private int storageStagingBallCount = 0;
private int stagingExitBallCount = 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 @@ -92,80 +99,84 @@ public InternalSubsystem(TurretSubsystem turretSubsystem) {

@Override
public void periodic() {
// Get last detected storage color from the sensor thread and check IR sensors for balls.
Color storageColor = matchColor(colorSensorThread.getLastStorage());
//boolean entranceDetected = entrance.get() >= 0.4;
//boolean stagingDetected = staging.get() >= 0.2;

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

/*
System.out.println("Entrance: " + e);
System.out.println("Staging: " + s);

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++;
// Check sensors for incoming and exiting balls and update ball counts accordingly
boolean entranceDetected = entrance.get() >= 0.4;
if (!prevEntranceDetected && entranceDetected) {
entranceStorageBallCount++;
System.out.println("entrance ball detected");
//TODO shouldn't this set rejectingChecked to false?
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It should not, because we only want to tell the turret to reject based on the color of the first ball. That's why rejectingChecked exists in the first place -- to not have a second ball in storage override the rejection logic for the first ball.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

oop i thought i deleted that TODO but yes, i did follow that logic

}
prevEntranceDetected = entranceDetected;

// If a ball has entered internals, start the bottom motor
if (entranceDetected) motorBottom.set(0.3);

// If there is a ball in storage, start the top motor
if (isBall(storageColor)) {
Color storageColor = matchColor(colorSensorThread.getLastStorage());
//System.out.println("storage color:: " + colorToString(storageColor));
boolean storageDetected = isBall(storageColor);
if (storageDetected) System.out.println("storage ball detected");
if (!prevStorageDetected && storageDetected) {
// 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");
entranceStorageBallCount--;
storageStagingBallCount++;
System.out.println();
System.out.println("new storage detected, - entrance + storage from prev storage detected");
}

// If there is a ball in staging, stop the bottom and top motors
if (stagingDetected) {
motorBottom.set(0);
motorTop.set(0);
//System.out.println("STAGING detected, stopping both motors");
prevStorageDetected = storageDetected;

boolean stagingDetected = staging.get() >= 0.13;
if (stagingDetected) System.out.println("staging ball detected");
if (!prevStagingDetected && stagingDetected) {
storageStagingBallCount--;
stagingExitBallCount++;
System.out.println("ball moved from storage to staging");
}
prevStagingDetected = stagingDetected;

System.out.println("Ent: " + entranceStorageBallCount + " Sto: " + storageStagingBallCount + " Sta: " + stagingExitBallCount);

if (driverOverride) return;

// If there is a ball in the entrance or between storage and staging *and* staging is empty, run the bottom motor.
// This serves to bring the first ball properly from storage to staging. For the second ball, the right hand side
// of the condition will evaluate to false so it will be stopped when the ball reaches storage.
motorBottom.set((entranceStorageBallCount > 0 && storageStagingBallCount == 0)
|| (storageStagingBallCount > 0 && stagingExitBallCount == 0)
? 0.3 : 0);

// If there is a ball between storage and staging and staging is empty, run the top motor
motorTop.set((storageStagingBallCount > 0 && stagingExitBallCount == 0) ? 0.5 : 0);

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();

motorTop.set(0);
//turretSubsystem.setFlywheelPower(0);

if (!stagingDetected) {
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We shouldn't set shotRequested to false if there's not a ball in staging. The idea of shotRequested is that the driver tells internals to shoot by setting the boolean to true, and internals will fire the ball as soon as everything is ready; shotRequested is just a way of saying "please shoot the next ball you get". Setting it to false if there's not a staging ball would break being able to press it when internals isn't fully ready yet.

shotRequested = false;
rejectingChecked = false;
ballCount--;

//System.out.println("Shot finished, terminating loop");
System.out.println("false alarm");
} else {
exitTimer.start();
motorTop.set(0.5);

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

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

}
}
}
Expand All @@ -188,11 +199,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