package org.firstinspires.ftc.teamcode; /* Modern Robotics Range Sensors Example Created 10/31/2016 by Colton Mehlhoff of Modern Robotics using FTC SDK 2.35 Reuse permitted with credit where credit is due Configuration: I2CDevice "range28" (MRI Range Sensor with default I2C address 0x28 I2CDevice "range2a" (MRI Color Sensor with I2C address 0x2a ModernRoboticsI2cGyro is not being used because it does not support .setI2CAddr(). To change range sensor I2C Addresses, go to http://modernroboticsedu.com/mod/lesson/view.php?id=96 Support is available by emailing support@modernroboticsinc.com. */ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.I2cAddr; import com.qualcomm.robotcore.hardware.I2cDevice; import com.qualcomm.robotcore.hardware.I2cDeviceSynch; import com.qualcomm.robotcore.hardware.I2cDeviceSynchImpl; import com.qualcomm.robotcore.util.ElapsedTime; @TeleOp(name = "Range Sensors", group = "MRI") //@Disabled public class MRI_Range_Sensors extends OpMode { /* Declare OpMode members. */ private ElapsedTime runtime = new ElapsedTime(); byte[] rangeAcache; byte[] rangeCcache; I2cDevice rangeA; I2cDevice rangeC; I2cDeviceSynch rangeAreader; I2cDeviceSynch rangeCreader; @Override public void init() { telemetry.addData("Status", "Initialized"); rangeA = hardwareMap.i2cDevice.get("range28"); rangeC = hardwareMap.i2cDevice.get("range2a"); rangeAreader = new I2cDeviceSynchImpl(rangeA, I2cAddr.create8bit(0x28), false); rangeCreader = new I2cDeviceSynchImpl(rangeC, I2cAddr.create8bit(0x2a), false); rangeAreader.engage(); rangeCreader.engage(); } @Override public void loop() { telemetry.addData("Status", "Running: " + runtime.toString()); rangeAcache = rangeAreader.read(0x04, 2); //Read 2 bytes starting at 0x04 rangeCcache = rangeCreader.read(0x04, 2); int RUS = rangeCcache[0] & 0xFF; //Ultrasonic value is at index 0. & 0xFF creates a value between 0 and 255 instead of -127 to 128 int LUS = rangeAcache[0] & 0xFF; int RODS = rangeCcache[1] & 0xFF; int LODS = rangeAcache[1] & 0xFF; //display values telemetry.addData("1 A US", LUS); telemetry.addData("2 A ODS", LODS); telemetry.addData("3 C US", RUS); telemetry.addData("4 C ODS", RODS); } }