this
Dependencies: ContinuousServo TCS3472_I2C Tach mbed
main.cpp@0:389040618ce2, 2018-04-30 (annotated)
- Committer:
- georgezolovick
- Date:
- Mon Apr 30 17:16:55 2018 +0000
- Revision:
- 0:389040618ce2
- Child:
- 1:50677ba704f5
30Apr2018
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
georgezolovick | 0:389040618ce2 | 1 | #include "mbed.h" |
georgezolovick | 0:389040618ce2 | 2 | #include "ContinuousServo.h" |
georgezolovick | 0:389040618ce2 | 3 | #include "Tach.h" |
georgezolovick | 0:389040618ce2 | 4 | #include "TCS3472_I2C.h" |
georgezolovick | 0:389040618ce2 | 5 | |
georgezolovick | 0:389040618ce2 | 6 | ContinuousServo left(p23); |
georgezolovick | 0:389040618ce2 | 7 | ContinuousServo right(p26); |
georgezolovick | 0:389040618ce2 | 8 | AnalogIn sonar(p19); |
georgezolovick | 0:389040618ce2 | 9 | PwmOut LB(p22); // PWM out signal to power LED on the sensor |
georgezolovick | 0:389040618ce2 | 10 | TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object |
georgezolovick | 0:389040618ce2 | 11 | DigitalOut hallpwr(p22); |
georgezolovick | 0:389040618ce2 | 12 | DigitalIn hall(p21); |
georgezolovick | 0:389040618ce2 | 13 | BusOut flash(LED1, LED2, LED3, LED4); |
georgezolovick | 0:389040618ce2 | 14 | Ticker mine; |
georgezolovick | 0:389040618ce2 | 15 | Ticker ultra; |
georgezolovick | 0:389040618ce2 | 16 | Serial pc(USBTX,USBRX); |
georgezolovick | 0:389040618ce2 | 17 | |
georgezolovick | 0:389040618ce2 | 18 | float wall = 100.0; |
georgezolovick | 0:389040618ce2 | 19 | float analog; |
georgezolovick | 0:389040618ce2 | 20 | float range; |
georgezolovick | 0:389040618ce2 | 21 | |
georgezolovick | 0:389040618ce2 | 22 | int rgb_data[4]; // declare a 4 element array to store RGB sensor readings |
georgezolovick | 0:389040618ce2 | 23 | float PWMbrightness = 1.0; // float specifying brightness of LED (between 0.0 and 1.0) |
georgezolovick | 0:389040618ce2 | 24 | int x; |
georgezolovick | 0:389040618ce2 | 25 | float a,b,c; |
georgezolovick | 0:389040618ce2 | 26 | |
georgezolovick | 0:389040618ce2 | 27 | void update() |
georgezolovick | 0:389040618ce2 | 28 | { |
georgezolovick | 0:389040618ce2 | 29 | a = 2.3*156.25*sonar.read(); |
georgezolovick | 0:389040618ce2 | 30 | //pc.printf("%f\r\n", a); |
georgezolovick | 0:389040618ce2 | 31 | wait(0.01); |
georgezolovick | 0:389040618ce2 | 32 | b = 2.3*156.25*sonar.read(); |
georgezolovick | 0:389040618ce2 | 33 | //pc.printf("%f\r\n", b); |
georgezolovick | 0:389040618ce2 | 34 | wait(0.01); |
georgezolovick | 0:389040618ce2 | 35 | c = 2.3*156.25*sonar.read(); |
georgezolovick | 0:389040618ce2 | 36 | //pc.printf("%f\r\n", c); |
georgezolovick | 0:389040618ce2 | 37 | wait(0.01); |
georgezolovick | 0:389040618ce2 | 38 | wall = (a+b+c)/3.0; |
georgezolovick | 0:389040618ce2 | 39 | pc.printf("%f\r\n", wall); |
georgezolovick | 0:389040618ce2 | 40 | } |
georgezolovick | 0:389040618ce2 | 41 | |
georgezolovick | 0:389040618ce2 | 42 | void sweep() |
georgezolovick | 0:389040618ce2 | 43 | { |
georgezolovick | 0:389040618ce2 | 44 | if(hall.read() == 1){ |
georgezolovick | 0:389040618ce2 | 45 | flash = 8.0; |
georgezolovick | 0:389040618ce2 | 46 | wait(0.1); |
georgezolovick | 0:389040618ce2 | 47 | hallpwr = 0; |
georgezolovick | 0:389040618ce2 | 48 | } |
georgezolovick | 0:389040618ce2 | 49 | else{ |
georgezolovick | 0:389040618ce2 | 50 | flash = 0.0; |
georgezolovick | 0:389040618ce2 | 51 | } |
georgezolovick | 0:389040618ce2 | 52 | } |
georgezolovick | 0:389040618ce2 | 53 | |
georgezolovick | 0:389040618ce2 | 54 | int main() |
georgezolovick | 0:389040618ce2 | 55 | { |
georgezolovick | 0:389040618ce2 | 56 | ultra.attach(&update, 0.1); |
georgezolovick | 0:389040618ce2 | 57 | |
georgezolovick | 0:389040618ce2 | 58 | hall.mode(PullUp); |
georgezolovick | 0:389040618ce2 | 59 | hallpwr = 1; |
georgezolovick | 0:389040618ce2 | 60 | mine.attach(&sweep, 0.1); |
georgezolovick | 0:389040618ce2 | 61 | |
georgezolovick | 0:389040618ce2 | 62 | rgb_sensor.enablePowerAndRGBC(); // Enable RGB sensor |
georgezolovick | 0:389040618ce2 | 63 | rgb_sensor.setIntegrationTime(100); // Set integration time of sensor |
georgezolovick | 0:389040618ce2 | 64 | |
georgezolovick | 0:389040618ce2 | 65 | wait(3); |
georgezolovick | 0:389040618ce2 | 66 | |
georgezolovick | 0:389040618ce2 | 67 | while(wall>=6.9) { // main code for driving goes here |
georgezolovick | 0:389040618ce2 | 68 | //x = hall.read(); |
georgezolovick | 0:389040618ce2 | 69 | //if(x == 1) { |
georgezolovick | 0:389040618ce2 | 70 | // flash = 8.0; |
georgezolovick | 0:389040618ce2 | 71 | // wait(0.25); |
georgezolovick | 0:389040618ce2 | 72 | // x = 0; |
georgezolovick | 0:389040618ce2 | 73 | //} else if (x == 0){ |
georgezolovick | 0:389040618ce2 | 74 | // flash = 0.0; |
georgezolovick | 0:389040618ce2 | 75 | //} |
georgezolovick | 0:389040618ce2 | 76 | //x = hall.read(); |
georgezolovick | 0:389040618ce2 | 77 | |
georgezolovick | 0:389040618ce2 | 78 | LB = PWMbrightness; // set brightness of sensor LED |
georgezolovick | 0:389040618ce2 | 79 | rgb_sensor.getAllColors( rgb_data ); |
georgezolovick | 0:389040618ce2 | 80 | pc.printf("%d\r\n", rgb_data[1]); |
georgezolovick | 0:389040618ce2 | 81 | left.speed(.15); |
georgezolovick | 0:389040618ce2 | 82 | right.speed(-.25); |
georgezolovick | 0:389040618ce2 | 83 | if(rgb_data[1]<900) { |
georgezolovick | 0:389040618ce2 | 84 | left.speed(.1); |
georgezolovick | 0:389040618ce2 | 85 | right.speed(-.25); |
georgezolovick | 0:389040618ce2 | 86 | } else if(rgb_data[1]>1000) { |
georgezolovick | 0:389040618ce2 | 87 | left.speed(.19); |
georgezolovick | 0:389040618ce2 | 88 | right.speed(-.18); |
georgezolovick | 0:389040618ce2 | 89 | } else{ |
georgezolovick | 0:389040618ce2 | 90 | left.speed(.15); |
georgezolovick | 0:389040618ce2 | 91 | right.speed(-.25); |
georgezolovick | 0:389040618ce2 | 92 | } |
georgezolovick | 0:389040618ce2 | 93 | } |
georgezolovick | 0:389040618ce2 | 94 | left.stop(); |
georgezolovick | 0:389040618ce2 | 95 | right.stop(); |
georgezolovick | 0:389040618ce2 | 96 | } |