Gripper and Color sensor basic functionality testing.
Dependencies: mbed
ColorSensor.cpp@0:b30091b30223, 2016-03-26 (annotated)
- Committer:
- z_g13
- Date:
- Sat Mar 26 23:30:29 2016 +0000
- Revision:
- 0:b30091b30223
3/26/2016 - ZG;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
z_g13 | 0:b30091b30223 | 1 | #include "ColorSensor.h" |
z_g13 | 0:b30091b30223 | 2 | #include "mbed.h" |
z_g13 | 0:b30091b30223 | 3 | |
z_g13 | 0:b30091b30223 | 4 | //sda and scl are for your i2c pins |
z_g13 | 0:b30091b30223 | 5 | ColorSensor::ColorSensor(PinName sda, PinName scl) : i2c(sda, scl), gain(4), scale(1) |
z_g13 | 0:b30091b30223 | 6 | { |
z_g13 | 0:b30091b30223 | 7 | //power on device and ADCs |
z_g13 | 0:b30091b30223 | 8 | i2c.write(ADDR, CTRL, 1); |
z_g13 | 0:b30091b30223 | 9 | i2c.write(ADDR, CTRL_INIT, 1); |
z_g13 | 0:b30091b30223 | 10 | |
z_g13 | 0:b30091b30223 | 11 | //intial gain is 4x, scale is 1x |
z_g13 | 0:b30091b30223 | 12 | i2c.write(ADDR, GAIN, 1); |
z_g13 | 0:b30091b30223 | 13 | i2c.write(ADDR, GAIN_INIT, 1); |
z_g13 | 0:b30091b30223 | 14 | } |
z_g13 | 0:b30091b30223 | 15 | |
z_g13 | 0:b30091b30223 | 16 | //returns red value |
z_g13 | 0:b30091b30223 | 17 | int ColorSensor::getR() |
z_g13 | 0:b30091b30223 | 18 | { |
z_g13 | 0:b30091b30223 | 19 | int red; |
z_g13 | 0:b30091b30223 | 20 | char data; |
z_g13 | 0:b30091b30223 | 21 | |
z_g13 | 0:b30091b30223 | 22 | i2c.write(ADDR, RED_LO, 1); |
z_g13 | 0:b30091b30223 | 23 | i2c.read(ADDR, &data, 1); |
z_g13 | 0:b30091b30223 | 24 | red = data; |
z_g13 | 0:b30091b30223 | 25 | i2c.write(ADDR, RED_HI, 1); |
z_g13 | 0:b30091b30223 | 26 | i2c.read(ADDR, &data, 1); |
z_g13 | 0:b30091b30223 | 27 | red += 256*data; |
z_g13 | 0:b30091b30223 | 28 | |
z_g13 | 0:b30091b30223 | 29 | return red; |
z_g13 | 0:b30091b30223 | 30 | } |
z_g13 | 0:b30091b30223 | 31 | |
z_g13 | 0:b30091b30223 | 32 | //returns green value |
z_g13 | 0:b30091b30223 | 33 | int ColorSensor::getG() |
z_g13 | 0:b30091b30223 | 34 | { |
z_g13 | 0:b30091b30223 | 35 | int grn; |
z_g13 | 0:b30091b30223 | 36 | char data; |
z_g13 | 0:b30091b30223 | 37 | |
z_g13 | 0:b30091b30223 | 38 | i2c.write(ADDR, GRN_LO, 1); |
z_g13 | 0:b30091b30223 | 39 | i2c.read(ADDR, &data, 1); |
z_g13 | 0:b30091b30223 | 40 | grn = data; |
z_g13 | 0:b30091b30223 | 41 | i2c.write(ADDR, GRN_HI, 1); |
z_g13 | 0:b30091b30223 | 42 | i2c.read(ADDR, &data, 1); |
z_g13 | 0:b30091b30223 | 43 | grn += 256*data; |
z_g13 | 0:b30091b30223 | 44 | |
z_g13 | 0:b30091b30223 | 45 | return grn; |
z_g13 | 0:b30091b30223 | 46 | } |
z_g13 | 0:b30091b30223 | 47 | |
z_g13 | 0:b30091b30223 | 48 | //returns blue value |
z_g13 | 0:b30091b30223 | 49 | int ColorSensor::getB() |
z_g13 | 0:b30091b30223 | 50 | { |
z_g13 | 0:b30091b30223 | 51 | int blu; |
z_g13 | 0:b30091b30223 | 52 | char data; |
z_g13 | 0:b30091b30223 | 53 | |
z_g13 | 0:b30091b30223 | 54 | i2c.write(ADDR, BLU_LO, 1); |
z_g13 | 0:b30091b30223 | 55 | i2c.read(ADDR, &data, 1); |
z_g13 | 0:b30091b30223 | 56 | blu = data; |
z_g13 | 0:b30091b30223 | 57 | i2c.write(ADDR, BLU_HI, 1); |
z_g13 | 0:b30091b30223 | 58 | i2c.read(ADDR, &data, 1); |
z_g13 | 0:b30091b30223 | 59 | blu += 256*data; |
z_g13 | 0:b30091b30223 | 60 | |
z_g13 | 0:b30091b30223 | 61 | return blu; |
z_g13 | 0:b30091b30223 | 62 | } |
z_g13 | 0:b30091b30223 | 63 | |
z_g13 | 0:b30091b30223 | 64 | //returns clear value |
z_g13 | 0:b30091b30223 | 65 | int ColorSensor::getC() |
z_g13 | 0:b30091b30223 | 66 | { |
z_g13 | 0:b30091b30223 | 67 | int clr; |
z_g13 | 0:b30091b30223 | 68 | char data; |
z_g13 | 0:b30091b30223 | 69 | |
z_g13 | 0:b30091b30223 | 70 | i2c.write(ADDR, CLR_LO, 1); |
z_g13 | 0:b30091b30223 | 71 | i2c.read(ADDR, &data, 1); |
z_g13 | 0:b30091b30223 | 72 | clr = data; |
z_g13 | 0:b30091b30223 | 73 | i2c.write(ADDR, CLR_HI, 1); |
z_g13 | 0:b30091b30223 | 74 | i2c.read(ADDR, &data, 1); |
z_g13 | 0:b30091b30223 | 75 | clr += 256*data; |
z_g13 | 0:b30091b30223 | 76 | |
z_g13 | 0:b30091b30223 | 77 | return clr; |
z_g13 | 0:b30091b30223 | 78 | } |
z_g13 | 0:b30091b30223 | 79 | |
z_g13 | 0:b30091b30223 | 80 | //sets ADC gain |
z_g13 | 0:b30091b30223 | 81 | //valid values 1, 4, 16, 64 |
z_g13 | 0:b30091b30223 | 82 | void ColorSensor::setGain(int g) |
z_g13 | 0:b30091b30223 | 83 | { |
z_g13 | 0:b30091b30223 | 84 | char newGain; |
z_g13 | 0:b30091b30223 | 85 | |
z_g13 | 0:b30091b30223 | 86 | switch(g) |
z_g13 | 0:b30091b30223 | 87 | { |
z_g13 | 0:b30091b30223 | 88 | case 1: newGain = 0x00; |
z_g13 | 0:b30091b30223 | 89 | break; |
z_g13 | 0:b30091b30223 | 90 | case 4: newGain = 0x10; |
z_g13 | 0:b30091b30223 | 91 | break; |
z_g13 | 0:b30091b30223 | 92 | case 16: newGain = 0x20; |
z_g13 | 0:b30091b30223 | 93 | break; |
z_g13 | 0:b30091b30223 | 94 | case 64: newGain = 0x30; |
z_g13 | 0:b30091b30223 | 95 | break; |
z_g13 | 0:b30091b30223 | 96 | default: return; |
z_g13 | 0:b30091b30223 | 97 | } |
z_g13 | 0:b30091b30223 | 98 | |
z_g13 | 0:b30091b30223 | 99 | gain = g; |
z_g13 | 0:b30091b30223 | 100 | |
z_g13 | 0:b30091b30223 | 101 | switch(scale) |
z_g13 | 0:b30091b30223 | 102 | { |
z_g13 | 0:b30091b30223 | 103 | case 1: newGain += 0x00; |
z_g13 | 0:b30091b30223 | 104 | break; |
z_g13 | 0:b30091b30223 | 105 | case 2: newGain += 0x01; |
z_g13 | 0:b30091b30223 | 106 | break; |
z_g13 | 0:b30091b30223 | 107 | case 4: newGain += 0x02; |
z_g13 | 0:b30091b30223 | 108 | break; |
z_g13 | 0:b30091b30223 | 109 | case 8: newGain += 0x03; |
z_g13 | 0:b30091b30223 | 110 | break; |
z_g13 | 0:b30091b30223 | 111 | case 16: newGain += 0x04; |
z_g13 | 0:b30091b30223 | 112 | break; |
z_g13 | 0:b30091b30223 | 113 | case 32: newGain += 0x05; |
z_g13 | 0:b30091b30223 | 114 | break; |
z_g13 | 0:b30091b30223 | 115 | case 64: newGain += 0x06; |
z_g13 | 0:b30091b30223 | 116 | } |
z_g13 | 0:b30091b30223 | 117 | |
z_g13 | 0:b30091b30223 | 118 | i2c.write(ADDR, GAIN, 1); |
z_g13 | 0:b30091b30223 | 119 | i2c.write(ADDR, &newGain, 1); |
z_g13 | 0:b30091b30223 | 120 | } |
z_g13 | 0:b30091b30223 | 121 | |
z_g13 | 0:b30091b30223 | 122 | //sets divides ADC sensitivity by scaler |
z_g13 | 0:b30091b30223 | 123 | //valid values 1, 2, 4, 8, 16, 32, 64 |
z_g13 | 0:b30091b30223 | 124 | void ColorSensor::setScaler(int s) |
z_g13 | 0:b30091b30223 | 125 | { |
z_g13 | 0:b30091b30223 | 126 | char newGain; |
z_g13 | 0:b30091b30223 | 127 | |
z_g13 | 0:b30091b30223 | 128 | switch(s) |
z_g13 | 0:b30091b30223 | 129 | { |
z_g13 | 0:b30091b30223 | 130 | case 1: newGain = 0x00; |
z_g13 | 0:b30091b30223 | 131 | break; |
z_g13 | 0:b30091b30223 | 132 | case 2: newGain = 0x01; |
z_g13 | 0:b30091b30223 | 133 | break; |
z_g13 | 0:b30091b30223 | 134 | case 4: newGain = 0x02; |
z_g13 | 0:b30091b30223 | 135 | break; |
z_g13 | 0:b30091b30223 | 136 | case 8: newGain = 0x03; |
z_g13 | 0:b30091b30223 | 137 | break; |
z_g13 | 0:b30091b30223 | 138 | case 16: newGain = 0x04; |
z_g13 | 0:b30091b30223 | 139 | break; |
z_g13 | 0:b30091b30223 | 140 | case 32: newGain = 0x05; |
z_g13 | 0:b30091b30223 | 141 | break; |
z_g13 | 0:b30091b30223 | 142 | case 64: newGain = 0x06; |
z_g13 | 0:b30091b30223 | 143 | break; |
z_g13 | 0:b30091b30223 | 144 | default: return; |
z_g13 | 0:b30091b30223 | 145 | } |
z_g13 | 0:b30091b30223 | 146 | |
z_g13 | 0:b30091b30223 | 147 | scale = s; |
z_g13 | 0:b30091b30223 | 148 | |
z_g13 | 0:b30091b30223 | 149 | switch(gain) |
z_g13 | 0:b30091b30223 | 150 | { |
z_g13 | 0:b30091b30223 | 151 | case 1: newGain += 0x00; |
z_g13 | 0:b30091b30223 | 152 | break; |
z_g13 | 0:b30091b30223 | 153 | case 4: newGain += 0x10; |
z_g13 | 0:b30091b30223 | 154 | break; |
z_g13 | 0:b30091b30223 | 155 | case 16: newGain += 0x20; |
z_g13 | 0:b30091b30223 | 156 | break; |
z_g13 | 0:b30091b30223 | 157 | case 64: newGain += 0x30; |
z_g13 | 0:b30091b30223 | 158 | } |
z_g13 | 0:b30091b30223 | 159 | |
z_g13 | 0:b30091b30223 | 160 | i2c.write(ADDR, GAIN, 1); |
z_g13 | 0:b30091b30223 | 161 | i2c.write(ADDR, &newGain, 1); |
z_g13 | 0:b30091b30223 | 162 | } |