Created
March 29, 2017 02:22
-
-
Save bweis/451b96c20b1b574fdb5e12d0515a3921 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
package org.firstinspires.ftc.teamcode.devices; | |
import android.util.Log; | |
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; | |
import com.qualcomm.robotcore.eventloop.opmode.OpMode; | |
import com.qualcomm.robotcore.hardware.HardwareMap; | |
import com.qualcomm.robotcore.hardware.I2cAddr; | |
import com.qualcomm.robotcore.hardware.I2cDevice; | |
import com.qualcomm.robotcore.hardware.I2cDeviceSynch; | |
import com.qualcomm.robotcore.hardware.I2cDeviceSynchImpl; | |
/** | |
* Created by Ben on 3/28/17. | |
*/ | |
public class MultiplexedColorSensor { | |
// Registers | |
static final int ENABLE = 0x80; | |
static final int ATIME = 0x81; | |
static final int CONTROL = 0x8F; | |
static final int ID = 0x92; | |
static final int STATUS = 0x93; | |
static final int CDATAL = 0x94; | |
// Default I2C address for multiplexer. The address can be changed to any | |
// value from 0x70 to 0x77, so this line would need to be changed if a | |
// non-default address is to be used. | |
static final I2cAddr MUX_ADDRESS = new I2cAddr(0x70); | |
private I2cDevice mux; | |
private I2cDeviceSynch muxReader; | |
// Only one color sensor is needed in code as the multiplexer switches | |
// between the physical sensors | |
private byte[] adaCache; | |
// I2C address for color sensor | |
static final I2cAddr ADA_ADDRESS = new I2cAddr(0x29); | |
private I2cDevice ada; | |
private I2cDeviceSynch adaReader; | |
private HardwareMap hardwareMap; | |
private int[] sensorPorts; | |
private double miliseconds; | |
private int gain; | |
private String muxName; | |
private String colorName; | |
public static int GAIN_1X = 0x00; | |
public static int GAIN_4X = 0x01; | |
public static int GAIN_16X = 0x02; | |
public static int GAIN_60X = 0x03; | |
/** | |
* Initializes Adafruit color sensors on the specified ports of the I2C | |
* multiplexer. | |
* | |
* @param hardwareMap hardwareMap from OpMode | |
* @param muxName Configuration name of I2CDevice for multiplexer | |
* @param colorName Configuration name of I2CDevice for color sensor | |
* @param sensorPorts Out ports on multiplexer with color sensors attached | |
* @param milliSeconds Integration time in milliseconds | |
* @param gain Gain (GAIN_1X, GAIN_4X, GAIN_16X, GAIN_60X) | |
*/ | |
public MultiplexedColorSensor(HardwareMap hardwareMap, String muxName, String colorName, int[] sensorPorts, double milliSeconds, int gain) { | |
this.hardwareMap = hardwareMap; | |
this.muxName = muxName; | |
this.colorName = colorName; | |
this.sensorPorts = sensorPorts; | |
this.miliseconds = milliSeconds; | |
this.gain = gain; | |
} | |
public void init(LinearOpMode linearOpMode) { | |
Log.i("ColorInit", "Active: "+ linearOpMode.opModeIsActive() + " \tStopReq: " + linearOpMode.isStopRequested()); | |
mux = hardwareMap.i2cDevice.get(muxName); | |
Log.i("ColorInit", "Active: "+ linearOpMode.opModeIsActive() + " \tStopReq: " + linearOpMode.isStopRequested()); | |
muxReader = new I2cDeviceSynchImpl(mux, MUX_ADDRESS, false); | |
Log.i("ColorInit", "Active: "+ linearOpMode.opModeIsActive() + " \tStopReq: " + linearOpMode.isStopRequested()); | |
muxReader.engage(); | |
Log.i("ColorInit", "Active: "+ linearOpMode.opModeIsActive() + " \tStopReq: " + linearOpMode.isStopRequested()); | |
ada = hardwareMap.i2cDevice.get(colorName); | |
Log.i("ColorInitLoop", "Active: "+ linearOpMode.opModeIsActive() + " \tStopReq: " + linearOpMode.isStopRequested()); | |
//muxReader.write8(0x0, 1 << sensorPorts[0], true); // Write to given output port on the multiplexer | |
Log.i("ColorInitLoop", "Active: "+ linearOpMode.opModeIsActive() + " \tStopReq: " + linearOpMode.isStopRequested()); | |
adaReader = new I2cDeviceSynchImpl(ada, ADA_ADDRESS, false); | |
Log.i("ColorInitLoop", "Active: "+ linearOpMode.opModeIsActive() + " \tStopReq: " + linearOpMode.isStopRequested()); | |
adaReader.engage(); | |
Log.i("ColorInitLoop", "Active: "+ linearOpMode.opModeIsActive() + " \tStopReq: " + linearOpMode.isStopRequested()); | |
final int time = integrationByte(miliseconds); | |
adaReader.write8(ENABLE, 0x03, true); // Power on and enable ADC | |
adaReader.read8(ID); // Read device ID | |
adaReader.write8(CONTROL, gain, true); // Set gain | |
adaReader.write8(ATIME, time, true); // Set integration time | |
} | |
/** | |
* Set the integration time on all the color sensors | |
* @param milliSeconds Time in millseconds | |
*/ | |
public void setIntegrationTime(double milliSeconds) { | |
int val = integrationByte(milliSeconds); | |
for (int i = 0; i < sensorPorts.length; i++) { | |
muxReader.write8(0x0, 1 << sensorPorts[i], true); | |
adaReader.write8(ATIME, val, true); | |
} | |
} | |
private int integrationByte(double milliSeconds) { | |
int count = (int)(milliSeconds/2.4); | |
if (count<1) count = 1; // Clamp the time range | |
if (count>256) count = 256; | |
return (256 - count); | |
} | |
/** | |
* Retrieve the color read by the given color sensor | |
* | |
* @param port Port on multiplexer of given color sensor | |
* @return Array containing the Clear, Red, Green, and Blue color values | |
*/ | |
public int[] getCRGB(int port) { | |
// Write to I2C port on the multiplexer | |
muxReader.write8(0x0, 1 << port, true); | |
// Read color registers | |
adaCache = adaReader.read(CDATAL, 8); | |
// Combine high and low bytes | |
int[] crgb = new int[4]; | |
for (int i=0; i<4; i++) { | |
crgb[i] = (adaCache[2*i] & 0xFF) + (adaCache[2*i+1] & 0xFF) * 256; | |
} | |
return crgb; | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment