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@2:b5031bb5303e, 2014-05-26 (annotated)

Committer:
bayagich
Date:
Mon May 26 07:40:29 2014 +0000
Revision:
2:b5031bb5303e
Parent:
1:4f52a001926a
Child:
3:bac13ce5f5d0
Cross Detection - not working yet

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 2:b5031bb5303e 12 void 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;
mmpeter 0:9ab1097149ca 26 thinggy.locate(0,1);
mmpeter 0:9ab1097149ca 27 thinggy.printf("Villan");
mmpeter 0:9ab1097149ca 28 thinggy.locate(0,0);
mmpeter 0:9ab1097149ca 29 thinggy.printf("Pimpin");
mmpeter 0:9ab1097149ca 30
mmpeter 0:9ab1097149ca 31 wait(1.0);
mmpeter 0:9ab1097149ca 32
mmpeter 0:9ab1097149ca 33 thinggy.sensor_auto_calibrate();
mmpeter 0:9ab1097149ca 34
mmpeter 0:9ab1097149ca 35 thinggy.calibrated_sensor(sensor);
mmpeter 0:9ab1097149ca 36
mmpeter 0:9ab1097149ca 37 //find the average of the three sensors
mmpeter 0:9ab1097149ca 38 returned = (sensor[1] + sensor[2] + sensor[3])/3;
mmpeter 0:9ab1097149ca 39
mmpeter 0:9ab1097149ca 40 while(1) {
mmpeter 0:9ab1097149ca 41
bayagich 2:b5031bb5303e 42 cross_detection(sensor, black_thresh, white_thresh);
bayagich 2:b5031bb5303e 43
mmpeter 0:9ab1097149ca 44 //check if it needs to turn
mmpeter 0:9ab1097149ca 45 while(returned <= 240){
bayagich 2:b5031bb5303e 46 cross_detection(sensor, black_thresh, white_thresh);
mmpeter 0:9ab1097149ca 47 //turns right
mmpeter 0:9ab1097149ca 48 while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
mmpeter 0:9ab1097149ca 49 thinggy.left_motor(turn_speed);
mmpeter 0:9ab1097149ca 50 thinggy.right_motor(-turn_speed);
mmpeter 0:9ab1097149ca 51 thinggy.calibrated_sensor(sensor);
mmpeter 0:9ab1097149ca 52 }
mmpeter 0:9ab1097149ca 53 //turns left
mmpeter 0:9ab1097149ca 54 while(sensor[4] > sensor[0] && thinggy.line_position() != 0){
mmpeter 0:9ab1097149ca 55 thinggy.left_motor(-turn_speed);
mmpeter 0:9ab1097149ca 56 thinggy.right_motor(turn_speed);
mmpeter 0:9ab1097149ca 57 thinggy.calibrated_sensor(sensor);
mmpeter 0:9ab1097149ca 58 }
mmpeter 0:9ab1097149ca 59 thinggy.calibrated_sensor(sensor);
mmpeter 1:4f52a001926a 60 returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
mmpeter 0:9ab1097149ca 61 }//while returned <= 220
mmpeter 0:9ab1097149ca 62
mmpeter 0:9ab1097149ca 63 // Curves and straightaways
mmpeter 0:9ab1097149ca 64 while(returned > 240){
bayagich 2:b5031bb5303e 65
bayagich 2:b5031bb5303e 66 cross_detection(sensor, black_thresh, white_thresh);
bayagich 2:b5031bb5303e 67
mmpeter 0:9ab1097149ca 68 float position = thinggy.line_position();
mmpeter 0:9ab1097149ca 69 correction = k*(position);
mmpeter 0:9ab1097149ca 70
mmpeter 0:9ab1097149ca 71 // -1.0 is far left, 1.0 is far right, 0.0 in the middle
mmpeter 0:9ab1097149ca 72
mmpeter 0:9ab1097149ca 73 //speed limiting for right motor
mmpeter 0:9ab1097149ca 74 if(speed + correction > 1){
mmpeter 0:9ab1097149ca 75 thinggy.right_motor(0.6);
mmpeter 0:9ab1097149ca 76 thinggy.left_motor(speed-correction);
mmpeter 0:9ab1097149ca 77 }
mmpeter 0:9ab1097149ca 78
mmpeter 0:9ab1097149ca 79 //speed limiting for left motor
mmpeter 0:9ab1097149ca 80 if(speed - correction > 1){
mmpeter 0:9ab1097149ca 81 thinggy.left_motor(0.6);
mmpeter 0:9ab1097149ca 82 thinggy.right_motor(speed+correction);
mmpeter 0:9ab1097149ca 83 }
mmpeter 0:9ab1097149ca 84 else{
mmpeter 0:9ab1097149ca 85 thinggy.left_motor(speed-correction);
mmpeter 0:9ab1097149ca 86 thinggy.right_motor(speed+correction);
mmpeter 0:9ab1097149ca 87
mmpeter 0:9ab1097149ca 88 //Infared: stop if obstructed
mmpeter 0:9ab1097149ca 89 m3pi_IN[0].mode(PullUp);
mmpeter 0:9ab1097149ca 90 while (m3pi_IN[0]==0){
mmpeter 0:9ab1097149ca 91 thinggy.stop();
mmpeter 0:9ab1097149ca 92 }
mmpeter 0:9ab1097149ca 93 }
mmpeter 0:9ab1097149ca 94 thinggy.calibrated_sensor(sensor);
mmpeter 1:4f52a001926a 95 returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
mmpeter 0:9ab1097149ca 96 }//while returned > 220
mmpeter 0:9ab1097149ca 97
bayagich 2:b5031bb5303e 98 thinggy.calibrated_sensor(sensor);
bayagich 2:b5031bb5303e 99 returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
mmpeter 0:9ab1097149ca 100 }//while(1)
bayagich 2:b5031bb5303e 101
bayagich 2:b5031bb5303e 102
mmpeter 0:9ab1097149ca 103 }
mmpeter 0:9ab1097149ca 104
bayagich 2:b5031bb5303e 105 //REQUIRES: array of 5 ints
bayagich 2:b5031bb5303e 106 //EFFECTS: stops the robot if it comes to any interesection where a decision has to be made
bayagich 2:b5031bb5303e 107 void cross_detection(int sensor[5], int black_thresh, int white_thresh){
bayagich 2:b5031bb5303e 108 //three directions to choose from NOT WORKING
bayagich 2:b5031bb5303e 109 if(sensor[0] > black_thresh and sensor[1] < white_thresh and sensor[2] > black_thresh and sensor[3] < white_thresh and sensor[4] > black_thresh){
bayagich 2:b5031bb5303e 110 thinggy.stop();
bayagich 2:b5031bb5303e 111 wait(300);
bayagich 2:b5031bb5303e 112 }
bayagich 2:b5031bb5303e 113 //left or forward
bayagich 2:b5031bb5303e 114 else if(sensor[0] > black_thresh and sensor[1] < white_thresh and sensor[2] > black_thresh and sensor[3] < white_thresh and sensor[4] < white_thresh){
bayagich 2:b5031bb5303e 115 thinggy.stop();
bayagich 2:b5031bb5303e 116 wait(300);
bayagich 2:b5031bb5303e 117 }
bayagich 2:b5031bb5303e 118 //left or right WORKING
bayagich 2:b5031bb5303e 119 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 120 thinggy.stop();
bayagich 2:b5031bb5303e 121 wait(300);
bayagich 2:b5031bb5303e 122 }
bayagich 2:b5031bb5303e 123 //forward or right
bayagich 2:b5031bb5303e 124 else if (sensor[0] < white_thresh and sensor[1] < white_thresh and sensor[2] > black_thresh and sensor[3] < white_thresh and sensor[4] > black_thresh){
bayagich 2:b5031bb5303e 125 thinggy.stop();
bayagich 2:b5031bb5303e 126 wait(300);
bayagich 2:b5031bb5303e 127 }
bayagich 2:b5031bb5303e 128 }
mmpeter 0:9ab1097149ca 129