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@1:4f52a001926a, 2014-05-22 (annotated)

Committer:
mmpeter
Date:
Thu May 22 14:52:13 2014 +0000
Revision:
1:4f52a001926a
Parent:
0:9ab1097149ca
Child:
2:b5031bb5303e
IR+turning+turnaround

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;
mmpeter 0:9ab1097149ca 11
mmpeter 0:9ab1097149ca 12 m3pi thinggy;
mmpeter 0:9ab1097149ca 13
mmpeter 0:9ab1097149ca 14 int main() {
mmpeter 0:9ab1097149ca 15
mmpeter 0:9ab1097149ca 16 float speed = 0.25;
mmpeter 0:9ab1097149ca 17 float turn_speed = 0.2;
mmpeter 0:9ab1097149ca 18 float correction;
mmpeter 0:9ab1097149ca 19 float k = -0.3;
mmpeter 0:9ab1097149ca 20 int sensor[5];
mmpeter 0:9ab1097149ca 21 int returned;
mmpeter 0:9ab1097149ca 22 thinggy.locate(0,1);
mmpeter 0:9ab1097149ca 23 thinggy.printf("Villan");
mmpeter 0:9ab1097149ca 24 thinggy.locate(0,0);
mmpeter 0:9ab1097149ca 25 thinggy.printf("Pimpin");
mmpeter 0:9ab1097149ca 26
mmpeter 0:9ab1097149ca 27 wait(1.0);
mmpeter 0:9ab1097149ca 28
mmpeter 0:9ab1097149ca 29 thinggy.sensor_auto_calibrate();
mmpeter 0:9ab1097149ca 30
mmpeter 0:9ab1097149ca 31 thinggy.calibrated_sensor(sensor);
mmpeter 0:9ab1097149ca 32
mmpeter 0:9ab1097149ca 33 //find the average of the three sensors
mmpeter 0:9ab1097149ca 34 returned = (sensor[1] + sensor[2] + sensor[3])/3;
mmpeter 0:9ab1097149ca 35
mmpeter 0:9ab1097149ca 36 while(1) {
mmpeter 0:9ab1097149ca 37
mmpeter 0:9ab1097149ca 38 //check if it needs to turn
mmpeter 0:9ab1097149ca 39 while(returned <= 240){
mmpeter 0:9ab1097149ca 40 //turns right
mmpeter 0:9ab1097149ca 41 while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
mmpeter 0:9ab1097149ca 42 thinggy.left_motor(turn_speed);
mmpeter 0:9ab1097149ca 43 thinggy.right_motor(-turn_speed);
mmpeter 0:9ab1097149ca 44 thinggy.calibrated_sensor(sensor);
mmpeter 0:9ab1097149ca 45 }
mmpeter 0:9ab1097149ca 46 //turns left
mmpeter 0:9ab1097149ca 47 while(sensor[4] > sensor[0] && thinggy.line_position() != 0){
mmpeter 0:9ab1097149ca 48 thinggy.left_motor(-turn_speed);
mmpeter 0:9ab1097149ca 49 thinggy.right_motor(turn_speed);
mmpeter 0:9ab1097149ca 50 thinggy.calibrated_sensor(sensor);
mmpeter 0:9ab1097149ca 51 }
mmpeter 0:9ab1097149ca 52 thinggy.calibrated_sensor(sensor);
mmpeter 1:4f52a001926a 53 returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
mmpeter 0:9ab1097149ca 54 }//while returned <= 220
mmpeter 0:9ab1097149ca 55
mmpeter 0:9ab1097149ca 56 // Curves and straightaways
mmpeter 0:9ab1097149ca 57 while(returned > 240){
mmpeter 0:9ab1097149ca 58 float position = thinggy.line_position();
mmpeter 0:9ab1097149ca 59 correction = k*(position);
mmpeter 0:9ab1097149ca 60
mmpeter 0:9ab1097149ca 61 // -1.0 is far left, 1.0 is far right, 0.0 in the middle
mmpeter 0:9ab1097149ca 62
mmpeter 0:9ab1097149ca 63 //speed limiting for right motor
mmpeter 0:9ab1097149ca 64 if(speed + correction > 1){
mmpeter 0:9ab1097149ca 65 thinggy.right_motor(0.6);
mmpeter 0:9ab1097149ca 66 thinggy.left_motor(speed-correction);
mmpeter 0:9ab1097149ca 67 }
mmpeter 0:9ab1097149ca 68
mmpeter 0:9ab1097149ca 69 //speed limiting for left motor
mmpeter 0:9ab1097149ca 70 if(speed - correction > 1){
mmpeter 0:9ab1097149ca 71 thinggy.left_motor(0.6);
mmpeter 0:9ab1097149ca 72 thinggy.right_motor(speed+correction);
mmpeter 0:9ab1097149ca 73 }
mmpeter 0:9ab1097149ca 74 else{
mmpeter 0:9ab1097149ca 75 thinggy.left_motor(speed-correction);
mmpeter 0:9ab1097149ca 76 thinggy.right_motor(speed+correction);
mmpeter 0:9ab1097149ca 77
mmpeter 0:9ab1097149ca 78 //Infared: stop if obstructed
mmpeter 0:9ab1097149ca 79 m3pi_IN[0].mode(PullUp);
mmpeter 0:9ab1097149ca 80 while (m3pi_IN[0]==0){
mmpeter 0:9ab1097149ca 81 thinggy.stop();
mmpeter 0:9ab1097149ca 82 }
mmpeter 0:9ab1097149ca 83 }
mmpeter 0:9ab1097149ca 84 thinggy.calibrated_sensor(sensor);
mmpeter 1:4f52a001926a 85 returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
mmpeter 0:9ab1097149ca 86 }//while returned > 220
mmpeter 0:9ab1097149ca 87
mmpeter 0:9ab1097149ca 88 }//while(1)
mmpeter 0:9ab1097149ca 89 }
mmpeter 0:9ab1097149ca 90
mmpeter 0:9ab1097149ca 91