224 lines
11 KiB
Java
224 lines
11 KiB
Java
/* Copyright (c) 2017-2020 FIRST. All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without modification,
|
|
* are permitted (subject to the limitations in the disclaimer below) provided that
|
|
* the following conditions are met:
|
|
*
|
|
* Redistributions of source code must retain the above copyright notice, this list
|
|
* of conditions and the following disclaimer.
|
|
*
|
|
* Redistributions in binary form must reproduce the above copyright notice, this
|
|
* list of conditions and the following disclaimer in the documentation and/or
|
|
* other materials provided with the distribution.
|
|
*
|
|
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
|
|
* promote products derived from this software without specific prior written permission.
|
|
*
|
|
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
|
|
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
|
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
|
|
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
|
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
|
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
|
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
package org.firstinspires.ftc.robotcontroller.external.samples;
|
|
|
|
import android.app.Activity;
|
|
import android.graphics.Color;
|
|
import android.view.View;
|
|
|
|
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
|
import com.qualcomm.robotcore.hardware.DistanceSensor;
|
|
import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
|
|
import com.qualcomm.robotcore.hardware.NormalizedRGBA;
|
|
import com.qualcomm.robotcore.hardware.SwitchableLight;
|
|
|
|
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
|
|
|
/**
|
|
* This is an example LinearOpMode that shows how to use a color sensor in a generic
|
|
* way, regardless of which particular make or model of color sensor is used. The Op Mode
|
|
* assumes that the color sensor is configured with a name of "sensor_color".
|
|
*
|
|
* There will be some variation in the values measured depending on the specific sensor you are using.
|
|
*
|
|
* You can increase the gain (a multiplier to make the sensor report higher values) by holding down
|
|
* the A button on the gamepad, and decrease the gain by holding down the B button on the gamepad.
|
|
*
|
|
* If the color sensor has a light which is controllable from software, you can use the X button on
|
|
* the gamepad to toggle the light on and off. The REV sensors don't support this, but instead have
|
|
* a physical switch on them to turn the light on and off, beginning with REV Color Sensor V2.
|
|
*
|
|
* If the color sensor also supports short-range distance measurements (usually via an infrared
|
|
* proximity sensor), the reported distance will be written to telemetry. As of September 2020,
|
|
* the only color sensors that support this are the ones from REV Robotics. These infrared proximity
|
|
* sensor measurements are only useful at very small distances, and are sensitive to ambient light
|
|
* and surface reflectivity. You should use a different sensor if you need precise distance measurements.
|
|
*
|
|
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
|
|
* Remove or comment out the @Disabled line to add this Op Mode to the Driver Station OpMode list
|
|
*/
|
|
@TeleOp(name = "Sensor: Color", group = "Sensor")
|
|
@Disabled
|
|
public class SensorColor extends LinearOpMode {
|
|
|
|
/** The colorSensor field will contain a reference to our color sensor hardware object */
|
|
NormalizedColorSensor colorSensor;
|
|
|
|
/** The relativeLayout field is used to aid in providing interesting visual feedback
|
|
* in this sample application; you probably *don't* need this when you use a color sensor on your
|
|
* robot. Note that you won't see anything change on the Driver Station, only on the Robot Controller. */
|
|
View relativeLayout;
|
|
|
|
/**
|
|
* The runOpMode() method is the root of this Op Mode, as it is in all LinearOpModes.
|
|
* Our implementation here, though is a bit unusual: we've decided to put all the actual work
|
|
* in the runSample() method rather than directly in runOpMode() itself. The reason we do that is
|
|
* that in this sample we're changing the background color of the robot controller screen as the
|
|
* Op Mode runs, and we want to be able to *guarantee* that we restore it to something reasonable
|
|
* and palatable when the Op Mode ends. The simplest way to do that is to use a try...finally
|
|
* block around the main, core logic, and an easy way to make that all clear was to separate
|
|
* the former from the latter in separate methods.
|
|
*/
|
|
@Override public void runOpMode() {
|
|
|
|
// Get a reference to the RelativeLayout so we can later change the background
|
|
// color of the Robot Controller app to match the hue detected by the RGB sensor.
|
|
int relativeLayoutId = hardwareMap.appContext.getResources().getIdentifier("RelativeLayout", "id", hardwareMap.appContext.getPackageName());
|
|
relativeLayout = ((Activity) hardwareMap.appContext).findViewById(relativeLayoutId);
|
|
|
|
try {
|
|
runSample(); // actually execute the sample
|
|
} finally {
|
|
// On the way out, *guarantee* that the background is reasonable. It doesn't actually start off
|
|
// as pure white, but it's too much work to dig out what actually was used, and this is good
|
|
// enough to at least make the screen reasonable again.
|
|
// Set the panel back to the default color
|
|
relativeLayout.post(new Runnable() {
|
|
public void run() {
|
|
relativeLayout.setBackgroundColor(Color.WHITE);
|
|
}
|
|
});
|
|
}
|
|
}
|
|
|
|
protected void runSample() {
|
|
// You can give the sensor a gain value, will be multiplied by the sensor's raw value before the
|
|
// normalized color values are calculated. Color sensors (especially the REV Color Sensor V3)
|
|
// can give very low values (depending on the lighting conditions), which only use a small part
|
|
// of the 0-1 range that is available for the red, green, and blue values. In brighter conditions,
|
|
// you should use a smaller gain than in dark conditions. If your gain is too high, all of the
|
|
// colors will report at or near 1, and you won't be able to determine what color you are
|
|
// actually looking at. For this reason, it's better to err on the side of a lower gain
|
|
// (but always greater than or equal to 1).
|
|
float gain = 2;
|
|
|
|
// Once per loop, we will update this hsvValues array. The first element (0) will contain the
|
|
// hue, the second element (1) will contain the saturation, and the third element (2) will
|
|
// contain the value. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html
|
|
// for an explanation of HSV color.
|
|
final float[] hsvValues = new float[3];
|
|
|
|
// xButtonPreviouslyPressed and xButtonCurrentlyPressed keep track of the previous and current
|
|
// state of the X button on the gamepad
|
|
boolean xButtonPreviouslyPressed = false;
|
|
boolean xButtonCurrentlyPressed = false;
|
|
|
|
// Get a reference to our sensor object. It's recommended to use NormalizedColorSensor over
|
|
// ColorSensor, because NormalizedColorSensor consistently gives values between 0 and 1, while
|
|
// the values you get from ColorSensor are dependent on the specific sensor you're using.
|
|
colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color");
|
|
|
|
// If possible, turn the light on in the beginning (it might already be on anyway,
|
|
// we just make sure it is if we can).
|
|
if (colorSensor instanceof SwitchableLight) {
|
|
((SwitchableLight)colorSensor).enableLight(true);
|
|
}
|
|
|
|
// Wait for the start button to be pressed.
|
|
waitForStart();
|
|
|
|
// Loop until we are asked to stop
|
|
while (opModeIsActive()) {
|
|
// Explain basic gain information via telemetry
|
|
telemetry.addLine("Hold the A button on gamepad 1 to increase gain, or B to decrease it.\n");
|
|
telemetry.addLine("Higher gain values mean that the sensor will report larger numbers for Red, Green, and Blue, and Value\n");
|
|
|
|
// Update the gain value if either of the A or B gamepad buttons is being held
|
|
if (gamepad1.a) {
|
|
// Only increase the gain by a small amount, since this loop will occur multiple times per second.
|
|
gain += 0.005;
|
|
} else if (gamepad1.b && gain > 1) { // A gain of less than 1 will make the values smaller, which is not helpful.
|
|
gain -= 0.005;
|
|
}
|
|
|
|
// Show the gain value via telemetry
|
|
telemetry.addData("Gain", gain);
|
|
|
|
// Tell the sensor our desired gain value (normally you would do this during initialization,
|
|
// not during the loop)
|
|
colorSensor.setGain(gain);
|
|
|
|
// Check the status of the X button on the gamepad
|
|
xButtonCurrentlyPressed = gamepad1.x;
|
|
|
|
// If the button state is different than what it was, then act
|
|
if (xButtonCurrentlyPressed != xButtonPreviouslyPressed) {
|
|
// If the button is (now) down, then toggle the light
|
|
if (xButtonCurrentlyPressed) {
|
|
if (colorSensor instanceof SwitchableLight) {
|
|
SwitchableLight light = (SwitchableLight)colorSensor;
|
|
light.enableLight(!light.isLightOn());
|
|
}
|
|
}
|
|
}
|
|
xButtonPreviouslyPressed = xButtonCurrentlyPressed;
|
|
|
|
// Get the normalized colors from the sensor
|
|
NormalizedRGBA colors = colorSensor.getNormalizedColors();
|
|
|
|
/* Use telemetry to display feedback on the driver station. We show the red, green, and blue
|
|
* normalized values from the sensor (in the range of 0 to 1), as well as the equivalent
|
|
* HSV (hue, saturation and value) values. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html
|
|
* for an explanation of HSV color. */
|
|
|
|
// Update the hsvValues array by passing it to Color.colorToHSV()
|
|
Color.colorToHSV(colors.toColor(), hsvValues);
|
|
|
|
telemetry.addLine()
|
|
.addData("Red", "%.3f", colors.red)
|
|
.addData("Green", "%.3f", colors.green)
|
|
.addData("Blue", "%.3f", colors.blue);
|
|
telemetry.addLine()
|
|
.addData("Hue", "%.3f", hsvValues[0])
|
|
.addData("Saturation", "%.3f", hsvValues[1])
|
|
.addData("Value", "%.3f", hsvValues[2]);
|
|
telemetry.addData("Alpha", "%.3f", colors.alpha);
|
|
|
|
/* If this color sensor also has a distance sensor, display the measured distance.
|
|
* Note that the reported distance is only useful at very close range, and is impacted by
|
|
* ambient light and surface reflectivity. */
|
|
if (colorSensor instanceof DistanceSensor) {
|
|
telemetry.addData("Distance (cm)", "%.3f", ((DistanceSensor) colorSensor).getDistance(DistanceUnit.CM));
|
|
}
|
|
|
|
telemetry.update();
|
|
|
|
// Change the Robot Controller's background color to match the color detected by the color sensor.
|
|
relativeLayout.post(new Runnable() {
|
|
public void run() {
|
|
relativeLayout.setBackgroundColor(Color.HSVToColor(hsvValues));
|
|
}
|
|
});
|
|
}
|
|
}
|
|
}
|