Revision 36:34f4b38039bb, committed 2013-04-10

Comitter:

rsavitski

Date:

Wed Apr 10 20:09:54 2013 +0000

Parent:

34:e1678450feec

Parent:

35:f8e7f0a72a3d

Child:

37:6ecf0d21e492

Commit message:

merge new gains and motion fixup

Changed in this revision

--- a/Processes/AI/ai.cpp	Wed Apr 10 20:06:29 2013 +0000
+++ b/Processes/AI/ai.cpp	Wed Apr 10 20:09:54 2013 +0000
@@ -29,7 +29,7 @@
     
     while(1)
     {
-        Thread::wait(100);
+        Thread::wait(50);
         
         waypoint_flag_mutex.lock();
         if (checkWaypointStatus())

--- a/Processes/Kalman/Kalman.cpp	Wed Apr 10 20:06:29 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Wed Apr 10 20:09:54 2013 +0000
@@ -19,7 +19,8 @@
 
 Ticker predictticker;
 
-DigitalOut OLED4(LED4);
+//DigitalOut OLED4(LED4);
+DigitalOut OLED4(p10);
 DigitalOut OLED1(LED1);
 DigitalOut OLED2(LED2);
 

--- a/Processes/Motion/motion.cpp	Wed Apr 10 20:06:29 2013 +0000
+++ b/Processes/Motion/motion.cpp	Wed Apr 10 20:09:54 2013 +0000
@@ -6,7 +6,7 @@
 ////////////////////////////////////////////////////////////////////////////////
 
 #include "motion.h"
-
+DigitalOut OLED4(LED4);
 namespace motion
 {
 
@@ -31,18 +31,17 @@
     if (distance_err < target_waypoint.pos_threshold)
     {
         distance_err = 0;
-        angle_err = constrainAngle(target_waypoint.theta - current_state.theta);
         
-        if (abs(angle_err) < target_waypoint.angle_threshold)
+        if (abs(constrainAngle(target_waypoint.theta - current_state.theta)) < target_waypoint.angle_threshold)
         {
             angle_err = 0;
         }
-    }
-    
+    } 
         
     AI::waypoint_flag_mutex.lock(); // proper way would be to construct a function to evaluate the condition and pass the function pointer to a conditional setter function for reached flag
     if (distance_err == 0 && angle_err == 0)
     {
+        OLED4 = 1;
         AI::setWaypointReached();
         return;
     }