-
Notifications
You must be signed in to change notification settings - Fork 1
/
SensorColor.java
221 lines (194 loc) · 11.4 KB
/
SensorColor.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
/* 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 OpMode shows how to use a color sensor in a generic
* way, regardless of which particular make or model of color sensor is used. The OpMode
* 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 OpMode 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 OpMode, 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
* OpMode runs, and we want to be able to *guarantee* that we restore it to something reasonable
* and palatable when the OpMode 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));
}
});
}
}
}