public Team getColor() {
int r = sensorRGB_1.red() - offsetRed, b = sensorRGB_1.blue() - offsetBlue;
if (b > r && b > COLOR_THRESHOLD) {
return Team.BLUE;
} else if (r > b && r > COLOR_THRESHOLD) {
return Team.RED;
} else {
return Team.UNKNOWN;
}
}
@Override
public void runOpMode() throws InterruptedException {
// motorRF = hardwareMap.dcMotor.get("motorRF");//sets DcMotors to type of motor
// motorLF = hardwareMap.dcMotor.get("motorLF");
// motorRB = hardwareMap.dcMotor.get("motorRB");
// motorLB = hardwareMap.dcMotor.get("motorLB");
// servoPeople = hardwareMap.servo.get("servoPeople");
// motorLB.setDirection(DcMotor.Direction.REVERSE);//changes left wheels so that they turn
// correctly
// motorLF.setDirection(DcMotor.Direction.REVERSE);
RGB = hardwareMap.colorSensor.get("RGB");
// servo = hardwareMap.servo.get("servo");
// touchSensor = hardwareMap.touchSensor.get("sensor_touch");
// opticalDistanceSensor = hardwareMap.opticalDistanceSensor.get("sensor_distance");
waitForStart();
DbgLog.msg("NUMBERS HERE!!!!!");
DbgLog.msg(RGB.getDeviceName());
DbgLog.msg(RGB.getConnectionInfo());
RGB.enableLed(false);
sleep(1000);
if (RGB.alpha() >= 1 && RGB.alpha() <= 4) DbgLog.msg("BLUE!!!!!!!!!!!");
else DbgLog.msg("I DON'T KNOW!");
sleep(1000);
if (RGB.alpha() >= 9 && RGB.alpha() <= 11) DbgLog.msg("RED!!!!!!!!!!!");
else DbgLog.msg("I DON'T KNOW!");
sleep(1000);
if (RGB.alpha() >= 13) DbgLog.msg("WHITE!!!!!!!!!!!!");
else DbgLog.msg("I DON'T KNOW!");
sleep(1000);
RGB.enableLed(false);
}
public static ColorSensor create(OpMode context, ColorSensor target) {
I2cController controller;
int port;
int i2cAddr8Bit;
if (target instanceof AdafruitI2cColorSensor) {
AdafruitI2cColorSensor colorTarget = (AdafruitI2cColorSensor) target;
controller = colorTarget.getI2cController();
port = colorTarget.getPort();
i2cAddr8Bit = ADDRESS_I2C;
} else
throw new IllegalArgumentException(
String.format("incorrect color sensor class: %s", target.getClass().getSimpleName()));
return create(context, controller, port, i2cAddr8Bit, target);
}
@Override
public void runOpMode() throws InterruptedException {
// hsvValues is an array that will hold the hue, saturation, and value information.
float hsvValues[] = {0F, 0F, 0F};
// values is a reference to the hsvValues array.
final float values[] = hsvValues;
// get a reference to the RelativeLayout so we can change the background
// color of the Robot Controller app to match the hue detected by the RGB sensor.
final View relativeLayout =
((Activity) hardwareMap.appContext).findViewById(R.id.RelativeLayout);
// bPrevState and bCurrState represent the previous and current state of the button.
boolean bPrevState = false;
boolean bCurrState = false;
// bLedOn represents the state of the LED.
boolean bLedOn = true;
// get a reference to our DeviceInterfaceModule object.
cdim = hardwareMap.deviceInterfaceModule.get("dim");
// set the digital channel to output mode.
// remember, the Adafruit sensor is actually two devices.
// It's an I2C sensor and it's also an LED that can be turned on or off.
cdim.setDigitalChannelMode(LED_CHANNEL, DigitalChannelController.Mode.OUTPUT);
// get a reference to our ColorSensor object.
sensorRGB = hardwareMap.colorSensor.get("color");
// turn the LED on in the beginning, just so user will know that the sensor is active.
cdim.setDigitalChannelState(LED_CHANNEL, bLedOn);
// wait for the start button to be pressed.
waitForStart();
// loop and read the RGB data.
// Note we use opModeIsActive() as our loop condition because it is an interruptible method.
while (opModeIsActive()) {
// check the status of the x button on gamepad.
bCurrState = gamepad1.x;
// check for button-press state transitions.
if ((bCurrState == true) && (bCurrState != bPrevState)) {
// button is transitioning to a pressed state. Toggle the LED.
bLedOn = !bLedOn;
cdim.setDigitalChannelState(LED_CHANNEL, bLedOn);
}
// update previous state variable.
bPrevState = bCurrState;
// convert the RGB values to HSV values.
Color.RGBToHSV(
(sensorRGB.red() * 255) / 800,
(sensorRGB.green() * 255) / 800,
(sensorRGB.blue() * 255) / 800,
hsvValues);
// send the info back to driver station using telemetry function.
telemetry.addData("LED", bLedOn ? "On" : "Off");
telemetry.addData("Clear", sensorRGB.alpha());
telemetry.addData("Red ", sensorRGB.red());
telemetry.addData("Green", sensorRGB.green());
telemetry.addData("Blue ", sensorRGB.blue());
telemetry.addData("Hue", hsvValues[0]);
// change the background color to match the color detected by the RGB sensor.
// pass a reference to the hue, saturation, and value array as an argument
// to the HSVToColor method.
relativeLayout.post(
new Runnable() {
public void run() {
relativeLayout.setBackgroundColor(Color.HSVToColor(0xff, values));
}
});
telemetry.update();
idle(); // Always call idle() at the bottom of your while(opModeIsActive()) loop
}
}
public void calibrateColors() {
offsetRed = sensorRGB_1.red() + 200;
offsetGreen = sensorRGB_1.green();
offsetBlue = sensorRGB_1.blue();
}
@Override
public void runOpMode() throws InterruptedException {
// write some device information (connection info, name and type)
// to the log file.
hardwareMap.logDevices();
// get a reference to our ColorSensor object.
sensorRGB = hardwareMap.colorSensor.get("cs");
// bEnabled represents the state of the LED.
boolean bEnabled = true;
// turn the LED on in the beginning, just so user will know that the sensor is active.
sensorRGB.enableLed(true);
// wait one cycle.
waitOneFullHardwareCycle();
// wait for the start button to be pressed.
waitForStart();
// hsvValues is an array that will hold the hue, saturation, and value information.
float hsvValues[] = {0F, 0F, 0F};
// values is a reference to the hsvValues array.
final float values[] = hsvValues;
// get a reference to the RelativeLayout so we can change the background
// color of the Robot Controller app to match the hue detected by the RGB sensor.
final View relativeLayout =
((Activity) hardwareMap.appContext).findViewById(R.id.RelativeLayout);
// bPrevState and bCurrState represent the previous and current state of the button.
boolean bPrevState = false;
boolean bCurrState = false;
// while the op mode is active, loop and read the RGB data.
// Note we use opModeIsActive() as our loop condition because it is an interruptible method.
while (opModeIsActive()) {
// check the status of the x button on either gamepad.
bCurrState = gamepad1.x || gamepad2.x;
// check for button state transitions.
if (bCurrState == true && bCurrState != bPrevState) {
// button is transitioning to a pressed state.
// print a debug statement.
DbgLog.msg("MY_DEBUG - x button was pressed!");
// update previous state variable.
bPrevState = bCurrState;
// on button press, enable the LED.
bEnabled = true;
// turn on the LED.
sensorRGB.enableLed(bEnabled);
} else if (bCurrState == false && bCurrState != bPrevState) {
// button is transitioning to a released state.
// print a debug statement.
DbgLog.msg("MY_DEBUG - x button was released!");
// update previous state variable.
bPrevState = bCurrState;
// on button press, enable the LED.
bEnabled = false;
// turn off the LED.
sensorRGB.enableLed(false);
}
// convert the RGB values to HSV values.
Color.RGBToHSV(sensorRGB.red(), sensorRGB.green(), sensorRGB.blue(), hsvValues);
// send the info back to driver station using telemetry function.
telemetry.addData("Clear", sensorRGB.alpha());
telemetry.addData("Red ", sensorRGB.red());
telemetry.addData("Green", sensorRGB.green());
telemetry.addData("Blue ", sensorRGB.blue());
telemetry.addData("Hue", hsvValues[0]);
// change the background color to match the color detected by the RGB sensor.
// pass a reference to the hue, saturation, and value array as an argument
// to the HSVToColor method.
relativeLayout.post(
new Runnable() {
public void run() {
relativeLayout.setBackgroundColor(Color.HSVToColor(0xff, values));
}
});
// wait a hardware cycle before iterating.
waitOneFullHardwareCycle();
}
}
public String GetColor() {
String response = "Unknown";
RGBSensor.enableLed(true);
if (RGBSensor.blue() > RGBSensor.red() && RGBSensor.blue() > RGBSensor.green()) {
response = "Blue";
} else if (RGBSensor.green() > RGBSensor.red() && RGBSensor.green() > RGBSensor.blue()) {
response = "Green";
} else if (RGBSensor.red() > RGBSensor.blue() && RGBSensor.red() > RGBSensor.green()) {
response = "Red";
} else {
telemetry.addData("Error: Color = ", response);
}
RGBSensor.enableLed(false);
return response;
}
@Override
public void main() throws InterruptedException {
// write some device information (connection info, name and type)
// to the log file.
hardwareMap.logDevices();
// get a reference to our DeviceInterfaceModule object.
cdim = hardwareMap.deviceInterfaceModule.get("dim");
// set the digital channel to output mode.
// remember, the Adafruit sensor is actually two devices.
// It's an I2C sensor and it's also an LED that can be turned on or off.
cdim.setDigitalChannelMode(LED_CHANNEL, DigitalChannelController.Mode.OUTPUT);
// get a reference to our ColorSensor object.
sensorRGB = hardwareMap.colorSensor.get("color");
// bEnabled represents the state of the LED.
boolean bEnabled = true;
// turn the LED on in the beginning, just so user will know that the sensor is active.
cdim.setDigitalChannelState(LED_CHANNEL, bEnabled);
// Set up our dashboard computations
composeDashboard();
// wait for the start button to be pressed.
waitForStart();
// hsvValues is an array that will hold the hue, saturation, and value information.
float hsvValues[] = {0F, 0F, 0F};
// values is a reference to the hsvValues array.
final float values[] = hsvValues;
// get a reference to the RelativeLayout so we can change the background
// color of the Robot Controller app to match the hue detected by the RGB sensor.
final View relativeLayout =
((Activity) hardwareMap.appContext).findViewById(R.id.RelativeLayout);
// bPrevState and bCurrState represent the previous and current state of the button.
boolean bPrevState = false;
boolean bCurrState = false;
// while the op mode is active, loop and read the RGB data.
// Note we use opModeIsActive() as our loop condition because it is an interruptible method.
while (opModeIsActive()) {
// check the status of the x button on either gamepad.
bCurrState = gamepad1.x || gamepad2.x;
// check for button state transitions.
if (bCurrState == true && bCurrState != bPrevState) {
// button is transitioning to a pressed state.
// print a debug statement.
DbgLog.msg("MY_DEBUG - x button was pressed!");
// update previous state variable.
bPrevState = bCurrState;
// on button press, enable the LED.
bEnabled = true;
// turn on the LED.
cdim.setDigitalChannelState(LED_CHANNEL, bEnabled);
} else if (bCurrState == false && bCurrState != bPrevState) {
// button is transitioning to a released state.
// print a debug statement.
DbgLog.msg("MY_DEBUG - x button was released!");
// update previous state variable.
bPrevState = bCurrState;
// on button press, enable the LED.
bEnabled = false;
// turn off the LED.
cdim.setDigitalChannelState(LED_CHANNEL, bEnabled);
}
// convert the RGB values to HSV values.
Color.RGBToHSV(
(sensorRGB.red() * 255) / 800,
(sensorRGB.green() * 255) / 800,
(sensorRGB.blue() * 255) / 800,
hsvValues);
this.hue = hsvValues[0];
// change the background color to match the color detected by the RGB sensor.
// pass a reference to the hue, saturation, and value array as an argument
// to the HSVToColor method.
relativeLayout.post(
new Runnable() {
public void run() {
relativeLayout.setBackgroundColor(Color.HSVToColor(0xff, values));
}
});
this.telemetry.update();
this.idle();
}
}
public void handleBeacon() {
int colorState = 1;
switch (colorState) {
case 1:
// Find color of beacon.
if (color.red() > color.blue()) {
// Color is red.
beaconRed = true;
// Deploy left pusher.
beacon.rightStow();
beacon.colorDeploy();
} else {
// Color is blue.
beaconRed = false;
// Deploy right pusher.
beacon.leftStow();
beacon.rightDeploy();
}
colorState = 2;
break;
case 2:
// Make sure pushers are in correct positions.
if (beaconRed) {
boolean leftDeployed =
(leftPusher.getPosition() == NeverlandServoConstants.LEFT_PUSHER_DEPLOYED);
boolean rightStowed =
(rightPusher.getPosition() == NeverlandServoConstants.RIGHT_PUSHER_STOWED);
if (leftDeployed && rightStowed) {
colorState = 3;
}
} else {
boolean leftStowed =
(leftPusher.getPosition() == NeverlandServoConstants.LEFT_PUSHER_STOWED);
boolean rightDeployed =
(rightPusher.getPosition() == NeverlandServoConstants.RIGHT_PUSHER_DEPLOYED);
if (leftStowed && rightDeployed) {
colorState = 3;
}
}
break;
// Case 3: Press button, dispense climbers, and move backwards.
case 3:
int beacon = 1;
switch (beacon) {
case 1:
moveState(1.0, 8, beacon, 2);
break;
case 2:
dispenseClimbers(beacon, 3);
break;
case 3:
moveState(-1.0, 8, beacon, 4);
break;
default:
// End of switch (or bug).
break;
}
break;
default:
// End or bug.
break;
}
}
