4180
Dependencies: mbed Servo mbed-rtos X_NUCLEO_53L0A1
main.cpp@1:c162c077430f, 2018-12-12 (annotated)
- Committer:
- AbuAbdella
- Date:
- Wed Dec 12 05:24:22 2018 +0000
- Revision:
- 1:c162c077430f
- Parent:
- 0:bdd172e29b8b
4180 project
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
monwonga3 | 0:bdd172e29b8b | 1 | #include "mbed.h" |
monwonga3 | 0:bdd172e29b8b | 2 | #include "Servo.h" |
monwonga3 | 0:bdd172e29b8b | 3 | #include "rtos.h" |
monwonga3 | 0:bdd172e29b8b | 4 | |
AbuAbdella | 1:c162c077430f | 5 | |
monwonga3 | 0:bdd172e29b8b | 6 | Servo shoulder(p26); // pwm |
monwonga3 | 0:bdd172e29b8b | 7 | Servo elbow(p25); // pwm |
monwonga3 | 0:bdd172e29b8b | 8 | Servo wrist(p24); // pwm |
monwonga3 | 0:bdd172e29b8b | 9 | Servo hand(p23); // pwm |
AbuAbdella | 1:c162c077430f | 10 | DigitalIn pb(p8); |
monwonga3 | 0:bdd172e29b8b | 11 | |
monwonga3 | 0:bdd172e29b8b | 12 | I2C i2c(p9, p10); //pins for I2C communication (SDA, SCL) |
monwonga3 | 0:bdd172e29b8b | 13 | Serial pc(USBTX, USBRX); //Used to view the colors that are read in |
monwonga3 | 0:bdd172e29b8b | 14 | |
monwonga3 | 0:bdd172e29b8b | 15 | int sensor_addr = 41 << 1; |
monwonga3 | 0:bdd172e29b8b | 16 | |
monwonga3 | 0:bdd172e29b8b | 17 | float clear_value; |
monwonga3 | 0:bdd172e29b8b | 18 | float red_value; |
monwonga3 | 0:bdd172e29b8b | 19 | float blue_value; |
monwonga3 | 0:bdd172e29b8b | 20 | float green_value; |
monwonga3 | 0:bdd172e29b8b | 21 | |
monwonga3 | 0:bdd172e29b8b | 22 | float newRedValue; |
monwonga3 | 0:bdd172e29b8b | 23 | float oldRedValue; |
monwonga3 | 0:bdd172e29b8b | 24 | |
AbuAbdella | 1:c162c077430f | 25 | int new_pb = 0; |
AbuAbdella | 1:c162c077430f | 26 | int old_pb = 0; |
AbuAbdella | 1:c162c077430f | 27 | bool state = 0; |
AbuAbdella | 1:c162c077430f | 28 | |
monwonga3 | 0:bdd172e29b8b | 29 | |
monwonga3 | 0:bdd172e29b8b | 30 | |
monwonga3 | 0:bdd172e29b8b | 31 | DigitalOut green(LED1); |
monwonga3 | 0:bdd172e29b8b | 32 | DigitalOut led4(LED4); |
monwonga3 | 0:bdd172e29b8b | 33 | DigitalOut led(p11); |
AbuAbdella | 1:c162c077430f | 34 | DigitalOut led3(LED3); |
monwonga3 | 0:bdd172e29b8b | 35 | |
monwonga3 | 0:bdd172e29b8b | 36 | void stroke(); |
monwonga3 | 0:bdd172e29b8b | 37 | void dunk(); |
monwonga3 | 0:bdd172e29b8b | 38 | void rotate(); |
monwonga3 | 0:bdd172e29b8b | 39 | void readLight(); |
AbuAbdella | 1:c162c077430f | 40 | void gohere(); |
monwonga3 | 0:bdd172e29b8b | 41 | |
monwonga3 | 0:bdd172e29b8b | 42 | float shoulderPosition; |
AbuAbdella | 1:c162c077430f | 43 | float sh; |
AbuAbdella | 1:c162c077430f | 44 | //float shoulder; |
monwonga3 | 0:bdd172e29b8b | 45 | // |
monwonga3 | 0:bdd172e29b8b | 46 | Thread threadLight; |
monwonga3 | 0:bdd172e29b8b | 47 | Thread threadRotate; |
monwonga3 | 0:bdd172e29b8b | 48 | |
monwonga3 | 0:bdd172e29b8b | 49 | void blinky() { |
monwonga3 | 0:bdd172e29b8b | 50 | led4 = !led4; |
monwonga3 | 0:bdd172e29b8b | 51 | //wait(0.2); |
monwonga3 | 0:bdd172e29b8b | 52 | } |
monwonga3 | 0:bdd172e29b8b | 53 | |
monwonga3 | 0:bdd172e29b8b | 54 | int main() |
monwonga3 | 0:bdd172e29b8b | 55 | { |
monwonga3 | 0:bdd172e29b8b | 56 | |
AbuAbdella | 1:c162c077430f | 57 | pb.mode(PullUp); |
monwonga3 | 0:bdd172e29b8b | 58 | |
AbuAbdella | 1:c162c077430f | 59 | while(1){ |
AbuAbdella | 1:c162c077430f | 60 | |
AbuAbdella | 1:c162c077430f | 61 | new_pb=pb; |
AbuAbdella | 1:c162c077430f | 62 | |
AbuAbdella | 1:c162c077430f | 63 | if (state == 0){ |
AbuAbdella | 1:c162c077430f | 64 | |
AbuAbdella | 1:c162c077430f | 65 | led4=0; |
AbuAbdella | 1:c162c077430f | 66 | |
AbuAbdella | 1:c162c077430f | 67 | |
AbuAbdella | 1:c162c077430f | 68 | |
AbuAbdella | 1:c162c077430f | 69 | if((new_pb == 0) && (old_pb==1)){ |
AbuAbdella | 1:c162c077430f | 70 | |
AbuAbdella | 1:c162c077430f | 71 | state =1; |
AbuAbdella | 1:c162c077430f | 72 | |
AbuAbdella | 1:c162c077430f | 73 | } |
AbuAbdella | 1:c162c077430f | 74 | |
AbuAbdella | 1:c162c077430f | 75 | } |
AbuAbdella | 1:c162c077430f | 76 | else { |
AbuAbdella | 1:c162c077430f | 77 | |
AbuAbdella | 1:c162c077430f | 78 | led4=1; |
AbuAbdella | 1:c162c077430f | 79 | // stroke(); |
AbuAbdella | 1:c162c077430f | 80 | threadLight.start(readLight); // the address of the function to be attached (flip) and the interval (2 seconds) |
AbuAbdella | 1:c162c077430f | 81 | threadRotate.start(rotate);// to stop thread thread_name.terminate(); |
AbuAbdella | 1:c162c077430f | 82 | |
AbuAbdella | 1:c162c077430f | 83 | |
AbuAbdella | 1:c162c077430f | 84 | if((new_pb == 0) && (old_pb==1)){ |
AbuAbdella | 1:c162c077430f | 85 | |
AbuAbdella | 1:c162c077430f | 86 | state =0; |
AbuAbdella | 1:c162c077430f | 87 | |
AbuAbdella | 1:c162c077430f | 88 | } |
AbuAbdella | 1:c162c077430f | 89 | |
AbuAbdella | 1:c162c077430f | 90 | } |
AbuAbdella | 1:c162c077430f | 91 | |
AbuAbdella | 1:c162c077430f | 92 | old_pb = new_pb; |
AbuAbdella | 1:c162c077430f | 93 | |
AbuAbdella | 1:c162c077430f | 94 | } |
monwonga3 | 0:bdd172e29b8b | 95 | |
monwonga3 | 0:bdd172e29b8b | 96 | |
monwonga3 | 0:bdd172e29b8b | 97 | |
monwonga3 | 0:bdd172e29b8b | 98 | } |
monwonga3 | 0:bdd172e29b8b | 99 | |
AbuAbdella | 1:c162c077430f | 100 | |
monwonga3 | 0:bdd172e29b8b | 101 | void rotate(){ |
monwonga3 | 0:bdd172e29b8b | 102 | |
AbuAbdella | 1:c162c077430f | 103 | // shoulder=0.37f; |
AbuAbdella | 1:c162c077430f | 104 | wrist = 0.1f; |
AbuAbdella | 1:c162c077430f | 105 | elbow = 0.10f; |
AbuAbdella | 1:c162c077430f | 106 | |
AbuAbdella | 1:c162c077430f | 107 | // elbow = 0.00f; |
monwonga3 | 0:bdd172e29b8b | 108 | //while(doRotate){ |
AbuAbdella | 1:c162c077430f | 109 | |
monwonga3 | 0:bdd172e29b8b | 110 | |
AbuAbdella | 1:c162c077430f | 111 | for (float i = 0.44f; i > 0.0f; i-=0.01f) {// Forward cycle |
AbuAbdella | 1:c162c077430f | 112 | |
AbuAbdella | 1:c162c077430f | 113 | led3=0; |
AbuAbdella | 1:c162c077430f | 114 | shoulder=i; |
AbuAbdella | 1:c162c077430f | 115 | wait_ms(100); |
AbuAbdella | 1:c162c077430f | 116 | // pc.printf("shoulder %.2f \n",i); |
AbuAbdella | 1:c162c077430f | 117 | |
AbuAbdella | 1:c162c077430f | 118 | } |
AbuAbdella | 1:c162c077430f | 119 | |
AbuAbdella | 1:c162c077430f | 120 | |
AbuAbdella | 1:c162c077430f | 121 | for (float j = 0; j < 0.34f; j+=0.01f) { // Return cycle take reading |
AbuAbdella | 1:c162c077430f | 122 | |
AbuAbdella | 1:c162c077430f | 123 | led3=0; |
AbuAbdella | 1:c162c077430f | 124 | shoulder=j; |
AbuAbdella | 1:c162c077430f | 125 | wait_ms(100); |
AbuAbdella | 1:c162c077430f | 126 | |
AbuAbdella | 1:c162c077430f | 127 | if(newRedValue <= 0.615 ){//abs(newRedValue-oldRedValue)>=0.03){ //newRedValue <= oldRedValue - .02 ){ |
AbuAbdella | 1:c162c077430f | 128 | |
AbuAbdella | 1:c162c077430f | 129 | shoulderPosition = shoulder; |
AbuAbdella | 1:c162c077430f | 130 | |
AbuAbdella | 1:c162c077430f | 131 | } |
AbuAbdella | 1:c162c077430f | 132 | |
AbuAbdella | 1:c162c077430f | 133 | // pc.printf("shoulder %.2f \n",j); |
AbuAbdella | 1:c162c077430f | 134 | |
AbuAbdella | 1:c162c077430f | 135 | } |
AbuAbdella | 1:c162c077430f | 136 | |
AbuAbdella | 1:c162c077430f | 137 | for (float j = 0.34; j < 0.50f; j+=0.01f) { // Return cycle |
AbuAbdella | 1:c162c077430f | 138 | |
AbuAbdella | 1:c162c077430f | 139 | led3=1; |
AbuAbdella | 1:c162c077430f | 140 | shoulder=j; |
AbuAbdella | 1:c162c077430f | 141 | wait_ms(100); |
AbuAbdella | 1:c162c077430f | 142 | |
AbuAbdella | 1:c162c077430f | 143 | // pc.printf("shoulder %.2f \n",j); |
AbuAbdella | 1:c162c077430f | 144 | |
AbuAbdella | 1:c162c077430f | 145 | } |
AbuAbdella | 1:c162c077430f | 146 | |
monwonga3 | 0:bdd172e29b8b | 147 | |
monwonga3 | 0:bdd172e29b8b | 148 | |
AbuAbdella | 1:c162c077430f | 149 | // float count = 1.0f; |
AbuAbdella | 1:c162c077430f | 150 | // |
AbuAbdella | 1:c162c077430f | 151 | // while (count>=0.0f) { |
AbuAbdella | 1:c162c077430f | 152 | // |
AbuAbdella | 1:c162c077430f | 153 | // led3=1; |
AbuAbdella | 1:c162c077430f | 154 | // float sCount = count/2; |
AbuAbdella | 1:c162c077430f | 155 | // |
AbuAbdella | 1:c162c077430f | 156 | // shoulder = 0.0f + sCount; |
AbuAbdella | 1:c162c077430f | 157 | // sh = 0.0f + sCount; |
AbuAbdella | 1:c162c077430f | 158 | // |
AbuAbdella | 1:c162c077430f | 159 | // |
AbuAbdella | 1:c162c077430f | 160 | // count-= 0.01f; |
AbuAbdella | 1:c162c077430f | 161 | // |
AbuAbdella | 1:c162c077430f | 162 | // // wait(.05); |
AbuAbdella | 1:c162c077430f | 163 | // Thread::wait(50); |
AbuAbdella | 1:c162c077430f | 164 | // |
AbuAbdella | 1:c162c077430f | 165 | // } |
AbuAbdella | 1:c162c077430f | 166 | // |
AbuAbdella | 1:c162c077430f | 167 | // |
AbuAbdella | 1:c162c077430f | 168 | // while (count<=1.0f) { |
AbuAbdella | 1:c162c077430f | 169 | // |
AbuAbdella | 1:c162c077430f | 170 | // led3=0; |
AbuAbdella | 1:c162c077430f | 171 | // float sCount = count/2; |
AbuAbdella | 1:c162c077430f | 172 | // |
AbuAbdella | 1:c162c077430f | 173 | // |
AbuAbdella | 1:c162c077430f | 174 | // shoulder = 0.0f + sCount; |
AbuAbdella | 1:c162c077430f | 175 | // |
AbuAbdella | 1:c162c077430f | 176 | // if(newRedValue <= oldRedValue - .04 ){ |
AbuAbdella | 1:c162c077430f | 177 | // // shoulderPosition = 0.5f + sCount; |
AbuAbdella | 1:c162c077430f | 178 | // shoulderPosition = shoulder; |
AbuAbdella | 1:c162c077430f | 179 | // } |
AbuAbdella | 1:c162c077430f | 180 | // |
AbuAbdella | 1:c162c077430f | 181 | // count+= 0.01f; |
AbuAbdella | 1:c162c077430f | 182 | // Thread::wait(50); |
AbuAbdella | 1:c162c077430f | 183 | // |
AbuAbdella | 1:c162c077430f | 184 | // //wait(.05); |
AbuAbdella | 1:c162c077430f | 185 | // |
AbuAbdella | 1:c162c077430f | 186 | // } |
monwonga3 | 0:bdd172e29b8b | 187 | |
monwonga3 | 0:bdd172e29b8b | 188 | threadLight.terminate(); |
AbuAbdella | 1:c162c077430f | 189 | |
monwonga3 | 0:bdd172e29b8b | 190 | |
AbuAbdella | 1:c162c077430f | 191 | dunk(); |
monwonga3 | 0:bdd172e29b8b | 192 | |
monwonga3 | 0:bdd172e29b8b | 193 | threadRotate.terminate(); |
monwonga3 | 0:bdd172e29b8b | 194 | |
monwonga3 | 0:bdd172e29b8b | 195 | |
monwonga3 | 0:bdd172e29b8b | 196 | //} |
monwonga3 | 0:bdd172e29b8b | 197 | |
monwonga3 | 0:bdd172e29b8b | 198 | |
monwonga3 | 0:bdd172e29b8b | 199 | } |
monwonga3 | 0:bdd172e29b8b | 200 | |
AbuAbdella | 1:c162c077430f | 201 | |
monwonga3 | 0:bdd172e29b8b | 202 | void stroke(){ |
monwonga3 | 0:bdd172e29b8b | 203 | |
AbuAbdella | 1:c162c077430f | 204 | shoulder = shoulderPosition-.10; |
AbuAbdella | 1:c162c077430f | 205 | |
AbuAbdella | 1:c162c077430f | 206 | Thread::wait(2000); |
monwonga3 | 0:bdd172e29b8b | 207 | |
monwonga3 | 0:bdd172e29b8b | 208 | for (float i = 0.0f; i < 0.5f; i+=0.01f) { |
monwonga3 | 0:bdd172e29b8b | 209 | float wCount = i/1.5; |
monwonga3 | 0:bdd172e29b8b | 210 | float eCount = i/2; |
monwonga3 | 0:bdd172e29b8b | 211 | //hand = 1.5f ; |
monwonga3 | 0:bdd172e29b8b | 212 | wrist = 0.0f + wCount; |
monwonga3 | 0:bdd172e29b8b | 213 | elbow = 0.19f + eCount;//wCount; //0.47f; |
monwonga3 | 0:bdd172e29b8b | 214 | wait(.05); |
monwonga3 | 0:bdd172e29b8b | 215 | //printf("wCount is %f \n", wCount); |
monwonga3 | 0:bdd172e29b8b | 216 | // printf("eCount is %f \n", eCount); |
monwonga3 | 0:bdd172e29b8b | 217 | |
monwonga3 | 0:bdd172e29b8b | 218 | |
monwonga3 | 0:bdd172e29b8b | 219 | } |
AbuAbdella | 1:c162c077430f | 220 | |
monwonga3 | 0:bdd172e29b8b | 221 | for (float i = 0.0f; i < 0.5f; i+=0.01f) { |
monwonga3 | 0:bdd172e29b8b | 222 | float wCount = i/1.5; |
monwonga3 | 0:bdd172e29b8b | 223 | float eCount = i/2; |
monwonga3 | 0:bdd172e29b8b | 224 | //hand = 1.5f ; |
monwonga3 | 0:bdd172e29b8b | 225 | wrist = 0.33f - wCount; |
monwonga3 | 0:bdd172e29b8b | 226 | elbow = 0.44f - eCount;//wCount; //0.47f; |
monwonga3 | 0:bdd172e29b8b | 227 | //printf("wCount is %f \n", wCount); |
monwonga3 | 0:bdd172e29b8b | 228 | // printf("eCount is %f \n", eCount); |
monwonga3 | 0:bdd172e29b8b | 229 | wait(.05); |
monwonga3 | 0:bdd172e29b8b | 230 | |
monwonga3 | 0:bdd172e29b8b | 231 | } |
AbuAbdella | 1:c162c077430f | 232 | |
monwonga3 | 0:bdd172e29b8b | 233 | } |
monwonga3 | 0:bdd172e29b8b | 234 | |
monwonga3 | 0:bdd172e29b8b | 235 | void dunk(){ |
monwonga3 | 0:bdd172e29b8b | 236 | |
AbuAbdella | 1:c162c077430f | 237 | shoulder = 0.50f; |
monwonga3 | 0:bdd172e29b8b | 238 | |
monwonga3 | 0:bdd172e29b8b | 239 | float wristIN; |
monwonga3 | 0:bdd172e29b8b | 240 | float elbowIN; |
monwonga3 | 0:bdd172e29b8b | 241 | |
monwonga3 | 0:bdd172e29b8b | 242 | for (float i = 0.0f; i < 0.5f; i+=0.01f) { |
monwonga3 | 0:bdd172e29b8b | 243 | float wCount = i/1.5; |
monwonga3 | 0:bdd172e29b8b | 244 | float eCount = i/2; |
monwonga3 | 0:bdd172e29b8b | 245 | //hand = 1.5f ; |
monwonga3 | 0:bdd172e29b8b | 246 | wristIN = 0.2f - wCount;//equals.33f at end |
monwonga3 | 0:bdd172e29b8b | 247 | //elbowIN = 0.5f + eCount;//equals .44f at the end |
monwonga3 | 0:bdd172e29b8b | 248 | //wait(.5); |
monwonga3 | 0:bdd172e29b8b | 249 | wrist = wristIN; |
monwonga3 | 0:bdd172e29b8b | 250 | elbow = 0.3f; |
monwonga3 | 0:bdd172e29b8b | 251 | // |
monwonga3 | 0:bdd172e29b8b | 252 | // printf("wCount is %f \n", wCount); |
monwonga3 | 0:bdd172e29b8b | 253 | // printf("eCount is %f \n", eCount); |
monwonga3 | 0:bdd172e29b8b | 254 | wait(.05); |
monwonga3 | 0:bdd172e29b8b | 255 | |
monwonga3 | 0:bdd172e29b8b | 256 | } |
monwonga3 | 0:bdd172e29b8b | 257 | for (float i = 0.0f; i < 0.5f; i+=0.01f) { |
monwonga3 | 0:bdd172e29b8b | 258 | float wCount = i/1.5; |
monwonga3 | 0:bdd172e29b8b | 259 | float eCount = i/2; |
monwonga3 | 0:bdd172e29b8b | 260 | //hand = 1.5f ; |
monwonga3 | 0:bdd172e29b8b | 261 | //wait(.5); |
monwonga3 | 0:bdd172e29b8b | 262 | wrist = wristIN + wCount; |
monwonga3 | 0:bdd172e29b8b | 263 | //elbow = wristIN - eCount;//wCount; //0.47f; |
monwonga3 | 0:bdd172e29b8b | 264 | // printf("wCount is %f \n", wCount); |
monwonga3 | 0:bdd172e29b8b | 265 | // printf("eCount is %f \n", eCount); |
monwonga3 | 0:bdd172e29b8b | 266 | wait(.05); |
monwonga3 | 0:bdd172e29b8b | 267 | |
monwonga3 | 0:bdd172e29b8b | 268 | } |
monwonga3 | 0:bdd172e29b8b | 269 | |
monwonga3 | 0:bdd172e29b8b | 270 | stroke(); |
monwonga3 | 0:bdd172e29b8b | 271 | |
monwonga3 | 0:bdd172e29b8b | 272 | } |
monwonga3 | 0:bdd172e29b8b | 273 | |
monwonga3 | 0:bdd172e29b8b | 274 | |
monwonga3 | 0:bdd172e29b8b | 275 | void readLight () { |
monwonga3 | 0:bdd172e29b8b | 276 | |
monwonga3 | 0:bdd172e29b8b | 277 | |
monwonga3 | 0:bdd172e29b8b | 278 | pc.baud(9600); |
monwonga3 | 0:bdd172e29b8b | 279 | green = 1; // off |
monwonga3 | 0:bdd172e29b8b | 280 | |
monwonga3 | 0:bdd172e29b8b | 281 | // Connect to the Color sensor and verify |
monwonga3 | 0:bdd172e29b8b | 282 | |
monwonga3 | 0:bdd172e29b8b | 283 | i2c.frequency(200000); |
monwonga3 | 0:bdd172e29b8b | 284 | |
monwonga3 | 0:bdd172e29b8b | 285 | char id_regval[1] = {146}; |
monwonga3 | 0:bdd172e29b8b | 286 | char data[1] = {0}; |
monwonga3 | 0:bdd172e29b8b | 287 | i2c.write(sensor_addr,id_regval,1, true); |
monwonga3 | 0:bdd172e29b8b | 288 | i2c.read(sensor_addr,data,1,false); |
monwonga3 | 0:bdd172e29b8b | 289 | |
monwonga3 | 0:bdd172e29b8b | 290 | if (data[0]==68) { |
monwonga3 | 0:bdd172e29b8b | 291 | green = 0; |
monwonga3 | 0:bdd172e29b8b | 292 | wait (2); |
monwonga3 | 0:bdd172e29b8b | 293 | green = 1; |
monwonga3 | 0:bdd172e29b8b | 294 | } else { |
monwonga3 | 0:bdd172e29b8b | 295 | green = 1; |
monwonga3 | 0:bdd172e29b8b | 296 | } |
monwonga3 | 0:bdd172e29b8b | 297 | |
monwonga3 | 0:bdd172e29b8b | 298 | // Initialize color sensor |
monwonga3 | 0:bdd172e29b8b | 299 | |
monwonga3 | 0:bdd172e29b8b | 300 | char timing_register[2] = {129,0}; |
monwonga3 | 0:bdd172e29b8b | 301 | i2c.write(sensor_addr,timing_register,2,false); |
monwonga3 | 0:bdd172e29b8b | 302 | |
monwonga3 | 0:bdd172e29b8b | 303 | char control_register[2] = {143,0}; |
monwonga3 | 0:bdd172e29b8b | 304 | i2c.write(sensor_addr,control_register,2,false); |
monwonga3 | 0:bdd172e29b8b | 305 | |
monwonga3 | 0:bdd172e29b8b | 306 | char enable_register[2] = {128,3}; |
monwonga3 | 0:bdd172e29b8b | 307 | i2c.write(sensor_addr,enable_register,2,false); |
monwonga3 | 0:bdd172e29b8b | 308 | |
monwonga3 | 0:bdd172e29b8b | 309 | // Read data from color sensor (Clear/Red/Green/Blue) |
monwonga3 | 0:bdd172e29b8b | 310 | led = 1; |
monwonga3 | 0:bdd172e29b8b | 311 | while (true) { |
monwonga3 | 0:bdd172e29b8b | 312 | char clear_reg[1] = {148}; |
monwonga3 | 0:bdd172e29b8b | 313 | char clear_data[2] = {0,0}; |
monwonga3 | 0:bdd172e29b8b | 314 | i2c.write(sensor_addr,clear_reg,1, true); |
monwonga3 | 0:bdd172e29b8b | 315 | i2c.read(sensor_addr,clear_data,2, false); |
monwonga3 | 0:bdd172e29b8b | 316 | |
monwonga3 | 0:bdd172e29b8b | 317 | clear_value = ((int)clear_data[1] << 8) | clear_data[0]; |
monwonga3 | 0:bdd172e29b8b | 318 | |
monwonga3 | 0:bdd172e29b8b | 319 | char red_reg[1] = {150}; |
monwonga3 | 0:bdd172e29b8b | 320 | char red_data[2] = {0,0}; |
monwonga3 | 0:bdd172e29b8b | 321 | i2c.write(sensor_addr,red_reg,1, true); |
monwonga3 | 0:bdd172e29b8b | 322 | i2c.read(sensor_addr,red_data,2, false); |
monwonga3 | 0:bdd172e29b8b | 323 | |
monwonga3 | 0:bdd172e29b8b | 324 | red_value = ((int)red_data[1] << 8) | red_data[0]; |
monwonga3 | 0:bdd172e29b8b | 325 | |
monwonga3 | 0:bdd172e29b8b | 326 | char green_reg[1] = {152}; |
monwonga3 | 0:bdd172e29b8b | 327 | char green_data[2] = {0,0}; |
monwonga3 | 0:bdd172e29b8b | 328 | i2c.write(sensor_addr,green_reg,1, true); |
monwonga3 | 0:bdd172e29b8b | 329 | i2c.read(sensor_addr,green_data,2, false); |
monwonga3 | 0:bdd172e29b8b | 330 | |
monwonga3 | 0:bdd172e29b8b | 331 | green_value = ((int)green_data[1] << 8) | green_data[0]; |
monwonga3 | 0:bdd172e29b8b | 332 | |
monwonga3 | 0:bdd172e29b8b | 333 | char blue_reg[1] = {154}; |
monwonga3 | 0:bdd172e29b8b | 334 | char blue_data[2] = {0,0}; |
monwonga3 | 0:bdd172e29b8b | 335 | i2c.write(sensor_addr,blue_reg,1, true); |
monwonga3 | 0:bdd172e29b8b | 336 | i2c.read(sensor_addr,blue_data,2, false); |
monwonga3 | 0:bdd172e29b8b | 337 | |
monwonga3 | 0:bdd172e29b8b | 338 | blue_value = ((int)blue_data[1] << 8) | blue_data[0]; |
monwonga3 | 0:bdd172e29b8b | 339 | |
monwonga3 | 0:bdd172e29b8b | 340 | // print sensor readings |
monwonga3 | 0:bdd172e29b8b | 341 | red_value = red_value / clear_value; |
monwonga3 | 0:bdd172e29b8b | 342 | newRedValue = red_value; |
monwonga3 | 0:bdd172e29b8b | 343 | green_value = green_value/clear_value; |
monwonga3 | 0:bdd172e29b8b | 344 | blue_value = blue_value/clear_value; |
AbuAbdella | 1:c162c077430f | 345 | //pc.printf("Red (%.2f), old red (%.2f), new red (%.2f), shoul(%.2f)\n, Shoulder position %.2f \n", red_value, oldRedValue, newRedValue,sh ,shoulderPosition); |
monwonga3 | 0:bdd172e29b8b | 346 | //The above code displays the red, green, and blue values read in by the color sensor. |
monwonga3 | 0:bdd172e29b8b | 347 | //wait(0.5); |
AbuAbdella | 1:c162c077430f | 348 | |
AbuAbdella | 1:c162c077430f | 349 | |
AbuAbdella | 1:c162c077430f | 350 | pc.printf("new Red Value %.2f old Red value %.2f shoulder %.2f \n",newRedValue,oldRedValue,shoulderPosition); |
AbuAbdella | 1:c162c077430f | 351 | // pc.printf("%.2f\n",newRedValue); |
monwonga3 | 0:bdd172e29b8b | 352 | |
AbuAbdella | 1:c162c077430f | 353 | |
AbuAbdella | 1:c162c077430f | 354 | Thread::wait(300); |
AbuAbdella | 1:c162c077430f | 355 | oldRedValue = newRedValue; |
AbuAbdella | 1:c162c077430f | 356 | |
monwonga3 | 0:bdd172e29b8b | 357 | |
monwonga3 | 0:bdd172e29b8b | 358 | |
monwonga3 | 0:bdd172e29b8b | 359 | } |
monwonga3 | 0:bdd172e29b8b | 360 | |
monwonga3 | 0:bdd172e29b8b | 361 | |
monwonga3 | 0:bdd172e29b8b | 362 | |
monwonga3 | 0:bdd172e29b8b | 363 | } |
monwonga3 | 0:bdd172e29b8b | 364 |