() {
+ @Override public String value() {
+ return String.format(Locale.getDefault(), "%.3f",
+ Math.sqrt(gravity.xAccel*gravity.xAccel
+ + gravity.yAccel*gravity.yAccel
+ + gravity.zAccel*gravity.zAccel));
+ }
+ });
+ }
+
+ //----------------------------------------------------------------------------------------------
+ // Formatting
+ //----------------------------------------------------------------------------------------------
+
+ String formatAngle(AngleUnit angleUnit, double angle) {
+ return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
+ }
+
+ String formatDegrees(double degrees){
+ return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
+ }
+}
diff --git a/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java
new file mode 100644
index 0000000..064e8a4
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java
@@ -0,0 +1,232 @@
+/* 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.hardware.adafruit.AdafruitBNO055IMU;
+import com.qualcomm.hardware.bosch.BNO055IMU;
+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.util.ReadWriteFile;
+
+import org.firstinspires.ftc.robotcore.external.Func;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+
+import java.io.File;
+import java.util.Locale;
+
+/**
+ * {@link SensorBNO055IMUCalibration} calibrates the IMU accelerometer per
+ * "Section 3.11 Calibration" of the BNO055 specification.
+ *
+ * Manual calibration of the IMU is definitely NOT necessary: except for the magnetometer
+ * (which is not used by the default {@link BNO055IMU.SensorMode#IMU
+ * SensorMode#IMU}), the BNO055 is internally self-calibrating and thus can be very successfully
+ * used without manual intervention. That said, performing a one-time calibration, saving the
+ * results persistently, then loading them again at each run can help reduce the time that automatic
+ * calibration requires.
+ *
+ * This summary of the calibration process, from
+ * Intel, is informative:
+ *
+ * "This device requires calibration in order to operate accurately. [...] Calibration data is
+ * lost on a power cycle. See one of the examples for a description of how to calibrate the device,
+ * but in essence:
+ *
+ * There is a calibration status register available [...] that returns the calibration status
+ * of the accelerometer (ACC), magnetometer (MAG), gyroscope (GYR), and overall system (SYS).
+ * Each of these values range from 0 (uncalibrated) to 3 (fully calibrated). Calibration [ideally]
+ * involves certain motions to get all 4 values at 3. The motions are as follows (though see the
+ * datasheet for more information):
+ *
+ *
+ * GYR: Simply let the sensor sit flat for a few seconds.
+ * ACC: Move the sensor in various positions. Start flat, then rotate slowly by 45
+ * degrees, hold for a few seconds, then continue rotating another 45 degrees and
+ * hold, etc. 6 or more movements of this type may be required. You can move through
+ * any axis you desire, but make sure that the device is lying at least once
+ * perpendicular to the x, y, and z axis.
+ * MAG: Move slowly in a figure 8 pattern in the air, until the calibration values reaches 3.
+ * SYS: This will usually reach 3 when the other items have also reached 3. If not, continue
+ * slowly moving the device though various axes until it does."
+ *
+ *
+ * To calibrate the IMU, run this sample opmode with a gamepad attached to the driver station.
+ * Once the IMU has reached sufficient calibration as reported on telemetry, press the 'A'
+ * button on the gamepad to write the calibration to a file. That file can then be indicated
+ * later when running an opmode which uses the IMU.
+ *
+ * Note: if your intended uses of the IMU do not include use of all its sensors (for exmaple,
+ * you might not use the magnetometer), then it makes little sense for you to wait for full
+ * calibration of the sensors you are not using before saving the calibration data. Indeed,
+ * it appears that in a SensorMode that doesn't use the magnetometer (for example), the
+ * magnetometer cannot actually be calibrated.
+ *
+ * @see AdafruitBNO055IMU
+ * @see BNO055IMU.Parameters#calibrationDataFile
+ * @see BNO055 product page
+ * @see BNO055 specification
+ */
+@TeleOp(name = "Sensor: BNO055 IMU Calibration", group = "Sensor")
+@Disabled // Uncomment this to add to the opmode list
+public class SensorBNO055IMUCalibration extends LinearOpMode
+ {
+ //----------------------------------------------------------------------------------------------
+ // State
+ //----------------------------------------------------------------------------------------------
+
+ // Our sensors, motors, and other devices go here, along with other long term state
+ BNO055IMU imu;
+
+ // State used for updating telemetry
+ Orientation angles;
+
+ //----------------------------------------------------------------------------------------------
+ // Main logic
+ //----------------------------------------------------------------------------------------------
+
+ @Override public void runOpMode() {
+
+ telemetry.log().setCapacity(12);
+ telemetry.log().add("");
+ telemetry.log().add("Please refer to the calibration instructions");
+ telemetry.log().add("contained in the Adafruit IMU calibration");
+ telemetry.log().add("sample opmode.");
+ telemetry.log().add("");
+ telemetry.log().add("When sufficient calibration has been reached,");
+ telemetry.log().add("press the 'A' button to write the current");
+ telemetry.log().add("calibration data to a file.");
+ telemetry.log().add("");
+
+ // We are expecting the IMU to be attached to an I2C port on a Core Device Interface Module and named "imu".
+ BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+ parameters.loggingEnabled = true;
+ parameters.loggingTag = "IMU";
+ imu = hardwareMap.get(BNO055IMU.class, "imu");
+ imu.initialize(parameters);
+
+ composeTelemetry();
+ telemetry.log().add("Waiting for start...");
+
+ // Wait until we're told to go
+ while (!isStarted()) {
+ telemetry.update();
+ idle();
+ }
+
+ telemetry.log().add("...started...");
+
+ while (opModeIsActive()) {
+
+ if (gamepad1.a) {
+
+ // Get the calibration data
+ BNO055IMU.CalibrationData calibrationData = imu.readCalibrationData();
+
+ // Save the calibration data to a file. You can choose whatever file
+ // name you wish here, but you'll want to indicate the same file name
+ // when you initialize the IMU in an opmode in which it is used. If you
+ // have more than one IMU on your robot, you'll of course want to use
+ // different configuration file names for each.
+ String filename = "AdafruitIMUCalibration.json";
+ File file = AppUtil.getInstance().getSettingsFile(filename);
+ ReadWriteFile.writeFile(file, calibrationData.serialize());
+ telemetry.log().add("saved to '%s'", filename);
+
+ // Wait for the button to be released
+ while (gamepad1.a) {
+ telemetry.update();
+ idle();
+ }
+ }
+
+ telemetry.update();
+ }
+ }
+
+ void composeTelemetry() {
+
+ // At the beginning of each telemetry update, grab a bunch of data
+ // from the IMU that we will then display in separate lines.
+ telemetry.addAction(new Runnable() { @Override public void run()
+ {
+ // Acquiring the angles is relatively expensive; we don't want
+ // to do that in each of the three items that need that info, as that's
+ // three times the necessary expense.
+ angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
+ }
+ });
+
+ telemetry.addLine()
+ .addData("status", new Func() {
+ @Override public String value() {
+ return imu.getSystemStatus().toShortString();
+ }
+ })
+ .addData("calib", new Func() {
+ @Override public String value() {
+ return imu.getCalibrationStatus().toString();
+ }
+ });
+
+ telemetry.addLine()
+ .addData("heading", new Func() {
+ @Override public String value() {
+ return formatAngle(angles.angleUnit, angles.firstAngle);
+ }
+ })
+ .addData("roll", new Func() {
+ @Override public String value() {
+ return formatAngle(angles.angleUnit, angles.secondAngle);
+ }
+ })
+ .addData("pitch", new Func() {
+ @Override public String value() {
+ return formatAngle(angles.angleUnit, angles.thirdAngle);
+ }
+ });
+ }
+
+ //----------------------------------------------------------------------------------------------
+ // Formatting
+ //----------------------------------------------------------------------------------------------
+
+ String formatAngle(AngleUnit angleUnit, double angle) {
+ return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
+ }
+
+ String formatDegrees(double degrees){
+ return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
+ }
+}
\ No newline at end of file
diff --git a/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java
new file mode 100644
index 0000000..8209b83
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java
@@ -0,0 +1,223 @@
+/* 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));
+ }
+ });
+ }
+ }
+}
diff --git a/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java
new file mode 100644
index 0000000..b367924
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java
@@ -0,0 +1,88 @@
+/* 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.DigitalChannel;
+
+/*
+ * This is an example LinearOpMode that shows how to use
+ * a REV Robotics Touch Sensor.
+ *
+ * It assumes that the touch sensor is configured with a name of "sensor_digital".
+ *
+ * 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: Digital touch", group = "Sensor")
+@Disabled
+public class SensorDigitalTouch extends LinearOpMode {
+ /**
+ * The REV Robotics Touch Sensor
+ * is treated as a digital channel. It is HIGH if the button is unpressed.
+ * It pulls LOW if the button is pressed.
+ *
+ * Also, when you connect a REV Robotics Touch Sensor to the digital I/O port on the
+ * Expansion Hub using a 4-wire JST cable, the second pin gets connected to the Touch Sensor.
+ * The lower (first) pin stays unconnected.*
+ */
+
+ DigitalChannel digitalTouch; // Hardware Device Object
+
+ @Override
+ public void runOpMode() {
+
+ // get a reference to our digitalTouch object.
+ digitalTouch = hardwareMap.get(DigitalChannel.class, "sensor_digital");
+
+ // set the digital channel to input.
+ digitalTouch.setMode(DigitalChannel.Mode.INPUT);
+
+ // wait for the start button to be pressed.
+ waitForStart();
+
+ // while the op mode is active, loop and read the light levels.
+ // Note we use opModeIsActive() as our loop condition because it is an interruptible method.
+ while (opModeIsActive()) {
+
+ // send the info back to driver station using telemetry function.
+ // if the digital channel returns true it's HIGH and the button is unpressed.
+ if (digitalTouch.getState() == true) {
+ telemetry.addData("Digital Touch", "Is Not Pressed");
+ } else {
+ telemetry.addData("Digital Touch", "Is Pressed");
+ }
+
+ telemetry.update();
+ }
+ }
+}
diff --git a/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java
new file mode 100644
index 0000000..583f525
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java
@@ -0,0 +1,129 @@
+/* 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.hardware.kauailabs.NavxMicroNavigationSensor;
+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.Gyroscope;
+import com.qualcomm.robotcore.hardware.IntegratingGyroscope;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+
+/*
+ * This is an example LinearOpMode that shows how to use Kauai Labs navX Micro Robotics Navigation
+ * Sensor. It assumes that the sensor is configured with a name of "navx".
+ *
+ * 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: KL navX Micro", group = "Sensor")
+@Disabled
+public class SensorKLNavxMicro extends LinearOpMode {
+
+ /** In this sample, for illustration purposes we use two interfaces on the one gyro object.
+ * That's likely atypical: you'll probably use one or the other in any given situation,
+ * depending on what you're trying to do. {@link IntegratingGyroscope} (and it's base interface,
+ * {@link Gyroscope}) are common interfaces supported by possibly several different gyro
+ * implementations. {@link NavxMicroNavigationSensor}, by contrast, provides functionality that
+ * is unique to the navX Micro sensor.
+ */
+ IntegratingGyroscope gyro;
+ NavxMicroNavigationSensor navxMicro;
+
+ // A timer helps provide feedback while calibration is taking place
+ ElapsedTime timer = new ElapsedTime();
+
+ @Override public void runOpMode() throws InterruptedException {
+ // Get a reference to a Modern Robotics GyroSensor object. We use several interfaces
+ // on this object to illustrate which interfaces support which functionality.
+ navxMicro = hardwareMap.get(NavxMicroNavigationSensor.class, "navx");
+ gyro = (IntegratingGyroscope)navxMicro;
+ // If you're only interested int the IntegratingGyroscope interface, the following will suffice.
+ // gyro = hardwareMap.get(IntegratingGyroscope.class, "navx");
+
+ // The gyro automatically starts calibrating. This takes a few seconds.
+ telemetry.log().add("Gyro Calibrating. Do Not Move!");
+
+ // Wait until the gyro calibration is complete
+ timer.reset();
+ while (navxMicro.isCalibrating()) {
+ telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|");
+ telemetry.update();
+ Thread.sleep(50);
+ }
+ telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start.");
+ telemetry.clear(); telemetry.update();
+
+ // Wait for the start button to be pressed
+ waitForStart();
+ telemetry.log().clear();
+
+ while (opModeIsActive()) {
+
+ // Read dimensionalized data from the gyro. This gyro can report angular velocities
+ // about all three axes. Additionally, it internally integrates the Z axis to
+ // be able to report an absolute angular Z orientation.
+ AngularVelocity rates = gyro.getAngularVelocity(AngleUnit.DEGREES);
+ Orientation angles = gyro.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
+
+ telemetry.addLine()
+ .addData("dx", formatRate(rates.xRotationRate))
+ .addData("dy", formatRate(rates.yRotationRate))
+ .addData("dz", "%s deg/s", formatRate(rates.zRotationRate));
+
+ telemetry.addLine()
+ .addData("heading", formatAngle(angles.angleUnit, angles.firstAngle))
+ .addData("roll", formatAngle(angles.angleUnit, angles.secondAngle))
+ .addData("pitch", "%s deg", formatAngle(angles.angleUnit, angles.thirdAngle));
+ telemetry.update();
+
+ idle(); // Always call idle() at the bottom of your while(opModeIsActive()) loop
+ }
+ }
+
+ String formatRate(float rate) {
+ return String.format("%.3f", rate);
+ }
+
+ String formatAngle(AngleUnit angleUnit, double angle) {
+ return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
+ }
+
+ String formatDegrees(double degrees){
+ return String.format("%.1f", AngleUnit.DEGREES.normalize(degrees));
+ }
+}
diff --git a/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRColor.java b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRColor.java
new file mode 100644
index 0000000..efba7d1
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRColor.java
@@ -0,0 +1,139 @@
+/* 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 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.ColorSensor;
+
+/*
+ *
+ * This is an example LinearOpMode that shows how to use
+ * a Modern Robotics Color Sensor.
+ *
+ * The op mode assumes that the color sensor
+ * is configured with a name of "sensor_color".
+ *
+ * You can use the X button on gamepad1 to toggle the LED on and off.
+ *
+ * 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: MR Color", group = "Sensor")
+@Disabled
+public class SensorMRColor extends LinearOpMode {
+
+ ColorSensor colorSensor; // Hardware Device Object
+
+
+ @Override
+ public void runOpMode() {
+
+ // 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.
+ int relativeLayoutId = hardwareMap.appContext.getResources().getIdentifier("RelativeLayout", "id", hardwareMap.appContext.getPackageName());
+ final View relativeLayout = ((Activity) hardwareMap.appContext).findViewById(relativeLayoutId);
+
+ // 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 ColorSensor object.
+ colorSensor = hardwareMap.get(ColorSensor.class, "sensor_color");
+
+ // Set the LED in the beginning
+ colorSensor.enableLed(bLedOn);
+
+ // wait for the start button to be pressed.
+ waitForStart();
+
+ // 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;
+
+ // check for button state transitions.
+ if (bCurrState && (bCurrState != bPrevState)) {
+
+ // button is transitioning to a pressed state. So Toggle LED
+ bLedOn = !bLedOn;
+ colorSensor.enableLed(bLedOn);
+ }
+
+ // update previous state variable.
+ bPrevState = bCurrState;
+
+ // convert the RGB values to HSV values.
+ Color.RGBToHSV(colorSensor.red() * 8, colorSensor.green() * 8, colorSensor.blue() * 8, hsvValues);
+
+ // send the info back to driver station using telemetry function.
+ telemetry.addData("LED", bLedOn ? "On" : "Off");
+ telemetry.addData("Clear", colorSensor.alpha());
+ telemetry.addData("Red ", colorSensor.red());
+ telemetry.addData("Green", colorSensor.green());
+ telemetry.addData("Blue ", colorSensor.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();
+ }
+
+ // Set the panel back to the default color
+ relativeLayout.post(new Runnable() {
+ public void run() {
+ relativeLayout.setBackgroundColor(Color.WHITE);
+ }
+ });
+ }
+}
diff --git a/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRCompass.java b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRCompass.java
new file mode 100644
index 0000000..583ac1a
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRCompass.java
@@ -0,0 +1,148 @@
+/* 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.hardware.modernrobotics.ModernRoboticsI2cCompassSensor;
+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.CompassSensor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
+
+/**
+ * The {@link SensorMRCompass} op mode provides a demonstration of the
+ * functionality provided by the Modern Robotics compass sensor.
+ *
+ * The op mode assumes that the MR compass is configured with a name of "compass".
+ *
+ * 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
+ *
+ * @see MR Compass Sensor
+ */
+@TeleOp(name = "Sensor: MR compass", group = "Sensor")
+@Disabled // comment out or remove this line to enable this opmode
+public class SensorMRCompass extends LinearOpMode {
+
+ ModernRoboticsI2cCompassSensor compass;
+ ElapsedTime timer = new ElapsedTime();
+
+ @Override public void runOpMode() {
+
+ // get a reference to our compass
+ compass = hardwareMap.get(ModernRoboticsI2cCompassSensor.class, "compass");
+
+ telemetry.log().setCapacity(20);
+ telemetry.log().add("The compass sensor operates quite well out-of-the");
+ telemetry.log().add("box, as shipped by the manufacturer. Precision can");
+ telemetry.log().add("however be somewhat improved with calibration.");
+ telemetry.log().add("");
+ telemetry.log().add("To calibrate the compass once the opmode is");
+ telemetry.log().add("started, make sure the compass is level, then");
+ telemetry.log().add("press 'A' on the gamepad. Next, slowly rotate the ");
+ telemetry.log().add("compass in a full 360 degree circle while keeping");
+ telemetry.log().add("it level. When complete, press 'B'.");
+
+ // wait for the start button to be pressed
+ waitForStart();
+ telemetry.log().clear();
+
+ while (opModeIsActive()) {
+
+ // If the A button is pressed, start calibration and wait for the A button to rise
+ if (gamepad1.a && !compass.isCalibrating()) {
+
+ telemetry.log().clear();
+ telemetry.log().add("Calibration started");
+ telemetry.log().add("Slowly rotate compass 360deg");
+ telemetry.log().add("Press 'B' when complete");
+ compass.setMode(CompassSensor.CompassMode.CALIBRATION_MODE);
+ timer.reset();
+
+ while (gamepad1.a && opModeIsActive()) {
+ doTelemetry();
+ idle();
+ }
+ }
+
+ // If the B button is pressed, stop calibration and wait for the B button to rise
+ if (gamepad1.b && compass.isCalibrating()) {
+
+ telemetry.log().clear();
+ telemetry.log().add("Calibration complete");
+ compass.setMode(CompassSensor.CompassMode.MEASUREMENT_MODE);
+
+ if (compass.calibrationFailed()) {
+ telemetry.log().add("Calibration failed");
+ compass.writeCommand(ModernRoboticsI2cCompassSensor.Command.NORMAL);
+ }
+
+ while (gamepad1.a && opModeIsActive()) {
+ doTelemetry();
+ idle();
+ }
+ }
+
+ doTelemetry();
+ }
+ }
+
+ protected void doTelemetry() {
+
+ if (compass.isCalibrating()) {
+
+ telemetry.addData("compass", "calibrating %s", Math.round(timer.seconds())%2==0 ? "|.." : "..|");
+
+ } else {
+
+ // getDirection() returns a traditional compass heading in the range [0,360),
+ // with values increasing in a CW direction
+ telemetry.addData("heading", "%.1f", compass.getDirection());
+
+ // getAcceleration() returns the current 3D acceleration experienced by
+ // the sensor. This is used internally to the sensor to compute its tilt and thence
+ // to correct the magnetometer reading to produce tilt-corrected values in getDirection()
+ Acceleration accel = compass.getAcceleration();
+ double accelMagnitude = Math.sqrt(accel.xAccel*accel.xAccel + accel.yAccel*accel.yAccel + accel.zAccel*accel.zAccel);
+ telemetry.addData("accel", accel);
+ telemetry.addData("accel magnitude", "%.3f", accelMagnitude);
+
+ // getMagneticFlux returns the 3D magnetic field flux experienced by the sensor
+ telemetry.addData("mag flux", compass.getMagneticFlux());
+ }
+
+ // the command register provides status data
+ telemetry.addData("command", "%s", compass.readCommand());
+
+ telemetry.update();
+ }
+}
diff --git a/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRGyro.java b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRGyro.java
new file mode 100644
index 0000000..5425f0d
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRGyro.java
@@ -0,0 +1,162 @@
+/* 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.hardware.modernrobotics.ModernRoboticsI2cGyro;
+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.Gyroscope;
+import com.qualcomm.robotcore.hardware.IntegratingGyroscope;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+
+/*
+ * This is an example LinearOpMode that shows how to use the Modern Robotics Gyro.
+ *
+ * The op mode assumes that the gyro sensor is attached to a Device Interface Module
+ * I2C channel and is configured with a name of "gyro".
+ *
+ * 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: MR Gyro", group = "Sensor")
+@Disabled
+public class SensorMRGyro extends LinearOpMode {
+
+ /** In this sample, for illustration purposes we use two interfaces on the one gyro object.
+ * That's likely atypical: you'll probably use one or the other in any given situation,
+ * depending on what you're trying to do. {@link IntegratingGyroscope} (and it's base interface,
+ * {@link Gyroscope}) are common interfaces supported by possibly several different gyro
+ * implementations. {@link ModernRoboticsI2cGyro}, by contrast, provides functionality that
+ * is unique to the Modern Robotics gyro sensor.
+ */
+ IntegratingGyroscope gyro;
+ ModernRoboticsI2cGyro modernRoboticsI2cGyro;
+
+ // A timer helps provide feedback while calibration is taking place
+ ElapsedTime timer = new ElapsedTime();
+
+ @Override
+ public void runOpMode() {
+
+ boolean lastResetState = false;
+ boolean curResetState = false;
+
+ // Get a reference to a Modern Robotics gyro object. We use several interfaces
+ // on this object to illustrate which interfaces support which functionality.
+ modernRoboticsI2cGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro");
+ gyro = (IntegratingGyroscope)modernRoboticsI2cGyro;
+ // If you're only interested int the IntegratingGyroscope interface, the following will suffice.
+ // gyro = hardwareMap.get(IntegratingGyroscope.class, "gyro");
+ // A similar approach will work for the Gyroscope interface, if that's all you need.
+
+ // Start calibrating the gyro. This takes a few seconds and is worth performing
+ // during the initialization phase at the start of each OpMode.
+ telemetry.log().add("Gyro Calibrating. Do Not Move!");
+ modernRoboticsI2cGyro.calibrate();
+
+ // Wait until the gyro calibration is complete
+ timer.reset();
+ while (!isStopRequested() && modernRoboticsI2cGyro.isCalibrating()) {
+ telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|");
+ telemetry.update();
+ sleep(50);
+ }
+
+ telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start.");
+ telemetry.clear(); telemetry.update();
+
+ // Wait for the start button to be pressed
+ waitForStart();
+ telemetry.log().clear();
+ telemetry.log().add("Press A & B to reset heading");
+
+ // Loop until we're asked to stop
+ while (opModeIsActive()) {
+
+ // If the A and B buttons are pressed just now, reset Z heading.
+ curResetState = (gamepad1.a && gamepad1.b);
+ if (curResetState && !lastResetState) {
+ modernRoboticsI2cGyro.resetZAxisIntegrator();
+ }
+ lastResetState = curResetState;
+
+ // The raw() methods report the angular rate of change about each of the
+ // three axes directly as reported by the underlying sensor IC.
+ int rawX = modernRoboticsI2cGyro.rawX();
+ int rawY = modernRoboticsI2cGyro.rawY();
+ int rawZ = modernRoboticsI2cGyro.rawZ();
+ int heading = modernRoboticsI2cGyro.getHeading();
+ int integratedZ = modernRoboticsI2cGyro.getIntegratedZValue();
+
+ // Read dimensionalized data from the gyro. This gyro can report angular velocities
+ // about all three axes. Additionally, it internally integrates the Z axis to
+ // be able to report an absolute angular Z orientation.
+ AngularVelocity rates = gyro.getAngularVelocity(AngleUnit.DEGREES);
+ float zAngle = gyro.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle;
+
+ // Read administrative information from the gyro
+ int zAxisOffset = modernRoboticsI2cGyro.getZAxisOffset();
+ int zAxisScalingCoefficient = modernRoboticsI2cGyro.getZAxisScalingCoefficient();
+
+ telemetry.addLine()
+ .addData("dx", formatRate(rates.xRotationRate))
+ .addData("dy", formatRate(rates.yRotationRate))
+ .addData("dz", "%s deg/s", formatRate(rates.zRotationRate));
+ telemetry.addData("angle", "%s deg", formatFloat(zAngle));
+ telemetry.addData("heading", "%3d deg", heading);
+ telemetry.addData("integrated Z", "%3d", integratedZ);
+ telemetry.addLine()
+ .addData("rawX", formatRaw(rawX))
+ .addData("rawY", formatRaw(rawY))
+ .addData("rawZ", formatRaw(rawZ));
+ telemetry.addLine().addData("z offset", zAxisOffset).addData("z coeff", zAxisScalingCoefficient);
+ telemetry.update();
+ }
+ }
+
+ String formatRaw(int rawValue) {
+ return String.format("%d", rawValue);
+ }
+
+ String formatRate(float rate) {
+ return String.format("%.3f", rate);
+ }
+
+ String formatFloat(float rate) {
+ return String.format("%.3f", rate);
+ }
+
+}
diff --git a/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRIrSeeker.java b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRIrSeeker.java
new file mode 100644
index 0000000..9bd6452
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRIrSeeker.java
@@ -0,0 +1,84 @@
+/* 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.IrSeekerSensor;
+
+/*
+ * This is an example LinearOpMode that shows how to use
+ * the Modern Robotics ITR Seeker
+ *
+ * The op mode assumes that the IR Seeker
+ * is configured with a name of "sensor_ir".
+ *
+ * Set the switch on the Modern Robotics IR beacon to 1200 at 180.
+ * Turn on the IR beacon.
+ * Make sure the side of the beacon with the LED on is facing the robot.
+ *
+ * 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: MR IR Seeker", group = "Sensor")
+@Disabled
+public class SensorMRIrSeeker extends LinearOpMode {
+
+ @Override
+ public void runOpMode() {
+
+ IrSeekerSensor irSeeker; // Hardware Device Object
+
+ // get a reference to our GyroSensor object.
+ irSeeker = hardwareMap.get(IrSeekerSensor.class, "sensor_ir");
+
+ // wait for the start button to be pressed.
+ waitForStart();
+
+ while (opModeIsActive()) {
+
+ // Ensure we have a IR signal
+ if (irSeeker.signalDetected())
+ {
+ // Display angle and strength
+ telemetry.addData("Angle", irSeeker.getAngle());
+ telemetry.addData("Strength", irSeeker.getStrength());
+ }
+ else
+ {
+ // Display loss of signal
+ telemetry.addData("Seeker", "Signal Lost");
+ }
+
+ telemetry.update();
+ }
+ }
+}
diff --git a/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMROpticalDistance.java b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMROpticalDistance.java
new file mode 100644
index 0000000..8424ef7
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMROpticalDistance.java
@@ -0,0 +1,71 @@
+/* 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.OpticalDistanceSensor;
+
+/*
+ * This is an example LinearOpMode that shows how to use
+ * a Modern Robotics Optical Distance Sensor
+ * It assumes that the ODS sensor is configured with a name of "sensor_ods".
+ *
+ * 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: MR ODS", group = "Sensor")
+@Disabled
+public class SensorMROpticalDistance extends LinearOpMode {
+
+ OpticalDistanceSensor odsSensor; // Hardware Device Object
+
+ @Override
+ public void runOpMode() {
+
+ // get a reference to our Light Sensor object.
+ odsSensor = hardwareMap.get(OpticalDistanceSensor.class, "sensor_ods");
+
+ // wait for the start button to be pressed.
+ waitForStart();
+
+ // while the op mode is active, loop and read the light levels.
+ // Note we use opModeIsActive() as our loop condition because it is an interruptible method.
+ while (opModeIsActive()) {
+
+ // send the info back to driver station using telemetry function.
+ telemetry.addData("Raw", odsSensor.getRawLightDetected());
+ telemetry.addData("Normal", odsSensor.getLightDetected());
+
+ telemetry.update();
+ }
+ }
+}
diff --git a/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java
new file mode 100644
index 0000000..d7de1ff
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java
@@ -0,0 +1,72 @@
+/* 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.hardware.modernrobotics.ModernRoboticsI2cRangeSensor;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+/**
+ * {@link SensorMRRangeSensor} illustrates how to use the Modern Robotics
+ * Range Sensor.
+ *
+ * The op mode assumes that the range sensor is configured with a name of "sensor_range".
+ *
+ * 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
+ *
+ * @see MR Range Sensor
+ */
+@TeleOp(name = "Sensor: MR range sensor", group = "Sensor")
+@Disabled // comment out or remove this line to enable this opmode
+public class SensorMRRangeSensor extends LinearOpMode {
+
+ ModernRoboticsI2cRangeSensor rangeSensor;
+
+ @Override public void runOpMode() {
+
+ // get a reference to our compass
+ rangeSensor = hardwareMap.get(ModernRoboticsI2cRangeSensor.class, "sensor_range");
+
+ // wait for the start button to be pressed
+ waitForStart();
+
+ while (opModeIsActive()) {
+ telemetry.addData("raw ultrasonic", rangeSensor.rawUltrasonic());
+ telemetry.addData("raw optical", rangeSensor.rawOptical());
+ telemetry.addData("cm optical", "%.2f cm", rangeSensor.cmOptical());
+ telemetry.addData("cm", "%.2f cm", rangeSensor.getDistance(DistanceUnit.CM));
+ telemetry.update();
+ }
+ }
+}
diff --git a/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java
new file mode 100644
index 0000000..30bb377
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java
@@ -0,0 +1,89 @@
+/*
+Copyright (c) 2018 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.hardware.rev.Rev2mDistanceSensor;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DistanceSensor;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+/**
+ * {@link SensorREV2mDistance} illustrates how to use the REV Robotics
+ * Time-of-Flight Range Sensor.
+ *
+ * The op mode assumes that the range sensor is configured with a name of "sensor_range".
+ *
+ * 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
+ *
+ * @see REV Robotics Web Page
+ */
+@TeleOp(name = "Sensor: REV2mDistance", group = "Sensor")
+@Disabled
+public class SensorREV2mDistance extends LinearOpMode {
+
+ private DistanceSensor sensorRange;
+
+ @Override
+ public void runOpMode() {
+ // you can use this as a regular DistanceSensor.
+ sensorRange = hardwareMap.get(DistanceSensor.class, "sensor_range");
+
+ // you can also cast this to a Rev2mDistanceSensor if you want to use added
+ // methods associated with the Rev2mDistanceSensor class.
+ Rev2mDistanceSensor sensorTimeOfFlight = (Rev2mDistanceSensor)sensorRange;
+
+ telemetry.addData(">>", "Press start to continue");
+ telemetry.update();
+
+ waitForStart();
+ while(opModeIsActive()) {
+ // generic DistanceSensor methods.
+ telemetry.addData("deviceName",sensorRange.getDeviceName() );
+ telemetry.addData("range", String.format("%.01f mm", sensorRange.getDistance(DistanceUnit.MM)));
+ telemetry.addData("range", String.format("%.01f cm", sensorRange.getDistance(DistanceUnit.CM)));
+ telemetry.addData("range", String.format("%.01f m", sensorRange.getDistance(DistanceUnit.METER)));
+ telemetry.addData("range", String.format("%.01f in", sensorRange.getDistance(DistanceUnit.INCH)));
+
+ // Rev2mDistanceSensor specific methods.
+ telemetry.addData("ID", String.format("%x", sensorTimeOfFlight.getModelID()));
+ telemetry.addData("did time out", Boolean.toString(sensorTimeOfFlight.didTimeoutOccur()));
+
+ telemetry.update();
+ }
+ }
+
+}
\ No newline at end of file
diff --git a/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/readme.md b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/readme.md
new file mode 100644
index 0000000..326978d
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/readme.md
@@ -0,0 +1,45 @@
+
+## Caution
+No Team-specific code should be placed or modified in this ``.../samples`` folder.
+
+Samples should be Copied from here, and then Pasted into the team's
+[/TeamCode/src/main/java/org/firstinspires/ftc/teamcode](../../../../../../../../../../TeamCode/src/main/java/org/firstinspires/ftc/teamcode)
+ folder, using the Android Studio cut and paste commands. This automatically changes all file and
+class names to be consistent. From there, the sample can be modified to suit the team's needs.
+
+For more detailed instructions see the /teamcode readme.
+
+### Naming of Samples
+
+To gain a better understanding of how the samples are organized, and how to interpret the
+naming system, it will help to understand the conventions that were used during their creation.
+
+These conventions are described (in detail) in the sample_conventions.md file in this folder.
+
+To summarize: A range of different samples classes will reside in the java/external/samples.
+The class names will follow a naming convention which indicates the purpose of each class.
+The prefix of the name will be one of the following:
+
+Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure
+ of a particular style of OpMode. These are bare bones examples.
+
+Sensor: This is a Sample OpMode that shows how to use a specific sensor.
+ It is not intended to drive a functioning robot, it is simply showing the minimal code
+ required to read and display the sensor values.
+
+Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base.
+ It may be used to provide a common baseline driving OpMode, or
+ to demonstrate how a particular sensor or concept can be used to navigate.
+
+Concept: This is a sample OpMode that illustrates performing a specific function or concept.
+ These may be complex, but their operation should be explained clearly in the comments,
+ or the comments should reference an external doc, guide or tutorial.
+ Each OpMode should try to only demonstrate a single concept so they are easy to
+ locate based on their name. These OpModes may not produce a drivable robot.
+
+After the prefix, other conventions will apply:
+
+* Sensor class names are constructed as: Sensor - Company - Type
+* Robot class names are constructed as: Robot - Mode - Action - OpModetype
+* Concept class names are constructed as: Concept - Topic - OpModetype
+
diff --git a/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md
new file mode 100644
index 0000000..45968ef
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md
@@ -0,0 +1,108 @@
+## Sample Class/Opmode conventions
+#### V 1.1.0 8/9/2017
+
+This document defines the FTC Sample OpMode and Class conventions.
+
+### OpMode Name
+
+To gain a better understanding of how the samples are organized, and how to interpret the
+naming system, it will help to understand the conventions that were used during their creation.
+
+To summarize: A range of different samples classes will reside in the java/external/samples.
+The class names will follow a naming convention which indicates the purpose of each class.
+The prefix of the name will be one of the following:
+
+Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure
+ of a particular style of OpMode. These are bare bones examples.
+
+Sensor: This is a Sample OpMode that shows how to use a specific sensor.
+ It is not intended to drive a functioning robot, it is simply showing the minimal code
+ required to read and display the sensor values.
+
+Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base.
+ It may be used to provide a common baseline driving OpMode, or
+ to demonstrate how a particular sensor or concept can be used to navigate.
+
+Concept: This is a sample OpMode that illustrates performing a specific function or concept.
+ These may be complex, but their operation should be explained clearly in the comments,
+ or the comments should reference an external doc, guide or tutorial.
+ Each OpMode should try to only demonstrate a single concept so they are easy to
+ locate based on their name. These OpModes may not produce a drivable robot.
+
+After the prefix, other conventions will apply:
+
+* Sensor class names should constructed as: Sensor - Company - Type
+* Robot class names should be constructed as: Robot - Mode - Action - OpModetype
+* Concept class names should be constructed as: Concept - Topic - OpModetype
+
+### Sample OpMode Content/Style
+
+Code is formatted as per the Google Style Guide:
+
+https://google.github.io/styleguide/javaguide.html
+
+With “Sensor” and “Hardware” samples, the code should demonstrate the essential function,
+and not be embellished with too much additional “clever” code. If a sensor has special
+addressing needs, or has a variety of modes or outputs, these should be demonstrated as
+simply as possible.
+
+Special programming methods, or robot control techniques should be reserved for “Concept” Samples,
+and where possible, Samples should strive to only demonstrate a single concept,
+eg: State machine coding, or a User Menu system, and not combine them into a single “all inclusive”
+sample. This will prevent an “all inclusive” Sample being deleted just because one part of it
+becomes obsolete.
+
+### Device Configuration Names
+
+The following device names are used in the external samples
+
+** Motors:
+left_drive
+right_drive
+left_arm
+
+** Servos:
+left_hand
+right_hand
+arm
+claw
+
+** Sensors:
+sensor_color
+sensor_ir
+sensor_light
+sensor_ods
+sensor_range
+sensor_touch
+sensor_color_distance
+sensor_digital
+digin
+digout
+
+** Localization:
+compass
+gyro
+imu
+navx
+
+### Device Object Names
+
+Device Object names should use the same words as the device’s configuration name, but they
+should be re-structured to be a suitable Java variable name. This should keep the same word order,
+but adopt the style of beginning with a lower case letter, and then each subsequent word
+starting with an upper case letter.
+
+Eg: from the examples above: tool, leftMotor, rightClawServo, rearLightSensor.
+
+Note: Sometimes it’s helpful to put the device type first, followed by the variant.
+eg: motorLeft and motorRight, but this should only be done if the same word order
+is used on the device configuration name.
+
+### OpMode code Comments
+
+Sample comments should read like normal code comments, that is, as an explanation of what the
+sample code is doing. They should NOT be directives to the user,
+like: “insert your joystick code here” as these comments typically aren’t
+detailed enough to be useful. They also often get left in the code and become garbage.
+
+Instead, an example of the joystick code should be shown with a comment describing what it is doing.
diff --git a/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java b/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java
new file mode 100644
index 0000000..9a94f54
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java
@@ -0,0 +1,70 @@
+/* Copyright (c) 2014, 2015 Qualcomm Technologies Inc
+
+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 Qualcomm Technologies Inc 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.internal;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManager;
+import com.qualcomm.robotcore.eventloop.opmode.OpModeRegister;
+
+import org.firstinspires.ftc.robotcontroller.external.samples.ConceptNullOp;
+
+/**
+ * {@link FtcOpModeRegister} is responsible for registering opmodes for use in an FTC game.
+ * @see #register(OpModeManager)
+ */
+public class FtcOpModeRegister implements OpModeRegister {
+
+ /**
+ * {@link #register(OpModeManager)} is called by the SDK game in order to register
+ * OpMode classes or instances that will participate in an FTC game.
+ *
+ * There are two mechanisms by which an OpMode may be registered.
+ *
+ * 1) The preferred method is by means of class annotations in the OpMode itself.
+ * See, for example the class annotations in {@link ConceptNullOp}.
+ *
+ * 2) The other, retired, method is to modify this {@link #register(OpModeManager)}
+ * method to include explicit calls to OpModeManager.register().
+ * This method of modifying this file directly is discouraged, as it
+ * makes updates to the SDK harder to integrate into your code.
+ *
+ * @param manager the object which contains methods for carrying out OpMode registrations
+ *
+ * @see com.qualcomm.robotcore.eventloop.opmode.TeleOp
+ * @see com.qualcomm.robotcore.eventloop.opmode.Autonomous
+ */
+ public void register(OpModeManager manager) {
+
+ /**
+ * Any manual OpMode class registrations should go here.
+ */
+ }
+}
diff --git a/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java b/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java
new file mode 100644
index 0000000..327d3ec
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java
@@ -0,0 +1,845 @@
+/* Copyright (c) 2014, 2015 Qualcomm Technologies Inc
+
+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 Qualcomm Technologies Inc 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.internal;
+
+import android.app.ActionBar;
+import android.app.Activity;
+import android.app.ActivityManager;
+import android.content.ComponentName;
+import android.content.Context;
+import android.content.Intent;
+import android.content.ServiceConnection;
+import android.content.SharedPreferences;
+import android.content.res.Configuration;
+import android.hardware.usb.UsbDevice;
+import android.hardware.usb.UsbManager;
+import android.net.wifi.WifiManager;
+import android.os.Bundle;
+import android.os.IBinder;
+import android.preference.PreferenceManager;
+import androidx.annotation.NonNull;
+import androidx.annotation.Nullable;
+import androidx.annotation.StringRes;
+import android.view.Menu;
+import android.view.MenuItem;
+import android.view.MotionEvent;
+import android.view.View;
+import android.view.WindowManager;
+import android.webkit.WebView;
+import android.widget.ImageButton;
+import android.widget.LinearLayout;
+import android.widget.LinearLayout.LayoutParams;
+import android.widget.PopupMenu;
+import android.widget.TextView;
+
+import com.google.blocks.ftcrobotcontroller.ProgrammingWebHandlers;
+import com.google.blocks.ftcrobotcontroller.runtime.BlocksOpMode;
+import com.qualcomm.ftccommon.ClassManagerFactory;
+import com.qualcomm.ftccommon.FtcAboutActivity;
+import com.qualcomm.ftccommon.FtcEventLoop;
+import com.qualcomm.ftccommon.FtcEventLoopIdle;
+import com.qualcomm.ftccommon.FtcRobotControllerService;
+import com.qualcomm.ftccommon.FtcRobotControllerService.FtcRobotControllerBinder;
+import com.qualcomm.ftccommon.FtcRobotControllerSettingsActivity;
+import com.qualcomm.ftccommon.LaunchActivityConstantsList;
+import com.qualcomm.ftccommon.LaunchActivityConstantsList.RequestCode;
+import com.qualcomm.ftccommon.Restarter;
+import com.qualcomm.ftccommon.UpdateUI;
+import com.qualcomm.ftccommon.configuration.EditParameters;
+import com.qualcomm.ftccommon.configuration.FtcLoadFileActivity;
+import com.qualcomm.ftccommon.configuration.RobotConfigFile;
+import com.qualcomm.ftccommon.configuration.RobotConfigFileManager;
+import com.qualcomm.ftcrobotcontroller.BuildConfig;
+import com.qualcomm.ftcrobotcontroller.R;
+import com.qualcomm.hardware.HardwareFactory;
+import com.qualcomm.robotcore.eventloop.EventLoopManager;
+import com.qualcomm.robotcore.eventloop.opmode.FtcRobotControllerServiceState;
+import com.qualcomm.robotcore.eventloop.opmode.OpModeRegister;
+import com.qualcomm.robotcore.hardware.configuration.LynxConstants;
+import com.qualcomm.robotcore.hardware.configuration.Utility;
+import com.qualcomm.robotcore.robot.Robot;
+import com.qualcomm.robotcore.robot.RobotState;
+import com.qualcomm.robotcore.util.ClockWarningSource;
+import com.qualcomm.robotcore.util.Device;
+import com.qualcomm.robotcore.util.Dimmer;
+import com.qualcomm.robotcore.util.ImmersiveMode;
+import com.qualcomm.robotcore.util.RobotLog;
+import com.qualcomm.robotcore.util.WebServer;
+import com.qualcomm.robotcore.wifi.NetworkConnection;
+import com.qualcomm.robotcore.wifi.NetworkConnectionFactory;
+import com.qualcomm.robotcore.wifi.NetworkType;
+
+import org.firstinspires.ftc.ftccommon.external.SoundPlayingRobotMonitor;
+import org.firstinspires.ftc.ftccommon.internal.AnnotatedHooksClassFilter;
+import org.firstinspires.ftc.ftccommon.internal.FtcRobotControllerWatchdogService;
+import org.firstinspires.ftc.ftccommon.internal.ProgramAndManageActivity;
+import org.firstinspires.ftc.onbotjava.ExternalLibraries;
+import org.firstinspires.ftc.onbotjava.OnBotJavaHelperImpl;
+import org.firstinspires.ftc.onbotjava.OnBotJavaProgrammingMode;
+import org.firstinspires.ftc.robotcore.external.navigation.MotionDetection;
+import org.firstinspires.ftc.robotcore.internal.hardware.android.AndroidBoard;
+import org.firstinspires.ftc.robotcore.internal.network.DeviceNameManagerFactory;
+import org.firstinspires.ftc.robotcore.internal.network.PreferenceRemoterRC;
+import org.firstinspires.ftc.robotcore.internal.network.StartResult;
+import org.firstinspires.ftc.robotcore.internal.network.WifiDirectChannelChanger;
+import org.firstinspires.ftc.robotcore.internal.network.WifiMuteEvent;
+import org.firstinspires.ftc.robotcore.internal.network.WifiMuteStateMachine;
+import org.firstinspires.ftc.robotcore.internal.opmode.ClassManager;
+import org.firstinspires.ftc.robotcore.internal.opmode.OnBotJavaHelper;
+import org.firstinspires.ftc.robotcore.internal.system.AppAliveNotifier;
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+import org.firstinspires.ftc.robotcore.internal.system.Assert;
+import org.firstinspires.ftc.robotcore.internal.system.PreferencesHelper;
+import org.firstinspires.ftc.robotcore.internal.system.ServiceController;
+import org.firstinspires.ftc.robotcore.internal.ui.ThemedActivity;
+import org.firstinspires.ftc.robotcore.internal.ui.UILocation;
+import org.firstinspires.ftc.robotcore.internal.webserver.RobotControllerWebInfo;
+import org.firstinspires.ftc.robotserver.internal.programmingmode.ProgrammingModeManager;
+import org.firstinspires.inspection.RcInspectionActivity;
+import org.threeten.bp.YearMonth;
+import org.xmlpull.v1.XmlPullParserException;
+
+import java.io.FileNotFoundException;
+import java.util.List;
+import java.util.Queue;
+import java.util.concurrent.ConcurrentLinkedQueue;
+
+@SuppressWarnings("WeakerAccess")
+public class FtcRobotControllerActivity extends Activity
+ {
+ public static final String TAG = "RCActivity";
+ public String getTag() { return TAG; }
+
+ private static final int REQUEST_CONFIG_WIFI_CHANNEL = 1;
+ private static final int NUM_GAMEPADS = 2;
+
+ protected WifiManager.WifiLock wifiLock;
+ protected RobotConfigFileManager cfgFileMgr;
+
+ private OnBotJavaHelper onBotJavaHelper;
+
+ protected ProgrammingModeManager programmingModeManager;
+
+ protected UpdateUI.Callback callback;
+ protected Context context;
+ protected Utility utility;
+ protected StartResult prefRemoterStartResult = new StartResult();
+ protected StartResult deviceNameStartResult = new StartResult();
+ protected PreferencesHelper preferencesHelper;
+ protected final SharedPreferencesListener sharedPreferencesListener = new SharedPreferencesListener();
+
+ protected ImageButton buttonMenu;
+ protected TextView textDeviceName;
+ protected TextView textNetworkConnectionStatus;
+ protected TextView textRobotStatus;
+ protected TextView[] textGamepad = new TextView[NUM_GAMEPADS];
+ protected TextView textOpMode;
+ protected TextView textErrorMessage;
+ protected ImmersiveMode immersion;
+
+ protected UpdateUI updateUI;
+ protected Dimmer dimmer;
+ protected LinearLayout entireScreenLayout;
+
+ protected FtcRobotControllerService controllerService;
+ protected NetworkType networkType;
+
+ protected FtcEventLoop eventLoop;
+ protected Queue receivedUsbAttachmentNotifications;
+
+ protected WifiMuteStateMachine wifiMuteStateMachine;
+ protected MotionDetection motionDetection;
+
+ private static boolean permissionsValidated = false;
+
+ private WifiDirectChannelChanger wifiDirectChannelChanger;
+
+ protected class RobotRestarter implements Restarter {
+
+ public void requestRestart() {
+ requestRobotRestart();
+ }
+
+ }
+
+ protected boolean serviceShouldUnbind = false;
+ protected ServiceConnection connection = new ServiceConnection() {
+ @Override
+ public void onServiceConnected(ComponentName name, IBinder service) {
+ FtcRobotControllerBinder binder = (FtcRobotControllerBinder) service;
+ onServiceBind(binder.getService());
+ }
+
+ @Override
+ public void onServiceDisconnected(ComponentName name) {
+ RobotLog.vv(FtcRobotControllerService.TAG, "%s.controllerService=null", TAG);
+ controllerService = null;
+ }
+ };
+
+ @Override
+ protected void onNewIntent(Intent intent) {
+ super.onNewIntent(intent);
+
+ if (UsbManager.ACTION_USB_DEVICE_ATTACHED.equals(intent.getAction())) {
+ UsbDevice usbDevice = intent.getParcelableExtra(UsbManager.EXTRA_DEVICE);
+ RobotLog.vv(TAG, "ACTION_USB_DEVICE_ATTACHED: %s", usbDevice.getDeviceName());
+
+ if (usbDevice != null) { // paranoia
+ // We might get attachment notifications before the event loop is set up, so
+ // we hold on to them and pass them along only when we're good and ready.
+ if (receivedUsbAttachmentNotifications != null) { // *total* paranoia
+ receivedUsbAttachmentNotifications.add(usbDevice);
+ passReceivedUsbAttachmentsToEventLoop();
+ }
+ }
+ }
+ }
+
+ protected void passReceivedUsbAttachmentsToEventLoop() {
+ if (this.eventLoop != null) {
+ for (;;) {
+ UsbDevice usbDevice = receivedUsbAttachmentNotifications.poll();
+ if (usbDevice == null)
+ break;
+ this.eventLoop.onUsbDeviceAttached(usbDevice);
+ }
+ }
+ else {
+ // Paranoia: we don't want the pending list to grow without bound when we don't
+ // (yet) have an event loop
+ while (receivedUsbAttachmentNotifications.size() > 100) {
+ receivedUsbAttachmentNotifications.poll();
+ }
+ }
+ }
+
+ /**
+ * There are cases where a permission may be revoked and the system restart will restart the
+ * FtcRobotControllerActivity, instead of the launch activity. Detect when that happens, and throw
+ * the device back to the permission validator activity.
+ */
+ protected boolean enforcePermissionValidator() {
+ if (!permissionsValidated) {
+ RobotLog.vv(TAG, "Redirecting to permission validator");
+ Intent permissionValidatorIntent = new Intent(AppUtil.getDefContext(), PermissionValidatorWrapper.class);
+ startActivity(permissionValidatorIntent);
+ finish();
+ return true;
+ } else {
+ RobotLog.vv(TAG, "Permissions validated already");
+ return false;
+ }
+ }
+
+ public static void setPermissionsValidated() {
+ permissionsValidated = true;
+ }
+
+ @Override
+ protected void onCreate(Bundle savedInstanceState) {
+ super.onCreate(savedInstanceState);
+
+ if (enforcePermissionValidator()) {
+ return;
+ }
+
+ RobotLog.onApplicationStart(); // robustify against onCreate() following onDestroy() but using the same app instance, which apparently does happen
+ RobotLog.vv(TAG, "onCreate()");
+ ThemedActivity.appAppThemeToActivity(getTag(), this); // do this way instead of inherit to help AppInventor
+
+ // Oddly, sometimes after a crash & restart the root activity will be something unexpected, like from the before crash? We don't yet understand
+ RobotLog.vv(TAG, "rootActivity is of class %s", AppUtil.getInstance().getRootActivity().getClass().getSimpleName());
+ RobotLog.vv(TAG, "launchActivity is of class %s", FtcRobotControllerWatchdogService.launchActivity());
+ Assert.assertTrue(FtcRobotControllerWatchdogService.isLaunchActivity(AppUtil.getInstance().getRootActivity()));
+ Assert.assertTrue(AppUtil.getInstance().isRobotController());
+
+ // Quick check: should we pretend we're not here, and so allow the Lynx to operate as
+ // a stand-alone USB-connected module?
+ if (LynxConstants.isRevControlHub()) {
+ // Double-sure check that we can talk to the DB over the serial TTY
+ AndroidBoard.getInstance().getAndroidBoardIsPresentPin().setState(true);
+ }
+
+ context = this;
+ utility = new Utility(this);
+
+ DeviceNameManagerFactory.getInstance().start(deviceNameStartResult);
+
+ PreferenceRemoterRC.getInstance().start(prefRemoterStartResult);
+
+ receivedUsbAttachmentNotifications = new ConcurrentLinkedQueue();
+ eventLoop = null;
+
+ setContentView(R.layout.activity_ftc_controller);
+
+ preferencesHelper = new PreferencesHelper(TAG, context);
+ preferencesHelper.writeBooleanPrefIfDifferent(context.getString(R.string.pref_rc_connected), true);
+ preferencesHelper.getSharedPreferences().registerOnSharedPreferenceChangeListener(sharedPreferencesListener);
+
+ // Check if this RC app is from a later FTC season than what was installed previously
+ int ftcSeasonYearOfPreviouslyInstalledRc = preferencesHelper.readInt(getString(R.string.pref_ftc_season_year_of_current_rc), 0);
+ int ftcSeasonYearOfCurrentlyInstalledRc = AppUtil.getInstance().getFtcSeasonYear(AppUtil.getInstance().getLocalSdkBuildMonth()).getValue();
+ if (ftcSeasonYearOfCurrentlyInstalledRc > ftcSeasonYearOfPreviouslyInstalledRc) {
+ preferencesHelper.writeIntPrefIfDifferent(getString(R.string.pref_ftc_season_year_of_current_rc), ftcSeasonYearOfCurrentlyInstalledRc);
+ // Since it's a new FTC season, we should reset certain settings back to their default values.
+ preferencesHelper.writeBooleanPrefIfDifferent(getString(R.string.pref_warn_about_2_4_ghz_band), true);
+ preferencesHelper.writeBooleanPrefIfDifferent(getString(R.string.pref_warn_about_obsolete_software), true);
+ preferencesHelper.writeBooleanPrefIfDifferent(getString(R.string.pref_warn_about_mismatched_app_versions), true);
+ preferencesHelper.writeBooleanPrefIfDifferent(getString(R.string.pref_warn_about_incorrect_clocks), true);
+ }
+
+ entireScreenLayout = (LinearLayout) findViewById(R.id.entire_screen);
+ buttonMenu = (ImageButton) findViewById(R.id.menu_buttons);
+ buttonMenu.setOnClickListener(new View.OnClickListener() {
+ @Override
+ public void onClick(View v) {
+ PopupMenu popupMenu = new PopupMenu(FtcRobotControllerActivity.this, v);
+ popupMenu.setOnMenuItemClickListener(new PopupMenu.OnMenuItemClickListener() {
+ @Override
+ public boolean onMenuItemClick(MenuItem item) {
+ return onOptionsItemSelected(item); // Delegate to the handler for the hardware menu button
+ }
+ });
+ popupMenu.inflate(R.menu.ftc_robot_controller);
+ AnnotatedHooksClassFilter.getInstance().callOnCreateMenuMethods(
+ FtcRobotControllerActivity.this, popupMenu.getMenu());
+ popupMenu.show();
+ }
+ });
+
+ updateMonitorLayout(getResources().getConfiguration());
+
+ BlocksOpMode.setActivityAndWebView(this, (WebView) findViewById(R.id.webViewBlocksRuntime));
+
+ ExternalLibraries.getInstance().onCreate();
+ onBotJavaHelper = new OnBotJavaHelperImpl();
+
+ /*
+ * Paranoia as the ClassManagerFactory requires EXTERNAL_STORAGE permissions
+ * and we've seen on the DS where the finish() call above does not short-circuit
+ * the onCreate() call for the activity and then we crash here because we don't
+ * have permissions. So...
+ */
+ if (permissionsValidated) {
+ ClassManager.getInstance().setOnBotJavaClassHelper(onBotJavaHelper);
+ ClassManagerFactory.registerFilters();
+ ClassManagerFactory.processAllClasses();
+ }
+
+ cfgFileMgr = new RobotConfigFileManager(this);
+
+ // Clean up 'dirty' status after a possible crash
+ RobotConfigFile configFile = cfgFileMgr.getActiveConfig();
+ if (configFile.isDirty()) {
+ configFile.markClean();
+ cfgFileMgr.setActiveConfig(false, configFile);
+ }
+
+ textDeviceName = (TextView) findViewById(R.id.textDeviceName);
+ textNetworkConnectionStatus = (TextView) findViewById(R.id.textNetworkConnectionStatus);
+ textRobotStatus = (TextView) findViewById(R.id.textRobotStatus);
+ textOpMode = (TextView) findViewById(R.id.textOpMode);
+ textErrorMessage = (TextView) findViewById(R.id.textErrorMessage);
+ textGamepad[0] = (TextView) findViewById(R.id.textGamepad1);
+ textGamepad[1] = (TextView) findViewById(R.id.textGamepad2);
+ immersion = new ImmersiveMode(getWindow().getDecorView());
+ dimmer = new Dimmer(this);
+ dimmer.longBright();
+
+ programmingModeManager = new ProgrammingModeManager();
+ programmingModeManager.register(new ProgrammingWebHandlers());
+ programmingModeManager.register(new OnBotJavaProgrammingMode());
+
+ updateUI = createUpdateUI();
+ callback = createUICallback(updateUI);
+
+ PreferenceManager.setDefaultValues(this, R.xml.app_settings, false);
+
+ WifiManager wifiManager = (WifiManager) getApplicationContext().getSystemService(Context.WIFI_SERVICE);
+ wifiLock = wifiManager.createWifiLock(WifiManager.WIFI_MODE_FULL_HIGH_PERF, "");
+
+ hittingMenuButtonBrightensScreen();
+
+ wifiLock.acquire();
+ callback.networkConnectionUpdate(NetworkConnection.NetworkEvent.DISCONNECTED);
+ readNetworkType();
+ ServiceController.startService(FtcRobotControllerWatchdogService.class);
+ bindToService();
+ RobotLog.logAppInfo();
+ RobotLog.logDeviceInfo();
+ AndroidBoard.getInstance().logAndroidBoardInfo();
+
+ if (preferencesHelper.readBoolean(getString(R.string.pref_wifi_automute), false)) {
+ initWifiMute(true);
+ }
+
+ FtcAboutActivity.setBuildTimeFromBuildConfig(BuildConfig.APP_BUILD_TIME);
+
+ // check to see if there is a preferred Wi-Fi to use.
+ checkPreferredChannel();
+
+ AnnotatedHooksClassFilter.getInstance().callOnCreateMethods(this);
+ }
+
+ protected UpdateUI createUpdateUI() {
+ Restarter restarter = new RobotRestarter();
+ UpdateUI result = new UpdateUI(this, dimmer);
+ result.setRestarter(restarter);
+ result.setTextViews(textNetworkConnectionStatus, textRobotStatus, textGamepad, textOpMode, textErrorMessage, textDeviceName);
+ return result;
+ }
+
+ protected UpdateUI.Callback createUICallback(UpdateUI updateUI) {
+ UpdateUI.Callback result = updateUI.new Callback();
+ result.setStateMonitor(new SoundPlayingRobotMonitor());
+ return result;
+ }
+
+ @Override
+ protected void onStart() {
+ super.onStart();
+ RobotLog.vv(TAG, "onStart()");
+
+ entireScreenLayout.setOnTouchListener(new View.OnTouchListener() {
+ @Override
+ public boolean onTouch(View v, MotionEvent event) {
+ dimmer.handleDimTimer();
+ return false;
+ }
+ });
+ }
+
+ @Override
+ protected void onResume() {
+ super.onResume();
+ RobotLog.vv(TAG, "onResume()");
+
+ // In case the user just got back from fixing their clock, refresh ClockWarningSource
+ ClockWarningSource.getInstance().onPossibleRcClockUpdate();
+ }
+
+ @Override
+ protected void onPause() {
+ super.onPause();
+ RobotLog.vv(TAG, "onPause()");
+ }
+
+ @Override
+ protected void onStop() {
+ // Note: this gets called even when the configuration editor is launched. That is, it gets
+ // called surprisingly often. So, we don't actually do much here.
+ super.onStop();
+ RobotLog.vv(TAG, "onStop()");
+ }
+
+ @Override
+ protected void onDestroy() {
+ super.onDestroy();
+ RobotLog.vv(TAG, "onDestroy()");
+
+ shutdownRobot(); // Ensure the robot is put away to bed
+ if (callback != null) callback.close();
+
+ PreferenceRemoterRC.getInstance().stop(prefRemoterStartResult);
+ DeviceNameManagerFactory.getInstance().stop(deviceNameStartResult);
+
+ unbindFromService();
+ // If the app manually (?) is stopped, then we don't need the auto-starting function (?)
+ ServiceController.stopService(FtcRobotControllerWatchdogService.class);
+ if (wifiLock != null) wifiLock.release();
+ if (preferencesHelper != null) preferencesHelper.getSharedPreferences().unregisterOnSharedPreferenceChangeListener(sharedPreferencesListener);
+
+ RobotLog.cancelWriteLogcatToDisk();
+
+ AnnotatedHooksClassFilter.getInstance().callOnDestroyMethods(this);
+ }
+
+ protected void bindToService() {
+ readNetworkType();
+ Intent intent = new Intent(this, FtcRobotControllerService.class);
+ intent.putExtra(NetworkConnectionFactory.NETWORK_CONNECTION_TYPE, networkType);
+ serviceShouldUnbind = bindService(intent, connection, Context.BIND_AUTO_CREATE);
+ }
+
+ protected void unbindFromService() {
+ if (serviceShouldUnbind) {
+ unbindService(connection);
+ serviceShouldUnbind = false;
+ }
+ }
+
+ protected void readNetworkType() {
+ // Control hubs are always running the access point model. Everything else, for the time
+ // being always runs the Wi-Fi Direct model.
+ if (Device.isRevControlHub() == true) {
+ networkType = NetworkType.RCWIRELESSAP;
+ } else {
+ networkType = NetworkType.fromString(preferencesHelper.readString(context.getString(R.string.pref_pairing_kind), NetworkType.globalDefaultAsString()));
+ }
+
+ // update the app_settings
+ preferencesHelper.writeStringPrefIfDifferent(context.getString(R.string.pref_pairing_kind), networkType.toString());
+ }
+
+ @Override
+ public void onWindowFocusChanged(boolean hasFocus) {
+ super.onWindowFocusChanged(hasFocus);
+
+ if (hasFocus) {
+ immersion.hideSystemUI();
+ getWindow().setFlags(WindowManager.LayoutParams.FLAG_TRANSLUCENT_NAVIGATION, WindowManager.LayoutParams.FLAG_TRANSLUCENT_NAVIGATION);
+ }
+ }
+
+ @Override
+ public boolean onCreateOptionsMenu(Menu menu) {
+ getMenuInflater().inflate(R.menu.ftc_robot_controller, menu);
+ AnnotatedHooksClassFilter.getInstance().callOnCreateMenuMethods(this, menu);
+ return true;
+ }
+
+ private boolean isRobotRunning() {
+ if (controllerService == null) {
+ return false;
+ }
+
+ Robot robot = controllerService.getRobot();
+
+ if ((robot == null) || (robot.eventLoopManager == null)) {
+ return false;
+ }
+
+ RobotState robotState = robot.eventLoopManager.state;
+
+ if (robotState != RobotState.RUNNING) {
+ return false;
+ } else {
+ return true;
+ }
+ }
+
+ @Override
+ public boolean onOptionsItemSelected(MenuItem item) {
+ int id = item.getItemId();
+
+ if (id == R.id.action_program_and_manage) {
+ if (isRobotRunning()) {
+ Intent programmingModeIntent = new Intent(AppUtil.getDefContext(), ProgramAndManageActivity.class);
+ RobotControllerWebInfo webInfo = programmingModeManager.getWebServer().getConnectionInformation();
+ programmingModeIntent.putExtra(LaunchActivityConstantsList.RC_WEB_INFO, webInfo.toJson());
+ startActivity(programmingModeIntent);
+ } else {
+ AppUtil.getInstance().showToast(UILocation.ONLY_LOCAL, context.getString(R.string.toastWifiUpBeforeProgrammingMode));
+ }
+ } else if (id == R.id.action_inspection_mode) {
+ Intent inspectionModeIntent = new Intent(AppUtil.getDefContext(), RcInspectionActivity.class);
+ startActivity(inspectionModeIntent);
+ return true;
+ } else if (id == R.id.action_restart_robot) {
+ dimmer.handleDimTimer();
+ AppUtil.getInstance().showToast(UILocation.BOTH, context.getString(R.string.toastRestartingRobot));
+ requestRobotRestart();
+ return true;
+ }
+ else if (id == R.id.action_configure_robot) {
+ EditParameters parameters = new EditParameters();
+ Intent intentConfigure = new Intent(AppUtil.getDefContext(), FtcLoadFileActivity.class);
+ parameters.putIntent(intentConfigure);
+ startActivityForResult(intentConfigure, RequestCode.CONFIGURE_ROBOT_CONTROLLER.ordinal());
+ }
+ else if (id == R.id.action_settings) {
+ // historical: this once erroneously used FTC_CONFIGURE_REQUEST_CODE_ROBOT_CONTROLLER
+ Intent settingsIntent = new Intent(AppUtil.getDefContext(), FtcRobotControllerSettingsActivity.class);
+ startActivityForResult(settingsIntent, RequestCode.SETTINGS_ROBOT_CONTROLLER.ordinal());
+ return true;
+ }
+ else if (id == R.id.action_about) {
+ Intent intent = new Intent(AppUtil.getDefContext(), FtcAboutActivity.class);
+ startActivity(intent);
+ return true;
+ }
+ else if (id == R.id.action_exit_app) {
+
+ //Clear backstack and everything to prevent edge case where VM might be
+ //restarted (after it was exited) if more than one activity was on the
+ //backstack for some reason.
+ finishAffinity();
+
+ //For lollipop and up, we can clear ourselves from the recents list too
+ if (android.os.Build.VERSION.SDK_INT >= android.os.Build.VERSION_CODES.LOLLIPOP) {
+ ActivityManager manager = (ActivityManager) getSystemService(ACTIVITY_SERVICE);
+ List tasks = manager.getAppTasks();
+
+ for (ActivityManager.AppTask task : tasks) {
+ task.finishAndRemoveTask();
+ }
+ }
+
+ // Allow the user to use the Control Hub operating system's UI, instead of relaunching the app
+ AppAliveNotifier.getInstance().disableAppWatchdogUntilNextAppStart();
+
+ //Finally, nuke the VM from orbit
+ AppUtil.getInstance().exitApplication();
+
+ return true;
+ }
+
+ return super.onOptionsItemSelected(item);
+ }
+
+ @Override
+ public void onConfigurationChanged(Configuration newConfig) {
+ super.onConfigurationChanged(newConfig);
+ // don't destroy assets on screen rotation
+ updateMonitorLayout(newConfig);
+ }
+
+ /**
+ * Updates the orientation of monitorContainer (which contains cameraMonitorView and
+ * tfodMonitorView) based on the given configuration. Makes the children split the space.
+ */
+ private void updateMonitorLayout(Configuration configuration) {
+ LinearLayout monitorContainer = (LinearLayout) findViewById(R.id.monitorContainer);
+ if (configuration.orientation == Configuration.ORIENTATION_LANDSCAPE) {
+ // When the phone is landscape, lay out the monitor views horizontally.
+ monitorContainer.setOrientation(LinearLayout.HORIZONTAL);
+ for (int i = 0; i < monitorContainer.getChildCount(); i++) {
+ View view = monitorContainer.getChildAt(i);
+ view.setLayoutParams(new LayoutParams(0, LayoutParams.MATCH_PARENT, 1 /* weight */));
+ }
+ } else {
+ // When the phone is portrait, lay out the monitor views vertically.
+ monitorContainer.setOrientation(LinearLayout.VERTICAL);
+ for (int i = 0; i < monitorContainer.getChildCount(); i++) {
+ View view = monitorContainer.getChildAt(i);
+ view.setLayoutParams(new LayoutParams(LayoutParams.MATCH_PARENT, 0, 1 /* weight */));
+ }
+ }
+ monitorContainer.requestLayout();
+ }
+
+ @Override
+ protected void onActivityResult(int request, int result, Intent intent) {
+ if (request == REQUEST_CONFIG_WIFI_CHANNEL) {
+ if (result == RESULT_OK) {
+ AppUtil.getInstance().showToast(UILocation.BOTH, context.getString(R.string.toastWifiConfigurationComplete));
+ }
+ }
+ // was some historical confusion about launch codes here, so we err safely
+ if (request == RequestCode.CONFIGURE_ROBOT_CONTROLLER.ordinal() || request == RequestCode.SETTINGS_ROBOT_CONTROLLER.ordinal()) {
+ // We always do a refresh, whether it was a cancel or an OK, for robustness
+ shutdownRobot();
+ cfgFileMgr.getActiveConfigAndUpdateUI();
+ updateUIAndRequestRobotSetup();
+ }
+ }
+
+ public void onServiceBind(final FtcRobotControllerService service) {
+ RobotLog.vv(FtcRobotControllerService.TAG, "%s.controllerService=bound", TAG);
+ controllerService = service;
+ updateUI.setControllerService(controllerService);
+
+ controllerService.setOnBotJavaHelper(onBotJavaHelper);
+
+ updateUIAndRequestRobotSetup();
+ programmingModeManager.setState(new FtcRobotControllerServiceState() {
+ @NonNull
+ @Override
+ public WebServer getWebServer() {
+ return service.getWebServer();
+ }
+
+ @Nullable
+ @Override
+ public OnBotJavaHelper getOnBotJavaHelper() {
+ return service.getOnBotJavaHelper();
+ }
+
+ @Override
+ public EventLoopManager getEventLoopManager() {
+ return service.getRobot().eventLoopManager;
+ }
+ });
+
+ AnnotatedHooksClassFilter.getInstance().callWebHandlerRegistrarMethods(this,
+ service.getWebServer().getWebHandlerManager());
+ }
+
+ private void updateUIAndRequestRobotSetup() {
+ if (controllerService != null) {
+ callback.networkConnectionUpdate(controllerService.getNetworkConnectionStatus());
+ callback.updateRobotStatus(controllerService.getRobotStatus());
+ // Only show this first-time toast on headless systems: what we have now on non-headless suffices
+ requestRobotSetup(LynxConstants.isRevControlHub()
+ ? new Runnable() {
+ @Override public void run() {
+ showRestartRobotCompleteToast(R.string.toastRobotSetupComplete);
+ }
+ }
+ : null);
+ }
+ }
+
+ private void requestRobotSetup(@Nullable Runnable runOnComplete) {
+ if (controllerService == null) return;
+
+ RobotConfigFile file = cfgFileMgr.getActiveConfigAndUpdateUI();
+ HardwareFactory hardwareFactory = new HardwareFactory(context);
+ try {
+ hardwareFactory.setXmlPullParser(file.getXml());
+ } catch (FileNotFoundException | XmlPullParserException e) {
+ RobotLog.ww(TAG, e, "Unable to set configuration file %s. Falling back on noConfig.", file.getName());
+ file = RobotConfigFile.noConfig(cfgFileMgr);
+ try {
+ hardwareFactory.setXmlPullParser(file.getXml());
+ cfgFileMgr.setActiveConfigAndUpdateUI(false, file);
+ } catch (FileNotFoundException | XmlPullParserException e1) {
+ RobotLog.ee(TAG, e1, "Failed to fall back on noConfig");
+ }
+ }
+
+ OpModeRegister userOpModeRegister = createOpModeRegister();
+ eventLoop = new FtcEventLoop(hardwareFactory, userOpModeRegister, callback, this);
+ FtcEventLoopIdle idleLoop = new FtcEventLoopIdle(hardwareFactory, userOpModeRegister, callback, this);
+
+ controllerService.setCallback(callback);
+ controllerService.setupRobot(eventLoop, idleLoop, runOnComplete);
+
+ passReceivedUsbAttachmentsToEventLoop();
+ AndroidBoard.showErrorIfUnknownControlHub();
+
+ AnnotatedHooksClassFilter.getInstance().callOnCreateEventLoopMethods(this, eventLoop);
+ }
+
+ protected OpModeRegister createOpModeRegister() {
+ return new FtcOpModeRegister();
+ }
+
+ private void shutdownRobot() {
+ if (controllerService != null) controllerService.shutdownRobot();
+ }
+
+ private void requestRobotRestart() {
+ AppUtil.getInstance().showToast(UILocation.BOTH, AppUtil.getDefContext().getString(R.string.toastRestartingRobot));
+ //
+ RobotLog.clearGlobalErrorMsg();
+ RobotLog.clearGlobalWarningMsg();
+ shutdownRobot();
+ requestRobotSetup(new Runnable() {
+ @Override public void run() {
+ showRestartRobotCompleteToast(R.string.toastRestartRobotComplete);
+ }
+ });
+ }
+
+ private void showRestartRobotCompleteToast(@StringRes int resid) {
+ AppUtil.getInstance().showToast(UILocation.BOTH, AppUtil.getDefContext().getString(resid));
+ }
+
+ private void checkPreferredChannel() {
+ // For P2P network, check to see what preferred channel is.
+ if (networkType == NetworkType.WIFIDIRECT) {
+ int prefChannel = preferencesHelper.readInt(getString(com.qualcomm.ftccommon.R.string.pref_wifip2p_channel), -1);
+ if (prefChannel == -1) {
+ prefChannel = 0;
+ RobotLog.vv(TAG, "pref_wifip2p_channel: No preferred channel defined. Will use a default value of %d", prefChannel);
+ } else {
+ RobotLog.vv(TAG, "pref_wifip2p_channel: Found existing preferred channel (%d).", prefChannel);
+ }
+
+ // attempt to set the preferred channel.
+ RobotLog.vv(TAG, "pref_wifip2p_channel: attempting to set preferred channel...");
+ wifiDirectChannelChanger = new WifiDirectChannelChanger();
+ wifiDirectChannelChanger.changeToChannel(prefChannel);
+ }
+ }
+
+ protected void hittingMenuButtonBrightensScreen() {
+ ActionBar actionBar = getActionBar();
+ if (actionBar != null) {
+ actionBar.addOnMenuVisibilityListener(new ActionBar.OnMenuVisibilityListener() {
+ @Override
+ public void onMenuVisibilityChanged(boolean isVisible) {
+ if (isVisible) {
+ dimmer.handleDimTimer();
+ }
+ }
+ });
+ }
+ }
+
+ protected class SharedPreferencesListener implements SharedPreferences.OnSharedPreferenceChangeListener {
+ @Override public void onSharedPreferenceChanged(SharedPreferences sharedPreferences, String key) {
+ if (key.equals(context.getString(R.string.pref_app_theme))) {
+ ThemedActivity.restartForAppThemeChange(getTag(), getString(R.string.appThemeChangeRestartNotifyRC));
+ } else if (key.equals(context.getString(R.string.pref_wifi_automute))) {
+ if (preferencesHelper.readBoolean(context.getString(R.string.pref_wifi_automute), false)) {
+ initWifiMute(true);
+ } else {
+ initWifiMute(false);
+ }
+ }
+ }
+ }
+
+ protected void initWifiMute(boolean enable) {
+ if (enable) {
+ wifiMuteStateMachine = new WifiMuteStateMachine();
+ wifiMuteStateMachine.initialize();
+ wifiMuteStateMachine.start();
+
+ motionDetection = new MotionDetection(2.0, 10);
+ motionDetection.startListening();
+ motionDetection.registerListener(new MotionDetection.MotionDetectionListener() {
+ @Override
+ public void onMotionDetected(double vector)
+ {
+ wifiMuteStateMachine.consumeEvent(WifiMuteEvent.USER_ACTIVITY);
+ }
+ });
+ } else {
+ wifiMuteStateMachine.stop();
+ wifiMuteStateMachine = null;
+ motionDetection.stopListening();
+ motionDetection.purgeListeners();
+ motionDetection = null;
+ }
+ }
+
+ @Override
+ public void onUserInteraction() {
+ if (wifiMuteStateMachine != null) {
+ wifiMuteStateMachine.consumeEvent(WifiMuteEvent.USER_ACTIVITY);
+ }
+ }
+}
diff --git a/src/main/java/org/firstinspires/ftc/robotcontroller/internal/PermissionValidatorWrapper.java b/src/main/java/org/firstinspires/ftc/robotcontroller/internal/PermissionValidatorWrapper.java
new file mode 100644
index 0000000..a0094bc
--- /dev/null
+++ b/src/main/java/org/firstinspires/ftc/robotcontroller/internal/PermissionValidatorWrapper.java
@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2018 Craig MacFarlane
+ *
+ * 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 Craig MacFarlane 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.internal;
+
+import android.Manifest;
+import android.os.Bundle;
+
+import com.qualcomm.ftcrobotcontroller.R;
+
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+import org.firstinspires.ftc.robotcore.internal.system.PermissionValidatorActivity;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class PermissionValidatorWrapper extends PermissionValidatorActivity {
+
+ private final String TAG = "PermissionValidatorWrapper";
+
+ /*
+ * The list of dangerous permissions the robot controller needs.
+ */
+ protected List robotControllerPermissions = new ArrayList() {{
+ add(Manifest.permission.WRITE_EXTERNAL_STORAGE);
+ add(Manifest.permission.READ_EXTERNAL_STORAGE);
+ add(Manifest.permission.CAMERA);
+ add(Manifest.permission.ACCESS_COARSE_LOCATION);
+ add(Manifest.permission.ACCESS_FINE_LOCATION);
+ add(Manifest.permission.READ_PHONE_STATE);
+ }};
+
+ private final static Class startApplication = FtcRobotControllerActivity.class;
+
+ public String mapPermissionToExplanation(final String permission) {
+ if (permission.equals(Manifest.permission.WRITE_EXTERNAL_STORAGE)) {
+ return Misc.formatForUser(R.string.permRcWriteExternalStorageExplain);
+ } else if (permission.equals(Manifest.permission.READ_EXTERNAL_STORAGE)) {
+ return Misc.formatForUser(R.string.permRcReadExternalStorageExplain);
+ } else if (permission.equals(Manifest.permission.CAMERA)) {
+ return Misc.formatForUser(R.string.permRcCameraExplain);
+ } else if (permission.equals(Manifest.permission.ACCESS_COARSE_LOCATION)) {
+ return Misc.formatForUser(R.string.permAccessLocationExplain);
+ } else if (permission.equals(Manifest.permission.ACCESS_FINE_LOCATION)) {
+ return Misc.formatForUser(R.string.permAccessLocationExplain);
+ } else if (permission.equals(Manifest.permission.READ_PHONE_STATE)) {
+ return Misc.formatForUser(R.string.permReadPhoneState);
+ }
+ return Misc.formatForUser(R.string.permGenericExplain);
+ }
+
+ @Override
+ protected void onCreate(Bundle savedInstanceState)
+ {
+ super.onCreate(savedInstanceState);
+
+ permissions = robotControllerPermissions;
+ }
+
+ protected Class onStartApplication()
+ {
+ FtcRobotControllerActivity.setPermissionsValidated();
+ return startApplication;
+ }
+}
diff --git a/src/main/res/drawable-xhdpi/icon_menu.png b/src/main/res/drawable-xhdpi/icon_menu.png
new file mode 100644
index 0000000..6b9e997
Binary files /dev/null and b/src/main/res/drawable-xhdpi/icon_menu.png differ
diff --git a/src/main/res/drawable-xhdpi/icon_robotcontroller.png b/src/main/res/drawable-xhdpi/icon_robotcontroller.png
new file mode 100644
index 0000000..022552f
Binary files /dev/null and b/src/main/res/drawable-xhdpi/icon_robotcontroller.png differ
diff --git a/src/main/res/layout/activity_ftc_controller.xml b/src/main/res/layout/activity_ftc_controller.xml
new file mode 100644
index 0000000..935bb2b
--- /dev/null
+++ b/src/main/res/layout/activity_ftc_controller.xml
@@ -0,0 +1,191 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/main/res/menu/ftc_robot_controller.xml b/src/main/res/menu/ftc_robot_controller.xml
new file mode 100644
index 0000000..657c1aa
--- /dev/null
+++ b/src/main/res/menu/ftc_robot_controller.xml
@@ -0,0 +1,78 @@
+
+
+
diff --git a/src/main/res/raw/gold.wav b/src/main/res/raw/gold.wav
new file mode 100644
index 0000000..3a7baf8
Binary files /dev/null and b/src/main/res/raw/gold.wav differ
diff --git a/src/main/res/raw/silver.wav b/src/main/res/raw/silver.wav
new file mode 100644
index 0000000..25918a7
Binary files /dev/null and b/src/main/res/raw/silver.wav differ
diff --git a/src/main/res/values/dimens.xml b/src/main/res/values/dimens.xml
new file mode 100644
index 0000000..63f1bab
--- /dev/null
+++ b/src/main/res/values/dimens.xml
@@ -0,0 +1,40 @@
+
+
+
+
+
+ 16dp
+ 5dp
+
+
\ No newline at end of file
diff --git a/src/main/res/values/strings.xml b/src/main/res/values/strings.xml
new file mode 100644
index 0000000..6ea191e
--- /dev/null
+++ b/src/main/res/values/strings.xml
@@ -0,0 +1,72 @@
+
+
+
+
+
+
+ FTC Robot Controller
+
+
+ Self Inspect
+ Program & Manage
+ Blocks
+ Settings
+ Restart Robot
+ Configure Robot
+ About
+ Exit
+
+
+ Configuration Complete
+ Restarting Robot
+ The Robot Controller must be fully up and running before entering Program and Manage Mode.
+
+
+
+ - @style/AppThemeRedRC
+ - @style/AppThemeGreenRC
+ - @style/AppThemeBlueRC
+ - @style/AppThemePurpleRC
+ - @style/AppThemeOrangeRC
+ - @style/AppThemeTealRC
+
+
+ pref_ftc_season_year_of_current_rc_new
+
+ @string/packageNameRobotController
+
+
diff --git a/src/main/res/values/styles.xml b/src/main/res/values/styles.xml
new file mode 100644
index 0000000..07689c0
--- /dev/null
+++ b/src/main/res/values/styles.xml
@@ -0,0 +1,23 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/main/res/xml/app_settings.xml b/src/main/res/xml/app_settings.xml
new file mode 100644
index 0000000..58d3aa9
--- /dev/null
+++ b/src/main/res/xml/app_settings.xml
@@ -0,0 +1,93 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/main/res/xml/device_filter.xml b/src/main/res/xml/device_filter.xml
new file mode 100644
index 0000000..7b75350
--- /dev/null
+++ b/src/main/res/xml/device_filter.xml
@@ -0,0 +1,49 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+