Diff: main.cpp

Revision:

0:386c250325ce

Child:

1:9848e25989f1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 22 11:57:29 2014 +0000
@@ -0,0 +1,79 @@
+#include "mbed.h"
+#include "m3pi_ng.h"
+#include "cmath"
+#include "iostream"
+ 
+using namespace std;
+ 
+m3pi thinggy;
+ 
+ 
+int main() {
+    
+    float speed = 0.25;
+    float correction;
+    float k = -0.3;
+    int sensor[5];
+    int returned;   
+    thinggy.locate(0,1);
+    thinggy.printf("Villan");
+    thinggy.locate(0,0);
+    thinggy.printf("Pimpin");
+    
+    wait(1.0);
+ 
+    thinggy.sensor_auto_calibrate();
+ 
+    thinggy.calibrated_sensor(sensor);
+    returned = (sensor[1] + sensor[2] + sensor[3])/3;
+ 
+    
+    
+    while(1) {
+        // change "if" to while
+        while(returned < 220){
+                while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
+                    thinggy.left_motor(.2);
+                    thinggy.right_motor(-.2);
+                    thinggy.calibrated_sensor(sensor);
+                }
+                while(sensor[4] > sensor[0] && thinggy.line_position() != 0){
+                    thinggy.left_motor(-.2);
+                    thinggy.right_motor(.2);
+                    thinggy.calibrated_sensor(sensor);
+                }
+                thinggy.calibrated_sensor(sensor);
+                returned = (sensor[1] + sensor[2] + sensor[3])/3;
+        }
+        
+        // Curves and straightaways    
+        while(returned > 220){
+ 
+        float position = thinggy.line_position();
+        correction = k*(position);
+       
+         // -1.0 is far left, 1.0 is far right, 0.0 in the middle
+        
+        //speed limiting for right motor
+        if(speed + correction > 1){
+            thinggy.right_motor(0.6);
+            thinggy.left_motor(speed-correction);
+        } 
+        else if(speed - correction > 1){
+            thinggy.right_motor(speed+correction);
+            thinggy.left_motor(0.6);
+        }
+        else{
+            thinggy.left_motor(speed-correction);
+            thinggy.right_motor(speed+correction);
+        }
+         thinggy.calibrated_sensor(sensor);
+         returned = (sensor[1] + sensor[2] + sensor[3])/3;   
+        }
+    thinggy.calibrated_sensor(sensor);
+    returned = (sensor[1] + sensor[2] + sensor[3])/3;
+    }
+}
+ 
+ 
+ 
\ No newline at end of file