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