Texas State IEEE / Mbed 2 deprecated grippertest

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?

UserRevisionLine numberNew 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 }