Diff: main.cpp

Revision:

0:9ab1097149ca

Child:

1:4f52a001926a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 22 14:50:22 2014 +0000
@@ -0,0 +1,91 @@
+#include "mbed.h"
+#include "m3pi_ng.h"
+#include "cmath"
+#include "iostream"
+
+//Access infared sensors
+DigitalIn m3pi_IN[] = {(p12)};
+DigitalOut led_1(p13);
+
+using namespace std;
+ 
+m3pi thinggy; 
+ 
+int main() {
+    
+    float speed = 0.25;
+    float turn_speed = 0.2; 
+    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);
+    
+    //find the average of the three sensors 
+    returned = (sensor[1] + sensor[2] + sensor[3])/3;
+ 
+    while(1) {
+        
+        //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);
+                }
+                //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] + sensor[3])/3;
+        }//while returned <= 220
+        
+        // Curves and straightaways    
+        while(returned > 240){
+            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);
+            }    
+            
+            //speed limiting for left motor
+            if(speed - correction > 1){
+                thinggy.left_motor(0.6);
+                thinggy.right_motor(speed+correction);
+            }
+            else{
+                thinggy.left_motor(speed-correction);
+                thinggy.right_motor(speed+correction);
+                
+               //Infared: stop if obstructed           
+                m3pi_IN[0].mode(PullUp);
+                while (m3pi_IN[0]==0){
+                    thinggy.stop();
+                    }
+            }
+            thinggy.calibrated_sensor(sensor);
+            returned = (sensor[1] + sensor[2] + sensor[3])/3;   
+        }//while returned > 220
+        
+    }//while(1)
+}
+ 
+ 
\ No newline at end of file