Diff: main.cpp

Revision:

0:1a1f77ee9d09

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 26 01:23:02 2014 +0000
@@ -0,0 +1,85 @@
+#include "mbed.h"
+#include "Motor.h"
+
+//Distance Sensors
+AnalogIn right(PTB0);
+AnalogIn left(PTB1);
+AnalogIn front(PTB2);
+
+Serial pc(USBTX, USBRX);
+
+//Motors
+Motor motorL(PTA12, PTD4);
+Motor motorR(PTA5, PTA4);
+
+/**
+*Avoid obstacle while accounting for
+*whether the robot is following the 
+*inside or outside.
+*/
+bool avoidObstacle(bool){
+    //pc.printf("Oh! No!\r");
+    return true;
+ }
+ 
+int main() {
+    
+    float lastP  = 0, thisP = 0, lspeed = 0, rspeed = 0;
+    float proportional, derivative, pd;
+    bool followOutside = 1;
+    const int baseSpeed  = 1;
+    const float setPoint = .5;
+    const float pgain = 5.0;
+    const float dgain = 3.0;
+    
+    
+    while(1){
+        
+        /*
+        //check for obstacle in front
+        if(front > .4){
+            followOutside = avoidObstacle(followOutside);
+        }
+        */
+        //choose which side sensor to use
+        if(followOutside){
+            thisP = right.read();
+        }
+        else{
+            thisP = left;
+        }
+        
+        
+        //Assign P and D
+        proportional = thisP - setPoint;
+        derivative = thisP - lastP;
+        
+        //Calculate Proportional Derivative
+        pd = pgain*proportional  + dgain*derivative;
+        
+        //Calculate Motor Speeds
+        if(followOutside){
+            lspeed = baseSpeed - pd;
+            rspeed = baseSpeed + pd;
+            if(thisP<=.2 || left>=.8)lspeed =1;
+            if(thisP>=.8)rspeed =1;
+        }
+        else{
+            lspeed = baseSpeed + pd;
+            rspeed = baseSpeed - pd;
+            if(thisP<=.2 || right>=.8)rspeed =1;
+            if(thisP>=.8)lspeed =1;
+        }
+        
+        //Assign Motor Speeds
+        motorL.speed(lspeed);
+        motorR.speed(rspeed);
+        
+        ///  DON'T MESS WITH THE MAGICAL PRINTF!!!!!!!
+        pc.printf("prop: %f    deriv: %f   sensor: %f\r",proportional, derivative, right.read());
+        if(abs(lastP-thisP)<.5){
+        lastP = thisP;
+        }
+    }
+    
+}
\ No newline at end of file