Thomas Larrea / Mbed 2 deprecated ES202_Final_ColorSensor

not sure on the if statement at the bottom

Dependencies:   ContinuousServo Tach mbed TCS3472_I2C

color.cpp@0:7d319151aaa0, 2018-04-25 (annotated)

Committer:
PlayaLarrea
Date:
Wed Apr 25 13:33:13 2018 +0000
Revision:
0:7d319151aaa0
Child:
1:1dd468e2ad40
uh

Who changed what in which revision?

UserRevisionLine numberNew contents of line
PlayaLarrea 0:7d319151aaa0 1 #include "mbed.h"
PlayaLarrea 0:7d319151aaa0 2 #include "ContinuousServo.h"
PlayaLarrea 0:7d319151aaa0 3 #include "Tach.h"
PlayaLarrea 0:7d319151aaa0 4 #include "TCS3472_I2C.h"
PlayaLarrea 0:7d319151aaa0 5
PlayaLarrea 0:7d319151aaa0 6 DigitalIn hall(p21);
PlayaLarrea 0:7d319151aaa0 7 InterruptIn hall(p21);
PlayaLarrea 0:7d319151aaa0 8 BusOut flash(LED1, LED2, LED3, LED4);
PlayaLarrea 0:7d319151aaa0 9 TCS3472_I2C rgb_sensor(p9, p10);
PlayaLarrea 0:7d319151aaa0 10 ContinuousServo left(p23); //Set up left wheel driver
PlayaLarrea 0:7d319151aaa0 11 ContinuousServo right(p26); //Set up right wheel driver
PlayaLarrea 0:7d319151aaa0 12 Tach tleft(p17,64); //Set up left tachometer
PlayaLarrea 0:7d319151aaa0 13 Tach tright(p13, 64); //Set up right tachometer
PlayaLarrea 0:7d319151aaa0 14
PlayaLarrea 0:7d319151aaa0 15 float wl; //Left wheel velocity
PlayaLarrea 0:7d319151aaa0 16 float wr; //Right wheel velocity
PlayaLarrea 0:7d319151aaa0 17 float e; //Error
PlayaLarrea 0:7d319151aaa0 18
PlayaLarrea 0:7d319151aaa0 19 float r = 1.3125; //radius of wheels
PlayaLarrea 0:7d319151aaa0 20 float l = 4.0625; //width of wheel base
PlayaLarrea 0:7d319151aaa0 21 float omega = 0.0; //angular velocity of wheels
PlayaLarrea 0:7d319151aaa0 22 float v = 0.5; //translational velocity of wheels
PlayaLarrea 0:7d319151aaa0 23 float kp = 1.0; //Proportional control gain
PlayaLarrea 0:7d319151aaa0 24
PlayaLarrea 0:7d319151aaa0 25 float vl = ((omega*l)+(2*v))/(2*r); //Calculates left wheel velocity
PlayaLarrea 0:7d319151aaa0 26 float vr = ((omega*l)-(2*v))/(2*r); //Calculates right wheel velocity
PlayaLarrea 0:7d319151aaa0 27
PlayaLarrea 0:7d319151aaa0 28 int counter = 0.0;
PlayaLarrea 0:7d319151aaa0 29
PlayaLarrea 0:7d319151aaa0 30 void mine() {
PlayaLarrea 0:7d319151aaa0 31 flash = 16.0;
PlayaLarrea 0:7d319151aaa0 32 }
PlayaLarrea 0:7d319151aaa0 33
PlayaLarrea 0:7d319151aaa0 34 int main() {
PlayaLarrea 0:7d319151aaa0 35
PlayaLarrea 0:7d319151aaa0 36 hall.rise(&mine);
PlayaLarrea 0:7d319151aaa0 37 hall.fall(&mine);
PlayaLarrea 0:7d319151aaa0 38
PlayaLarrea 0:7d319151aaa0 39 while(1) { // wait around, interrupts will interrupt this!
PlayaLarrea 0:7d319151aaa0 40 left.speed(vl); //Move the left wheel
PlayaLarrea 0:7d319151aaa0 41 right.speed(vr); //Move the right wheel
PlayaLarrea 0:7d319151aaa0 42 wl = tleft.getSpeed(); //Read the left tachometer
PlayaLarrea 0:7d319151aaa0 43 wr = tright.getSpeed(); //Read the right tachometer
PlayaLarrea 0:7d319151aaa0 44
PlayaLarrea 0:7d319151aaa0 45 e = wl - (-wr); //Calculate the error
PlayaLarrea 0:7d319151aaa0 46 vr = vr - (kp*e); //Readjust the right wheel velocity
PlayaLarrea 0:7d319151aaa0 47
PlayaLarrea 0:7d319151aaa0 48 if(rgb_sensor){
PlayaLarrea 0:7d319151aaa0 49 flash = counter;
PlayaLarrea 0:7d319151aaa0 50 }
PlayaLarrea 0:7d319151aaa0 51 }
PlayaLarrea 0:7d319151aaa0 52 }