Gripper and Color sensor basic functionality testing.
Dependencies: mbed
Diff: ColorSensor.cpp
- Revision:
- 0:b30091b30223
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ColorSensor.cpp Sat Mar 26 23:30:29 2016 +0000 @@ -0,0 +1,162 @@ +#include "ColorSensor.h" +#include "mbed.h" + +//sda and scl are for your i2c pins +ColorSensor::ColorSensor(PinName sda, PinName scl) : i2c(sda, scl), gain(4), scale(1) +{ + //power on device and ADCs + i2c.write(ADDR, CTRL, 1); + i2c.write(ADDR, CTRL_INIT, 1); + + //intial gain is 4x, scale is 1x + i2c.write(ADDR, GAIN, 1); + i2c.write(ADDR, GAIN_INIT, 1); +} + +//returns red value +int ColorSensor::getR() +{ + int red; + char data; + + i2c.write(ADDR, RED_LO, 1); + i2c.read(ADDR, &data, 1); + red = data; + i2c.write(ADDR, RED_HI, 1); + i2c.read(ADDR, &data, 1); + red += 256*data; + + return red; +} + +//returns green value +int ColorSensor::getG() +{ + int grn; + char data; + + i2c.write(ADDR, GRN_LO, 1); + i2c.read(ADDR, &data, 1); + grn = data; + i2c.write(ADDR, GRN_HI, 1); + i2c.read(ADDR, &data, 1); + grn += 256*data; + + return grn; +} + +//returns blue value +int ColorSensor::getB() +{ + int blu; + char data; + + i2c.write(ADDR, BLU_LO, 1); + i2c.read(ADDR, &data, 1); + blu = data; + i2c.write(ADDR, BLU_HI, 1); + i2c.read(ADDR, &data, 1); + blu += 256*data; + + return blu; +} + +//returns clear value +int ColorSensor::getC() +{ + int clr; + char data; + + i2c.write(ADDR, CLR_LO, 1); + i2c.read(ADDR, &data, 1); + clr = data; + i2c.write(ADDR, CLR_HI, 1); + i2c.read(ADDR, &data, 1); + clr += 256*data; + + return clr; +} + +//sets ADC gain +//valid values 1, 4, 16, 64 +void ColorSensor::setGain(int g) +{ + char newGain; + + switch(g) + { + case 1: newGain = 0x00; + break; + case 4: newGain = 0x10; + break; + case 16: newGain = 0x20; + break; + case 64: newGain = 0x30; + break; + default: return; + } + + gain = g; + + switch(scale) + { + case 1: newGain += 0x00; + break; + case 2: newGain += 0x01; + break; + case 4: newGain += 0x02; + break; + case 8: newGain += 0x03; + break; + case 16: newGain += 0x04; + break; + case 32: newGain += 0x05; + break; + case 64: newGain += 0x06; + } + + i2c.write(ADDR, GAIN, 1); + i2c.write(ADDR, &newGain, 1); +} + +//sets divides ADC sensitivity by scaler +//valid values 1, 2, 4, 8, 16, 32, 64 +void ColorSensor::setScaler(int s) +{ + char newGain; + + switch(s) + { + case 1: newGain = 0x00; + break; + case 2: newGain = 0x01; + break; + case 4: newGain = 0x02; + break; + case 8: newGain = 0x03; + break; + case 16: newGain = 0x04; + break; + case 32: newGain = 0x05; + break; + case 64: newGain = 0x06; + break; + default: return; + } + + scale = s; + + switch(gain) + { + case 1: newGain += 0x00; + break; + case 4: newGain += 0x10; + break; + case 16: newGain += 0x20; + break; + case 64: newGain += 0x30; + } + + i2c.write(ADDR, GAIN, 1); + i2c.write(ADDR, &newGain, 1); +} \ No newline at end of file