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

Jump to bottom

Don't reconstruct color sensor if not connected #108

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Don't reconstruct color sensor if not connected #108

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
44 changes: 13 additions & 31 deletions src/main/java/frc/robot/subsystems/RollerSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.util.Color;
Expand Down Expand Up @@ -40,7 +39,6 @@ public enum HeldPiece {
private final TrackingTimer openTimer = new TrackingTimer();
private final TrackingTimer closeTimer = new TrackingTimer();
private final TrackingTimer cooldownTimer = new TrackingTimer();
private final TrackingTimer colorTimer = new TrackingTimer();

private static final double OPEN_TIME_SECONDS = 1.0;
private static final double CLOSE_TIME_SECONDS = 0.5;
Expand All @@ -65,7 +63,7 @@ public enum HeldPiece {
//for tuning
private final ShuffleboardTab shuffleboardTab;
private final GenericEntry limitEntry, proximityEntry, colorEntry, heldPieceEntry;
private final GenericEntry rEntry, gEntry, bEntry;
private final GenericEntry rEntry, gEntry, bEntry, connectedEntry;

private double rollPower = 0.0;

Expand Down Expand Up @@ -111,6 +109,9 @@ public RollerSubsystem() {
bEntry = shuffleboardTab.add("B", 0)
.withPosition(2, 1)
.getEntry();
connectedEntry = shuffleboardTab.add("Sensor connected", 0)
.withPosition(3, 1)
.getEntry();
}

/**
Expand Down Expand Up @@ -224,50 +225,31 @@ private HeldPiece getProximitySensorPiece() {
* @return The piece detected by the color sensor.
*/
private HeldPiece getColorSensorPiece() {
// If we're not connected, return EMPTY and fall through to limit switch.
colorSensorConnected = crolorSensor.isConnected();
connectedEntry.setBoolean(colorSensorConnected);
if (!colorSensorConnected) return HeldPiece.EMPTY;

Color detectedColor = crolorSensor.getColor();
double red = detectedColor.red * 255;
double green = detectedColor.green * 255;
double blue = detectedColor.blue * 255;

// for tuning
rEntry.setValue(red);
gEntry.setValue(green);
bEntry.setValue(blue);

//if the color is 0 0 0, the sensor is broken and should be rebooted
//only do this every couple seconds
if(red == 0 && green == 0 && blue == 0){
colorSensorConnected = false;
if(!colorTimer.hasStarted()){
crolorSensor = new ColorSensorV3(I2C.Port.kMXP);
colorTimer.start();
} else if (colorTimer.advanceIfElapsed(2)){
crolorSensor = new ColorSensorV3(I2C.Port.kMXP);
}
} else {
colorSensorConnected = true;
if (colorTimer.hasStarted()) {
colorTimer.stop();
colorTimer.reset();
}
}

//calculate 3d distance between measured point and each set points
// Calculate 3d distance between measured RGB and the EMPTY, CONE, and CUBE RGB values.
double emptyDist = Math.pow(EMPTY_RED - red, 2) + Math.pow(EMPTY_GREEN - green, 2) + Math.pow(EMPTY_BLUE - blue, 2);
double coneDist = Math.pow(CONE_RED - red, 2) + Math.pow(CONE_GREEN - green, 2) + Math.pow(CONE_BLUE - blue, 2);
double cubeDist = Math.pow(CUBE_RED - red, 2) + Math.pow(CUBE_GREEN - green, 2) + Math.pow(CUBE_BLUE - blue, 2);

// System.out.println("empty - " + emptyDist + " cone - " + coneDist + " cube - " + cubeDist);

//determine that the piece is the one with the least 3d distance
if (cubeDist < coneDist && cubeDist < emptyDist){
// System.out.print("CUBE ");
// Return the piece with the least 3d distance.
if (cubeDist < coneDist && cubeDist < emptyDist) {
return HeldPiece.CUBE;
} else if (coneDist < emptyDist){
// System.out.print("CONE ");
} else if (coneDist < emptyDist) {
return HeldPiece.CONE;
} else {
// System.out.print("NONE ");
return HeldPiece.EMPTY;
}
}
Expand Down