Diff: main.cpp

Revision:

14:669f2f1566b0

Parent:

13:aaac0105a486

Child:

15:407e4152cf3a

--- a/main.cpp	Mon Mar 18 02:28:12 2013 +0000
+++ b/main.cpp	Mon Mar 18 04:21:05 2013 +0000
@@ -1,6 +1,10 @@
 // Robot Control Code
 // Tom Elliott and Ian Colwell
 
+/* TODO on a sunny day
+- reduce the amount of global variables
+- try increasing bluetooth buad rate
+*/
 
 #include "mbed.h"
 #include "rtos.h"
@@ -58,6 +62,10 @@
 float TSpeedL = 0;
 float TSpeedR = 0;
 float Turn = 0.01;
+float aeW = 0;
+float eW;
+float uW;
+
 Mutex Var_Lock;
 
 // global variables to eventually be removed
@@ -120,6 +128,7 @@
     
     InitializeSystem();
     BtS.printf("\r\n --- Robot Initialization Complete --- \r\n");  
+    pc.printf("\r\n --- Robot Initialization Complete --- \r\n"); 
     DisplayMenu();
     
     //BtS.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
@@ -231,7 +240,10 @@
         if (action == 2)
         {
             Var_Lock.lock();
-            BtS.printf("IR   F: %f R: %f \n\r", DF, DR);
+            BtS.printf("IR   F: %f cm R: %f cm \n\r", DF, DR);
+            BtS.printf("Wall Error: %f \n\r", eW);
+            BtS.printf("Acc Error: %f \n\r", aeW);
+            BtS.printf("Diff. Setpoint: %f \n\r\n", uW);
             Var_Lock.unlock();
             Thread::wait(1500);
         }
@@ -258,12 +270,6 @@
           
 
 /*
-    InitializeSystem();
-    BTInit();
-    BtS.printf("\r\n --- Robot Initialization Complete --- \r\n");    
-    
-    BtS.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
-    char x;
     while(1)
     {
         Thread::wait(20);
@@ -494,6 +500,10 @@
 {
     BtS.printf("AT+BTCANCEL\r\n");
     BtS.printf("AT+BTSCAN\r\n");
+    
+    // also send to the pc so we know whats going on
+    pc.printf("AT+BTCANCEL\r\n");
+    pc.printf("AT+BTSCAN\r\n");    
 }
 
 // --- Other Functions
@@ -615,6 +625,41 @@
     float aR = 22.6021; // 34.2456
     float bR = -0.0376; // -0.0569
     float Nominal = 25;
+    float Kpos = 0.5;
+    float Kor = 0.5;
+    float KpW = 0.1;
+    float KiW = 0.4;
+    
+    // Read Sensors
+    IRF = 3.3*IRFront.read();
+    IRR = 3.3*IRRear.read();
+
+    // Calculate distance based on voltage
+    DF = aF/(IRF+bF);
+    DR = aR/(IRR+bR);
+    
+    // calculate errors
+    eW = Kpos*(Nominal - (DF + DR)/2) + Kor*(DR - DF);
+    
+    // accumulate error
+    aeW = aeW + eW;
+    
+    uW = KpW*eW + KiW*aeW;
+    
+    // set differential speeds
+    userSetL = TSpeedL + uW;
+    userSetR = TSpeedR - uW;   
+}
+
+/*
+void IRChecker()
+{
+    float IRF, IRR;
+    float aF = 22.6321; // 34.2911
+    float bF = -0.1721; // -0.2608
+    float aR = 22.6021; // 34.2456
+    float bR = -0.0376; // -0.0569
+    float Nominal = 25;
     
     // Read Sensors
     IRF = 3.3*IRFront.read();
@@ -659,6 +704,7 @@
         }
     } 
 }
+*/
 
 void DisplayMenu()
 {