Amanda Bayagich / Mbed 2 deprecated Final_comments

AKUL check if this has the correct right and left turning, if not replace it Code with lots of comments

Dependencies:   m3pi_ng mbed

Fork of Working_on_Left_and_Right by der Roboter

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?

UserRevisionLine numberNew 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