Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of WorkingwithIR by
main.cpp@3:bac13ce5f5d0, 2014-05-26 (annotated)
- Committer:
- bayagich
- Date:
- Mon May 26 09:34:16 2014 +0000
- Revision:
- 3:bac13ce5f5d0
- Parent:
- 2:b5031bb5303e
- Child:
- 4:057e904b1395
Cross detection that detects corners and intersection and stops; Does NOT have bluetooth commands to tell it what to do at interection
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mmpeter | 0:9ab1097149ca | 1 | #include "mbed.h" |
mmpeter | 0:9ab1097149ca | 2 | #include "m3pi_ng.h" |
mmpeter | 0:9ab1097149ca | 3 | #include "cmath" |
mmpeter | 0:9ab1097149ca | 4 | #include "iostream" |
mmpeter | 0:9ab1097149ca | 5 | |
mmpeter | 0:9ab1097149ca | 6 | //Access infared sensors |
mmpeter | 0:9ab1097149ca | 7 | DigitalIn m3pi_IN[] = {(p12)}; |
mmpeter | 0:9ab1097149ca | 8 | DigitalOut led_1(p13); |
mmpeter | 0:9ab1097149ca | 9 | |
mmpeter | 0:9ab1097149ca | 10 | using namespace std; |
bayagich | 2:b5031bb5303e | 11 | |
bayagich | 3:bac13ce5f5d0 | 12 | bool cross_detection(int sensor[5], int black_thresh, int white_thresh); |
mmpeter | 0:9ab1097149ca | 13 | |
mmpeter | 0:9ab1097149ca | 14 | m3pi thinggy; |
mmpeter | 0:9ab1097149ca | 15 | |
mmpeter | 0:9ab1097149ca | 16 | int main() { |
mmpeter | 0:9ab1097149ca | 17 | |
mmpeter | 0:9ab1097149ca | 18 | float speed = 0.25; |
mmpeter | 0:9ab1097149ca | 19 | float turn_speed = 0.2; |
mmpeter | 0:9ab1097149ca | 20 | float correction; |
mmpeter | 0:9ab1097149ca | 21 | float k = -0.3; |
mmpeter | 0:9ab1097149ca | 22 | int sensor[5]; |
mmpeter | 0:9ab1097149ca | 23 | int returned; |
bayagich | 2:b5031bb5303e | 24 | int black_thresh = 300; |
bayagich | 2:b5031bb5303e | 25 | int white_thresh = 240; |
bayagich | 3:bac13ce5f5d0 | 26 | bool cross = 0; |
mmpeter | 0:9ab1097149ca | 27 | thinggy.locate(0,1); |
mmpeter | 0:9ab1097149ca | 28 | thinggy.printf("Villan"); |
mmpeter | 0:9ab1097149ca | 29 | thinggy.locate(0,0); |
mmpeter | 0:9ab1097149ca | 30 | thinggy.printf("Pimpin"); |
mmpeter | 0:9ab1097149ca | 31 | |
mmpeter | 0:9ab1097149ca | 32 | wait(1.0); |
mmpeter | 0:9ab1097149ca | 33 | |
mmpeter | 0:9ab1097149ca | 34 | thinggy.sensor_auto_calibrate(); |
mmpeter | 0:9ab1097149ca | 35 | |
mmpeter | 0:9ab1097149ca | 36 | thinggy.calibrated_sensor(sensor); |
mmpeter | 0:9ab1097149ca | 37 | |
mmpeter | 0:9ab1097149ca | 38 | //find the average of the three sensors |
mmpeter | 0:9ab1097149ca | 39 | returned = (sensor[1] + sensor[2] + sensor[3])/3; |
mmpeter | 0:9ab1097149ca | 40 | |
mmpeter | 0:9ab1097149ca | 41 | while(1) { |
mmpeter | 0:9ab1097149ca | 42 | //check if it needs to turn |
mmpeter | 0:9ab1097149ca | 43 | while(returned <= 240){ |
mmpeter | 0:9ab1097149ca | 44 | //turns right |
mmpeter | 0:9ab1097149ca | 45 | while(sensor[0] < sensor[4] && thinggy.line_position() != 0){ |
mmpeter | 0:9ab1097149ca | 46 | thinggy.left_motor(turn_speed); |
mmpeter | 0:9ab1097149ca | 47 | thinggy.right_motor(-turn_speed); |
mmpeter | 0:9ab1097149ca | 48 | thinggy.calibrated_sensor(sensor); |
bayagich | 3:bac13ce5f5d0 | 49 | cross = cross_detection(sensor, black_thresh, white_thresh); |
bayagich | 3:bac13ce5f5d0 | 50 | if(cross){ |
bayagich | 3:bac13ce5f5d0 | 51 | //ask for command (left, right or forward) |
bayagich | 3:bac13ce5f5d0 | 52 | } |
mmpeter | 0:9ab1097149ca | 53 | } |
mmpeter | 0:9ab1097149ca | 54 | //turns left |
mmpeter | 0:9ab1097149ca | 55 | while(sensor[4] > sensor[0] && thinggy.line_position() != 0){ |
mmpeter | 0:9ab1097149ca | 56 | thinggy.left_motor(-turn_speed); |
mmpeter | 0:9ab1097149ca | 57 | thinggy.right_motor(turn_speed); |
mmpeter | 0:9ab1097149ca | 58 | thinggy.calibrated_sensor(sensor); |
bayagich | 3:bac13ce5f5d0 | 59 | cross = cross_detection(sensor, black_thresh, white_thresh); |
bayagich | 3:bac13ce5f5d0 | 60 | if(cross){ |
bayagich | 3:bac13ce5f5d0 | 61 | //ask for command (left, right or forward) |
bayagich | 3:bac13ce5f5d0 | 62 | } |
mmpeter | 0:9ab1097149ca | 63 | } |
mmpeter | 0:9ab1097149ca | 64 | thinggy.calibrated_sensor(sensor); |
mmpeter | 1:4f52a001926a | 65 | returned = (sensor[1] + sensor[2]*2 + sensor[3])/4; |
mmpeter | 0:9ab1097149ca | 66 | }//while returned <= 220 |
mmpeter | 0:9ab1097149ca | 67 | |
mmpeter | 0:9ab1097149ca | 68 | // Curves and straightaways |
mmpeter | 0:9ab1097149ca | 69 | while(returned > 240){ |
bayagich | 2:b5031bb5303e | 70 | |
bayagich | 2:b5031bb5303e | 71 | cross_detection(sensor, black_thresh, white_thresh); |
bayagich | 2:b5031bb5303e | 72 | |
mmpeter | 0:9ab1097149ca | 73 | float position = thinggy.line_position(); |
mmpeter | 0:9ab1097149ca | 74 | correction = k*(position); |
mmpeter | 0:9ab1097149ca | 75 | |
mmpeter | 0:9ab1097149ca | 76 | // -1.0 is far left, 1.0 is far right, 0.0 in the middle |
mmpeter | 0:9ab1097149ca | 77 | |
mmpeter | 0:9ab1097149ca | 78 | //speed limiting for right motor |
mmpeter | 0:9ab1097149ca | 79 | if(speed + correction > 1){ |
mmpeter | 0:9ab1097149ca | 80 | thinggy.right_motor(0.6); |
mmpeter | 0:9ab1097149ca | 81 | thinggy.left_motor(speed-correction); |
mmpeter | 0:9ab1097149ca | 82 | } |
mmpeter | 0:9ab1097149ca | 83 | |
mmpeter | 0:9ab1097149ca | 84 | //speed limiting for left motor |
mmpeter | 0:9ab1097149ca | 85 | if(speed - correction > 1){ |
mmpeter | 0:9ab1097149ca | 86 | thinggy.left_motor(0.6); |
mmpeter | 0:9ab1097149ca | 87 | thinggy.right_motor(speed+correction); |
mmpeter | 0:9ab1097149ca | 88 | } |
mmpeter | 0:9ab1097149ca | 89 | else{ |
mmpeter | 0:9ab1097149ca | 90 | thinggy.left_motor(speed-correction); |
mmpeter | 0:9ab1097149ca | 91 | thinggy.right_motor(speed+correction); |
mmpeter | 0:9ab1097149ca | 92 | |
mmpeter | 0:9ab1097149ca | 93 | //Infared: stop if obstructed |
mmpeter | 0:9ab1097149ca | 94 | m3pi_IN[0].mode(PullUp); |
mmpeter | 0:9ab1097149ca | 95 | while (m3pi_IN[0]==0){ |
mmpeter | 0:9ab1097149ca | 96 | thinggy.stop(); |
mmpeter | 0:9ab1097149ca | 97 | } |
mmpeter | 0:9ab1097149ca | 98 | } |
mmpeter | 0:9ab1097149ca | 99 | thinggy.calibrated_sensor(sensor); |
mmpeter | 1:4f52a001926a | 100 | returned = (sensor[1] + sensor[2]*2 + sensor[3])/4; |
mmpeter | 0:9ab1097149ca | 101 | }//while returned > 220 |
mmpeter | 0:9ab1097149ca | 102 | |
bayagich | 2:b5031bb5303e | 103 | thinggy.calibrated_sensor(sensor); |
bayagich | 2:b5031bb5303e | 104 | returned = (sensor[1] + sensor[2]*2 + sensor[3])/4; |
mmpeter | 0:9ab1097149ca | 105 | }//while(1) |
bayagich | 2:b5031bb5303e | 106 | |
bayagich | 2:b5031bb5303e | 107 | |
mmpeter | 0:9ab1097149ca | 108 | } |
mmpeter | 0:9ab1097149ca | 109 | |
bayagich | 2:b5031bb5303e | 110 | //REQUIRES: array of 5 ints |
bayagich | 2:b5031bb5303e | 111 | //EFFECTS: stops the robot if it comes to any interesection where a decision has to be made |
bayagich | 3:bac13ce5f5d0 | 112 | // and returns true if there is a cross |
bayagich | 3:bac13ce5f5d0 | 113 | bool cross_detection(int sensor[5], int black_thresh, int white_thresh){ |
bayagich | 2:b5031bb5303e | 114 | //three directions to choose from NOT WORKING |
bayagich | 3:bac13ce5f5d0 | 115 | if(sensor[0] > black_thresh and sensor[2] > black_thresh and sensor[4] > black_thresh){ |
bayagich | 2:b5031bb5303e | 116 | thinggy.stop(); |
bayagich | 3:bac13ce5f5d0 | 117 | wait(300); |
bayagich | 3:bac13ce5f5d0 | 118 | return 1; |
bayagich | 2:b5031bb5303e | 119 | } |
bayagich | 2:b5031bb5303e | 120 | //left or forward |
bayagich | 3:bac13ce5f5d0 | 121 | else if(sensor[0] > black_thresh and sensor[2] > black_thresh){ |
bayagich | 2:b5031bb5303e | 122 | thinggy.stop(); |
bayagich | 2:b5031bb5303e | 123 | wait(300); |
bayagich | 3:bac13ce5f5d0 | 124 | return 1; |
bayagich | 2:b5031bb5303e | 125 | } |
bayagich | 2:b5031bb5303e | 126 | //left or right WORKING |
bayagich | 2:b5031bb5303e | 127 | else if (sensor[0] > black_thresh and sensor[1] < white_thresh and sensor[2] < white_thresh and sensor[3] < white_thresh and sensor[4] > black_thresh ){ |
bayagich | 2:b5031bb5303e | 128 | thinggy.stop(); |
bayagich | 3:bac13ce5f5d0 | 129 | wait(300); |
bayagich | 3:bac13ce5f5d0 | 130 | return 1; |
bayagich | 2:b5031bb5303e | 131 | } |
bayagich | 2:b5031bb5303e | 132 | //forward or right |
bayagich | 3:bac13ce5f5d0 | 133 | else if (sensor[2] > black_thresh and sensor[4] > black_thresh){ |
bayagich | 2:b5031bb5303e | 134 | thinggy.stop(); |
bayagich | 3:bac13ce5f5d0 | 135 | wait(300); |
bayagich | 3:bac13ce5f5d0 | 136 | return 1; |
bayagich | 2:b5031bb5303e | 137 | } |
bayagich | 3:bac13ce5f5d0 | 138 | |
bayagich | 3:bac13ce5f5d0 | 139 | return 0; |
bayagich | 2:b5031bb5303e | 140 | } |
mmpeter | 0:9ab1097149ca | 141 |