Diff: main.cpp

Revision:

0:6b067dcbab9d

Child:

1:48163ae9fe4d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jun 05 09:16:52 2014 +0000
@@ -0,0 +1,181 @@
+#include "mbed.h"
+#include "m3pi_ng.h"
+#include "cmath"
+#include "iostream"
+#include <string>
+
+m3pi thinggy;
+
+void turn_right();
+int darkest=0;
+int dark;
+int previousdark;
+int v1=0,v2=0,v3=0,v4=0,v5=0,v6=0;
+bool found;
+int x;
+
+
+using namespace std;
+int sensor[5];
+
+int main() { 
+thinggy.calibrated_sensor(sensor);
+while(1){
+      //Initial sensor readings
+      while(found!=1){
+          v1=sensor[2];
+          thinggy.stop();
+          wait(0.5);
+          thinggy.cls();
+          thinggy.printf("Got 1");
+          turn_right();
+          //Turn 60 degrees from north
+          v2=sensor[2];
+          thinggy.stop();
+          wait(0.5);
+          thinggy.cls();
+          thinggy.printf("Got 2");
+          turn_right();
+          //120 degrees from north
+          v3=sensor[2];
+          thinggy.stop();
+          wait(0.5);
+          thinggy.cls();
+          thinggy.printf("Got 3");
+          turn_right();
+          //180 degrees from north
+          v4=sensor[2];
+          thinggy.stop();
+          wait(0.5);
+          thinggy.cls();
+          thinggy.printf("Got 4");
+          turn_right();
+          //240 degrees from north
+          v5=sensor[2];
+          thinggy.stop();
+          wait(0.5);
+          thinggy.cls();
+          thinggy.printf("Got 5");
+          turn_right();
+          //300 degrees from north
+          v6=sensor[2];
+          thinggy.stop();
+          wait(0.5);
+          thinggy.cls();
+          thinggy.printf("Got 6");
+          turn_right();
+          
+    //Determine which direction is darkest
+          //North is darkest
+          if(v1>v2 and v1>v3 and v1>v4 and v1>v5 and v1>v6){
+              x=1;
+              dark=v1;
+              thinggy.cls();
+              thinggy.printf("It's 1");
+              } 
+          //60 degrees from north is darkest
+          else if(v2>v1 and v2>v3 and v2>v4 and v2>v5 and v2>v6){
+             x=2;
+             dark=v2;
+             thinggy.cls();
+             thinggy.printf("It's 2");
+              } 
+          //120 degrees from north is darkest
+          else if(v3>v1 and v3>v2 and v3>v4 and v3>v5 and v3>v6){
+             x=3;
+             dark=v3;
+             thinggy.cls();
+             thinggy.printf("It's 3");
+              } 
+          //180 degrees from north is darkest
+          else if(v4>v1 and v4>v2 and v4>v3 and v4>v5 and v4>v6){
+             x=4;
+             dark=v4;
+             thinggy.cls();
+             thinggy.printf("It's 4");
+              } 
+          //240 degrees from north is darkest
+          else if(v5>v1 and v5>v2 and v5>v3 and v5>v4 and v5>v6){
+             x=5;
+             dark=v5;
+             thinggy.cls();
+             thinggy.printf("It's 5");
+              }
+          //300 degrees from north is darkest
+          else if(v6>v1 and v6>v2 and v6>v3 and v6>v4 and v6>v5){
+             x=6;
+             dark=v6;
+             thinggy.cls();
+             thinggy.printf("It's 6");
+              }
+          
+          switch(x){
+              case 1:
+                //Go forward
+                thinggy.forward(0.5);
+                break;
+              case 2:
+                //Go 60 degrees from north
+                turn_right();
+                thinggy.forward(0.5);
+                break;
+              case 3:
+                //Go 120 degrees from north
+                turn_right();
+                turn_right();
+                thinggy.forward(0.5);
+                break;
+              case 4:
+                //Go 180 degrees from north
+                turn_right();
+                turn_right();
+                turn_right();
+                thinggy.forward(0.5);
+                break;
+              case 5:
+                //Go 240 degrees from north
+                turn_right();
+                turn_right();
+                turn_right();
+                turn_right();
+                thinggy.forward(0.5);
+                break;
+              case 6:
+                //Go 300 degrees from north
+                turn_right();
+                turn_right();
+                turn_right();
+                turn_right();
+                turn_right();
+                thinggy.forward(0.5);
+                break;
+                }
+                
+            if(dark>darkest){
+              darkest=dark;
+              }  
+            //Condition for exiting the loop: darkest spot found  
+            if(previousdark>dark){
+                //Go 180 degrees
+                found=1;
+                thinggy.printf("Got it!");
+                }
+            dark=previousdark; 
+            
+          
+      
+      }
+          }
+          }
+          
+//EFFECTS: turns the robot right 
+void turn_right(){
+    int n=0;
+    wait(.08); 
+    while (n<467){
+        thinggy.left_motor(0.2);
+        thinggy.right_motor(-0.2);
+        n++;
+        }
+    wait(0.15);      
+}
\ No newline at end of file