Object Avoidance Program
Dependencies: 4DGL-uLCD-SE Motordriver mbed
main.cpp@6:c76078abc03e, 2014-04-29 (annotated)
- Committer:
- rokash000
- Date:
- Tue Apr 29 22:41:58 2014 +0000
- Revision:
- 6:c76078abc03e
- Parent:
- 5:50887f863c01
WORKING CHASSIS!
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rokash000 | 0:eb26279eeb24 | 1 | |
rokash000 | 0:eb26279eeb24 | 2 | #include "mbed.h" |
rokash000 | 0:eb26279eeb24 | 3 | #include "motordriver.h" |
rokash000 | 0:eb26279eeb24 | 4 | //#include "SHARPIR.h" |
rokash000 | 0:eb26279eeb24 | 5 | #include "uLCD_4DGL.h" |
rokash000 | 0:eb26279eeb24 | 6 | |
rokash000 | 0:eb26279eeb24 | 7 | Motor mA(p22, p6, p5, 1); // pwm, fwd, rev |
rokash000 | 0:eb26279eeb24 | 8 | Motor mB(p23, p8, p7, 1); // pwm, fwd, rev |
rokash000 | 0:eb26279eeb24 | 9 | |
rokash000 | 0:eb26279eeb24 | 10 | |
rokash000 | 0:eb26279eeb24 | 11 | Serial pc(USBTX, USBRX); // tx, rx |
rokash000 | 0:eb26279eeb24 | 12 | DigitalOut led_back(p16); // test led for Back_sensor |
rokash000 | 0:eb26279eeb24 | 13 | DigitalOut led_left(p15); // test led for Back_sensor |
rokash000 | 0:eb26279eeb24 | 14 | DigitalOut led_right(p14); // test led for Back_sensor |
rokash000 | 0:eb26279eeb24 | 15 | DigitalOut led_front(p13); // test led for Back_sensor |
rokash000 | 0:eb26279eeb24 | 16 | |
rokash000 | 0:eb26279eeb24 | 17 | AnalogIn irBack_ain(p18); |
rokash000 | 0:eb26279eeb24 | 18 | AnalogIn irFront_ain(p17); |
rokash000 | 0:eb26279eeb24 | 19 | AnalogIn irLeft_ain(p20); |
rokash000 | 0:eb26279eeb24 | 20 | AnalogIn irRight_ain(p19); |
rokash000 | 0:eb26279eeb24 | 21 | |
rokash000 | 0:eb26279eeb24 | 22 | //SHARPIR sensor_back(p18); //the output of the sharpIR sensor is connected to the MBEDs pin 10. |
rokash000 | 0:eb26279eeb24 | 23 | |
rokash000 | 0:eb26279eeb24 | 24 | //uLCD_4DGL uLCD(p28, p27, p29); // create a global lcd object |
rokash000 | 0:eb26279eeb24 | 25 | |
rokash000 | 0:eb26279eeb24 | 26 | //speed => 1 = forward, -1 = reverse ... range from -1 to 1 |
rokash000 | 0:eb26279eeb24 | 27 | |
rokash000 | 0:eb26279eeb24 | 28 | int main() { |
rokash000 | 0:eb26279eeb24 | 29 | //uLCD.cls(); |
rokash000 | 0:eb26279eeb24 | 30 | float backsensor = 0.0; |
rokash000 | 0:eb26279eeb24 | 31 | float frontsensor = 0.0; |
rokash000 | 0:eb26279eeb24 | 32 | float leftsensor = 0.0; |
rokash000 | 0:eb26279eeb24 | 33 | float rightsensor = 0.0; |
rokash000 | 0:eb26279eeb24 | 34 | int sensor = 0; |
rokash000 | 0:eb26279eeb24 | 35 | while(1){ |
rokash000 | 0:eb26279eeb24 | 36 | |
rokash000 | 0:eb26279eeb24 | 37 | |
rokash000 | 0:eb26279eeb24 | 38 | |
rokash000 | 0:eb26279eeb24 | 39 | |
rokash000 | 0:eb26279eeb24 | 40 | |
rokash000 | 0:eb26279eeb24 | 41 | //float backsensor = sensor_back.cm(); |
rokash000 | 0:eb26279eeb24 | 42 | |
rokash000 | 0:eb26279eeb24 | 43 | backsensor = irBack_ain * 100; |
rokash000 | 0:eb26279eeb24 | 44 | // pc.printf("FRONT Sensor is: %f \n", backsensor); |
rokash000 | 0:eb26279eeb24 | 45 | frontsensor = irFront_ain * 100; |
rokash000 | 0:eb26279eeb24 | 46 | leftsensor = irLeft_ain * 100; |
rokash000 | 0:eb26279eeb24 | 47 | rightsensor = irRight_ain * 100; |
rokash000 | 0:eb26279eeb24 | 48 | //pc.printf("Front Sensor is: %f \n", frontsensor); |
rokash000 | 0:eb26279eeb24 | 49 | // pc.printf("Left Sensor is: %f \n", leftsensor); |
rokash000 | 0:eb26279eeb24 | 50 | // pc.printf("Right Sensor is: %f \n", rightsensor); |
rokash000 | 6:c76078abc03e | 51 | // |
rokash000 | 6:c76078abc03e | 52 | if((backsensor > 52)&&(backsensor < 100)){ |
rokash000 | 6:c76078abc03e | 53 | sensor = 3;} |
rokash000 | 6:c76078abc03e | 54 | else if((frontsensor > 52)&&(frontsensor < 100)){ |
rokash000 | 6:c76078abc03e | 55 | sensor = 2;} |
rokash000 | 6:c76078abc03e | 56 | else if((leftsensor > 52)&&(leftsensor < 100)){ |
rokash000 | 6:c76078abc03e | 57 | sensor = 1;} |
rokash000 | 6:c76078abc03e | 58 | else if((rightsensor > 52)&&(rightsensor < 100)){ |
rokash000 | 6:c76078abc03e | 59 | sensor = 0;} |
rokash000 | 6:c76078abc03e | 60 | |
rokash000 | 6:c76078abc03e | 61 | |
rokash000 | 6:c76078abc03e | 62 | switch(sensor) |
rokash000 | 6:c76078abc03e | 63 | { |
rokash000 | 6:c76078abc03e | 64 | case 0: //Right |
rokash000 | 6:c76078abc03e | 65 | mA.speed(.4); |
rokash000 | 6:c76078abc03e | 66 | mB.speed(.6); |
rokash000 | 6:c76078abc03e | 67 | wait(0.5); |
rokash000 | 6:c76078abc03e | 68 | led_right = 1; |
rokash000 | 6:c76078abc03e | 69 | break; |
rokash000 | 6:c76078abc03e | 70 | case 1: // Left |
rokash000 | 6:c76078abc03e | 71 | mA.speed(.6); |
rokash000 | 6:c76078abc03e | 72 | mB.speed(.4); |
rokash000 | 6:c76078abc03e | 73 | wait(0.5); |
rokash000 | 6:c76078abc03e | 74 | led_left = 1; |
rokash000 | 6:c76078abc03e | 75 | break; |
rokash000 | 6:c76078abc03e | 76 | case 2: // Front |
rokash000 | 6:c76078abc03e | 77 | mA.speed(-.6); |
rokash000 | 6:c76078abc03e | 78 | mB.speed(-.8); |
rokash000 | 6:c76078abc03e | 79 | wait(0.9); |
rokash000 | 6:c76078abc03e | 80 | mA.stop(0.5); |
rokash000 | 6:c76078abc03e | 81 | mB.stop(0.5); |
rokash000 | 6:c76078abc03e | 82 | mA.speed(-.6); |
rokash000 | 6:c76078abc03e | 83 | mB.speed(-.8); |
rokash000 | 6:c76078abc03e | 84 | wait(0.9); |
rokash000 | 6:c76078abc03e | 85 | mA.speed(.2); |
rokash000 | 6:c76078abc03e | 86 | mB.speed(.2); |
rokash000 | 6:c76078abc03e | 87 | wait(0.9); |
rokash000 | 6:c76078abc03e | 88 | led_front = 1; |
rokash000 | 6:c76078abc03e | 89 | break; |
rokash000 | 6:c76078abc03e | 90 | case 3: // Back |
rokash000 | 6:c76078abc03e | 91 | mA.speed(.4); |
rokash000 | 6:c76078abc03e | 92 | mB.speed(.4); |
rokash000 | 6:c76078abc03e | 93 | wait(0.5); |
rokash000 | 6:c76078abc03e | 94 | mA.stop(0.5); |
rokash000 | 6:c76078abc03e | 95 | mB.stop(0.5); |
rokash000 | 6:c76078abc03e | 96 | mA.speed(.6); |
rokash000 | 6:c76078abc03e | 97 | mB.speed(.6); |
rokash000 | 6:c76078abc03e | 98 | led_back = 1; |
rokash000 | 6:c76078abc03e | 99 | break; |
rokash000 | 6:c76078abc03e | 100 | default : |
rokash000 | 6:c76078abc03e | 101 | mA.speed(.6); |
rokash000 | 6:c76078abc03e | 102 | mB.speed(.6); |
rokash000 | 6:c76078abc03e | 103 | wait(0.5); |
rokash000 | 6:c76078abc03e | 104 | led_left = 0; |
rokash000 | 6:c76078abc03e | 105 | led_back = 0; |
rokash000 | 6:c76078abc03e | 106 | led_front = 0; |
rokash000 | 6:c76078abc03e | 107 | led_right = 0; |
rokash000 | 6:c76078abc03e | 108 | break; |
rokash000 | 1:ab45b552644b | 109 | } |
rokash000 | 6:c76078abc03e | 110 | |
rokash000 | 6:c76078abc03e | 111 | sensor = 4; //DEFAULT CONDITION |
rokash000 | 6:c76078abc03e | 112 | // Back Sensor |
rokash000 | 6:c76078abc03e | 113 | // if((backsensor > 52)&&(backsensor < 100)){ |
rokash000 | 6:c76078abc03e | 114 | // mA.speed(.8); |
rokash000 | 6:c76078abc03e | 115 | // mB.speed(.8); |
rokash000 | 6:c76078abc03e | 116 | // led_back = 1; |
rokash000 | 6:c76078abc03e | 117 | // |
rokash000 | 6:c76078abc03e | 118 | // } |
rokash000 | 6:c76078abc03e | 119 | // // front Sensor |
rokash000 | 6:c76078abc03e | 120 | // else if((frontsensor > 52)&&(frontsensor < 100)){ |
rokash000 | 6:c76078abc03e | 121 | // mA.speed(-1); |
rokash000 | 6:c76078abc03e | 122 | // mB.speed(-1); |
rokash000 | 6:c76078abc03e | 123 | // led_front = 1; |
rokash000 | 6:c76078abc03e | 124 | // |
rokash000 | 6:c76078abc03e | 125 | // }else |
rokash000 | 6:c76078abc03e | 126 | // // right Sensor |
rokash000 | 6:c76078abc03e | 127 | // if((rightsensor > 52)&&(rightsensor < 100)){ |
rokash000 | 6:c76078abc03e | 128 | // mA.speed(.4); |
rokash000 | 6:c76078abc03e | 129 | // mB.speed(.6); |
rokash000 | 6:c76078abc03e | 130 | // led_right = 1; |
rokash000 | 6:c76078abc03e | 131 | // |
rokash000 | 6:c76078abc03e | 132 | // }else // Left Sensor |
rokash000 | 6:c76078abc03e | 133 | // if((leftsensor > 52)&&(leftsensor < 100)){ |
rokash000 | 6:c76078abc03e | 134 | // mA.speed(.6); |
rokash000 | 6:c76078abc03e | 135 | // mB.speed(.4); |
rokash000 | 6:c76078abc03e | 136 | // led_left = 1; |
rokash000 | 6:c76078abc03e | 137 | // |
rokash000 | 6:c76078abc03e | 138 | // }else{ |
rokash000 | 6:c76078abc03e | 139 | // mA.speed(1); |
rokash000 | 6:c76078abc03e | 140 | // mB.speed(1); |
rokash000 | 6:c76078abc03e | 141 | // led_left = 0; |
rokash000 | 6:c76078abc03e | 142 | // led_back = 0; |
rokash000 | 6:c76078abc03e | 143 | // led_front = 0; |
rokash000 | 6:c76078abc03e | 144 | // led_right = 0; |
rokash000 | 6:c76078abc03e | 145 | // } |
rokash000 | 6:c76078abc03e | 146 | |
rokash000 | 6:c76078abc03e | 147 | |
rokash000 | 6:c76078abc03e | 148 | wait(0.1); |
rokash000 | 6:c76078abc03e | 149 | } |
rokash000 | 0:eb26279eeb24 | 150 | } |