-
Notifications
You must be signed in to change notification settings - Fork 2
/
ConceptTelemetry.java
178 lines (156 loc) · 7.28 KB
/
ConceptTelemetry.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
/* Copyright (c) 2017 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 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.VoltageSensor;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.Func;
import org.firstinspires.ftc.robotcore.external.Telemetry;
/**
* {@link ConceptTelemetry} illustrates various ways in which telemetry can be
* transmitted from the robot controller to the driver station. The sample illustrates
* numeric and text data, formatted output, and optimized evaluation of expensive-to-acquire
* information. The telemetry {@link Telemetry#log() log} is illustrated by scrolling a poem
* to the driver station.
*
* @see Telemetry
*/
@TeleOp(name = "Concept: Telemetry", group = "Concept")
@Disabled
public class ConceptTelemetry extends LinearOpMode {
/** keeps track of the line of the poem which is to be emitted next */
int poemLine = 0;
/** keeps track of how long it's been since we last emitted a line of poetry */
ElapsedTime poemElapsed = new ElapsedTime();
static final String[] poem = new String[] {
"Mary had a little lamb,",
"His fleece was white as snow,",
"And everywhere that Mary went,",
"The lamb was sure to go.",
"",
"He followed her to school one day,",
"Which was against the rule,",
"It made the children laugh and play",
"To see a lamb at school.",
"",
"And so the teacher turned it out,",
"But still it lingered near,",
"And waited patiently about,",
"Till Mary did appear.",
"",
"\"Why does the lamb love Mary so?\"",
"The eager children cry.",
"\"Why, Mary loves the lamb, you know,\"",
"The teacher did reply.",
"",
""
};
@Override public void runOpMode() {
/* we keep track of how long it's been since the OpMode was started, just
* to have some interesting data to show */
ElapsedTime opmodeRunTime = new ElapsedTime();
// We show the log in oldest-to-newest order, as that's better for poetry
telemetry.log().setDisplayOrder(Telemetry.Log.DisplayOrder.OLDEST_FIRST);
// We can control the number of lines shown in the log
telemetry.log().setCapacity(6);
// The interval between lines of poetry, in seconds
double sPoemInterval = 0.6;
/**
* Wait until we've been given the ok to go. For something to do, we emit the
* elapsed time as we sit here and wait. If we didn't want to do anything while
* we waited, we would just call {@link #waitForStart()}.
*/
while (!isStarted()) {
telemetry.addData("time", "%.1f seconds", opmodeRunTime.seconds());
telemetry.update();
idle();
}
// Ok, we've been given the ok to go
/**
* As an illustration, the first line on our telemetry display will display the battery voltage.
* The idea here is that it's expensive to compute the voltage (at least for purposes of illustration)
* so you don't want to do it unless the data is <em>actually</em> going to make it to the
* driver station (recall that telemetry transmission is throttled to reduce bandwidth use.
* Note that getBatteryVoltage() below returns 'Infinity' if there's no voltage sensor attached.
*
* @see Telemetry#getMsTransmissionInterval()
*/
telemetry.addData("voltage", "%.1f volts", new Func<Double>() {
@Override public Double value() {
return getBatteryVoltage();
}
});
// Reset to keep some timing stats for the post-'start' part of the opmode
opmodeRunTime.reset();
int loopCount = 1;
// Go go gadget robot!
while (opModeIsActive()) {
// Emit poetry if it's been a while
if (poemElapsed.seconds() > sPoemInterval) {
emitPoemLine();
}
// As an illustration, show some loop timing information
telemetry.addData("loop count", loopCount);
telemetry.addData("ms/loop", "%.3f ms", opmodeRunTime.milliseconds() / loopCount);
// Show joystick information as some other illustrative data
telemetry.addLine("left joystick | ")
.addData("x", gamepad1.left_stick_x)
.addData("y", gamepad1.left_stick_y);
telemetry.addLine("right joystick | ")
.addData("x", gamepad1.right_stick_x)
.addData("y", gamepad1.right_stick_y);
/**
* Transmit the telemetry to the driver station, subject to throttling.
* @see Telemetry#getMsTransmissionInterval()
*/
telemetry.update();
/** Update loop info and play nice with the rest of the {@link Thread}s in the system */
loopCount++;
}
}
// emits a line of poetry to the telemetry log
void emitPoemLine() {
telemetry.log().add(poem[poemLine]);
poemLine = (poemLine+1) % poem.length;
poemElapsed.reset();
}
// Computes the current battery voltage
double getBatteryVoltage() {
double result = Double.POSITIVE_INFINITY;
for (VoltageSensor sensor : hardwareMap.voltageSensor) {
double voltage = sensor.getVoltage();
if (voltage > 0) {
result = Math.min(result, voltage);
}
}
return result;
}
}