Diff: main.cpp

Revision:

4:057e904b1395

Parent:

3:bac13ce5f5d0

--- a/main.cpp	Mon May 26 09:34:16 2014 +0000
+++ b/main.cpp	Tue Jun 03 08:03:58 2014 +0000
@@ -2,6 +2,7 @@
 #include "m3pi_ng.h"
 #include "cmath"
 #include "iostream"
+#include <string>
 
 //Access infared sensors
 DigitalIn m3pi_IN[] = {(p12)};
@@ -10,19 +11,34 @@
 using namespace std;
 
 bool cross_detection(int sensor[5], int black_thresh, int white_thresh); 
+void mapping(m3pi thinggy, string directions, float speed, int turns);
+string directions(int startpoint, int endpoint);
+int end(); 
+int start(); 
+
  
 m3pi thinggy; 
- 
-int main() {
-    
+
+    int black_thresh = 300;
+    int white_thresh = 240;
     float speed = 0.25;
     float turn_speed = 0.2; 
     float correction;
     float k = -0.3;
     int sensor[5];
-    int returned;   
-    int black_thresh = 300;
-    int white_thresh = 240;  
+    int returned;  
+
+ 
+int main() {
+    
+    //float speed = 0.25;
+    //float turn_speed = 0.2; 
+    //float correction;
+    //float k = -0.3;
+    //int sensor[5];
+    //int returned;   
+    //int black_thresh = 300;
+    //int white_thresh = 240;  
     bool cross = 0; 
     thinggy.locate(0,1);
     thinggy.printf("Villan");
@@ -37,52 +53,69 @@
     
     //find the average of the three sensors 
     returned = (sensor[1] + sensor[2] + sensor[3])/3;
+    
+    //finds the directions of the robot 
+    int startpt = 2; //start();  
+    int endpt = 3; //end(); 
+    string d = directions(startpt, endpt);
+    int turns = 0; 
  
     while(1) {
-        //check if it needs to turn  
+        //check if it needs to turn 
         while(returned <= 240){
                 //turns right 
                 while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
-                    thinggy.left_motor(turn_speed);
-                    thinggy.right_motor(-turn_speed);
-                    thinggy.calibrated_sensor(sensor);
                     cross = cross_detection(sensor, black_thresh, white_thresh);
                     if(cross){
-                        //ask for command (left, right or forward)
+                        mapping(thinggy, d, speed, turns);
+                        ++turns; 
                     }
+                    else {
+                        thinggy.left_motor(turn_speed);
+                        thinggy.right_motor(-turn_speed);
+                    }
+                    
+                    thinggy.calibrated_sensor(sensor);
                 }
                 //turns left 
                 while(sensor[4] > sensor[0] && thinggy.line_position() != 0){
-                    thinggy.left_motor(-turn_speed);
-                    thinggy.right_motor(turn_speed);
-                    thinggy.calibrated_sensor(sensor);
                     cross = cross_detection(sensor, black_thresh, white_thresh);
                     if(cross){
-                        //ask for command (left, right or forward)
+                        mapping(thinggy, d, speed, turns);
+                        ++turns; 
                     }
+                    else {
+                        thinggy.left_motor(-turn_speed);
+                        thinggy.right_motor(turn_speed);
+                    }
+                    
+                    thinggy.calibrated_sensor(sensor);
+                    
                 }
                 thinggy.calibrated_sensor(sensor);
                 returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
+                
         }//while returned <= 220
         
         // Curves and straightaways    
         while(returned > 240){
-            
-            cross_detection(sensor, black_thresh, white_thresh); 
-        
             float position = thinggy.line_position();
             correction = k*(position);
-       
+            cross = cross_detection(sensor, black_thresh, white_thresh);
+            if(cross){
+                mapping(thinggy, d, speed, turns);
+                ++turns; 
+            }
             // -1.0 is far left, 1.0 is far right, 0.0 in the middle
         
             //speed limiting for right motor
-            if(speed + correction > 1){
+            else if(speed + correction > 1){
                 thinggy.right_motor(0.6);
                 thinggy.left_motor(speed-correction);
             }    
             
             //speed limiting for left motor
-            if(speed - correction > 1){
+            else if(speed - correction > 1){
                 thinggy.left_motor(0.6);
                 thinggy.right_motor(speed+correction);
             }
@@ -94,7 +127,7 @@
                 m3pi_IN[0].mode(PullUp);
                 while (m3pi_IN[0]==0){
                     thinggy.stop();
-                    }
+                }
             }
             thinggy.calibrated_sensor(sensor);
             returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;   
@@ -106,36 +139,115 @@
     
 
 }
- 
+
+//DONE 
  //REQUIRES: array of 5 ints 
  //EFFECTS: stops the robot if it comes to any interesection where a decision has to be made
  // and returns true if there is a cross
  bool cross_detection(int sensor[5], int black_thresh, int white_thresh){
        //three directions to choose from NOT WORKING 
         if(sensor[0] > black_thresh and sensor[2] > black_thresh and sensor[4] > black_thresh){
-            thinggy.stop();
-            wait(300); 
             return 1;  
         }
         //left or forward 
         else if(sensor[0] > black_thresh and sensor[2] > black_thresh){
-            thinggy.stop(); 
-            wait(300); 
             return 1; 
         }
         //left or right WORKING
         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 ){
-            thinggy.stop();
-            wait(300); 
             return 1;  
         } 
         //forward or right
         else if (sensor[2] > black_thresh and sensor[4] > black_thresh){
-            thinggy.stop(); 
-            wait(300);
             return 1; 
         } 
         
         return 0; 
 }
- 
\ No newline at end of file
+
+//take in the starting point of the robot from bluetooth 
+int start(){
+ return 0;  
+}
+
+//take in the ending point of the robot from bluetooth 
+int end(){
+  return 0; 
+}
+
+//DONE
+//gives the string to use for the map 
+string directions(int startpoint, int endpoint){
+    string charmap[6][6] = {
+       {"", "RLLR", "RLFF", "RRRLLF", "RRLFLRRRL", "RRLR"}, 
+       {"LRRL", "", "LLF", "LFRLLF", "LFRLRRL", "LRFLR"},
+       {"FFRL", "FRR", "", "LL", "FLRLRRL", "FFFLR"},
+       {"FRRLRL", "RLRR", "RR", "", "FFRL", "FRLRL"},
+       {"RLLLRFRLL", "RLLRLFR", "RLFLR", "RLFF", "", "RLLLRL"},
+       {"LRLL", "LFLR", "LRFFF", "RLRLF", "RLRRRL", ""}
+    }; 
+    return charmap[startpoint - 1][endpoint - 1];
+}
+
+//DONE
+//takes in the string of directions and the number of turns 
+//completed and then tells it whether to do left, right, forward 
+void mapping(m3pi thinggy, string directions, float speed, int turns){
+    
+    char x = directions[turns]; //something in the string 
+    
+    //tells it which direction to go 
+    if(x == NULL){
+        thinggy.stop(); 
+        wait(300);
+    }
+    else if(x == 'L'){
+        thinggy.left(speed);
+        thinggy.calibrated_sensor(sensor);
+        returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
+        while(returned <= 240){
+                //turns right 
+                while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
+                    thinggy.left_motor(turn_speed);
+                    thinggy.right_motor(-turn_speed);
+                    thinggy.calibrated_sensor(sensor);
+                }
+                //turns left 
+                while(sensor[4] > sensor[0] && thinggy.line_position() != 0){
+                    thinggy.left_motor(-turn_speed);
+                    thinggy.right_motor(turn_speed);
+                    thinggy.calibrated_sensor(sensor);
+                    
+                }
+                thinggy.calibrated_sensor(sensor);
+                returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
+                
+        }//while returned <= 220 
+    }
+    else if(x == 'R'){
+        thinggy.calibrated_sensor(sensor);
+        returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
+        while(returned <= 240){
+                //turns right 
+                while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
+                    thinggy.left_motor(turn_speed);
+                    thinggy.right_motor(-turn_speed);
+                    thinggy.calibrated_sensor(sensor);
+                }
+                //turns left 
+                while(sensor[4] > sensor[0] && thinggy.line_position() != 0){
+                    thinggy.left_motor(-turn_speed);
+                    thinggy.right_motor(turn_speed);
+                    thinggy.calibrated_sensor(sensor);
+                    
+                }
+                thinggy.calibrated_sensor(sensor);
+                returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
+                
+        }//while returned <= 220 
+        thinggy.right(speed); 
+    }
+    else if(x == 'F'){
+        //do nothing 
+    }
+}
\ No newline at end of file