Ian Colwell / Mbed 2 deprecated RoboticsProject

New Robotics Code

Dependencies:   mbed-rtos mbed

Fork of Project by Thomas Elliott

Welcome to the robot wiki.

Files at this revision

API Documentation at this revision

Revision 16:cf9519c35510, committed 2013-03-19

Comitter:
LtBarbershop
Date:
Tue Mar 19 23:16:21 2013 +0000
Parent:
15:407e4152cf3a
Child:
17:bc13550f673b
Commit message:
March 18, 2012

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Mon Mar 18 14:38:40 2013 +0000
+++ b/main.cpp	Tue Mar 19 23:16:21 2013 +0000
@@ -243,9 +243,10 @@
             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);
+            BtS.printf("Diff. Setpoint: %f \n\r", uW);
+            BtS.printf("Setpoint L: %f R: %f \n\n\r", userSetL, userSetR);
             Var_Lock.unlock();
-            Thread::wait(1500);
+            Thread::wait(1000);
         }
         
         if (action == 3)
@@ -619,14 +620,14 @@
 
 void IRChecker()
 {
-    float IRF, IRR;
+    float IRF, IRR, prevDF, prevDR;
     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;
-    float Kpos = 0.5;
-    float Kor = 0.5;
+    float Kpos = 0.001;
+    float Kor = 0.001;
     float KpW = 0.1;
     float KiW = 0.4;
     
@@ -635,9 +636,19 @@
     IRR = 3.3*IRRear.read();
 
     // Calculate distance based on voltage
+    prevDF = DF;
+    prevDR = DR;
     DF = aF/(IRF+bF);
     DR = aR/(IRR+bR);
     
+    // check for invalid data
+    if ((DR - prevDR) > 5 || (prevDR - DR) > 5)
+    {
+        DR = prevDR 
+    
+    
+    
+    
     //
     
     // calculate errors
@@ -648,13 +659,13 @@
     
     uW = KpW*eW + KiW*aeW;
     
-    if (uW > 5)
+    if (uW > 0.02)
     {
-        uW = 5;
+        uW = 0.02;
     }
-    else if (uW < -5)
+    else if (uW < -0.02)
     {
-        uW = -5;
+        uW = -0.02;
     }
     
     // set differential speeds