Revision 40:44d3dea4adcc, committed 2013-04-11

Comitter:

rsavitski

Date:

Thu Apr 11 17:23:07 2013 +0000

Parent:

39:c9058a401410

Child:

41:fefdbb9b5968

Commit message:

moved waypoint functionality from ai to motion layer

Changed in this revision

--- a/Processes/AI/ai.cpp	Wed Apr 10 22:30:09 2013 +0000
+++ b/Processes/AI/ai.cpp	Thu Apr 11 17:23:07 2013 +0000
@@ -3,16 +3,10 @@
 namespace AI
 {
 
-Mutex waypoint_flag_mutex;
-
-Waypoint* current_waypoint = NULL; //global scope
-
-bool waypoint_reached = false; // is current waypoint reached
-
 void ailayer(void const *dummy)
 {
-    current_waypoint = new Waypoint[5];
-
+    Waypoint *current_waypoint = new Waypoint[5];
+/*
     current_waypoint[0].x = 1;
     current_waypoint[0].y = 1;
     current_waypoint[0].theta = 0.0;
@@ -26,7 +20,21 @@
     current_waypoint[1].angle_threshold = 0.05*PI;
 
     current_waypoint[2].x = -999;
+*/
 
+    current_waypoint[0].x = 0.5;
+    current_waypoint[0].y = 1.85;
+    current_waypoint[0].theta = 0.0;
+    current_waypoint[0].pos_threshold = 0.05;
+    current_waypoint[0].angle_threshold = 0.05*PI;
+
+    current_waypoint[1].x = 1.2;
+    current_waypoint[1].y = 0.18;
+    current_waypoint[1].theta = 0;
+    current_waypoint[1].pos_threshold = 0.01;
+    current_waypoint[1].angle_threshold = 0.00001;
+
+    current_waypoint[2].x = -999;
 /*
     //TODO: temp current waypoint hack
     current_waypoint = new Waypoint;
@@ -43,35 +51,24 @@
     secondwp->pos_threshold = 0.01;
     secondwp->angle_threshold = 0.00001;
 */    
-    
+    motion::setNewWaypoint(current_waypoint);
     while(1)
     {
         Thread::wait(50);
         
-        waypoint_flag_mutex.lock();
-        if (checkWaypointStatus())
+        motion::waypoint_flag_mutex.lock();
+        if (motion::checkWaypointStatus())
         {
-            clearWaypointReached();
-            if ((current_waypoint+1)->x != -999) 
+            
+            if ((current_waypoint+1)->x != -999)
+            { 
+                motion::clearWaypointReached();
                 current_waypoint++;
+                motion::setNewWaypoint(current_waypoint);
+            }
         }
-        waypoint_flag_mutex.unlock();
+        motion::waypoint_flag_mutex.unlock();
     }
 }
 
-void setWaypointReached()
-{
-    waypoint_reached = true;
-}
-
-void clearWaypointReached()
-{
-    waypoint_reached = false;
-}
-
-bool checkWaypointStatus()
-{
-    return waypoint_reached;
-}
-
 } //namespace
\ No newline at end of file

--- a/Processes/AI/ai.h	Wed Apr 10 22:30:09 2013 +0000
+++ b/Processes/AI/ai.h	Thu Apr 11 17:23:07 2013 +0000
@@ -3,20 +3,13 @@
 
 #include "rtos.h"
 #include "globals.h"
+#include "motion.h"
 
 namespace AI
 {
 
 void ailayer(void const *dummy);
 
-void setWaypointReached();
-void clearWaypointReached();
-bool checkWaypointStatus();
-
-
-extern Waypoint *current_waypoint; 
-extern Mutex waypoint_flag_mutex;
-    
 }
 
 #endif
\ No newline at end of file

--- a/Processes/Motion/motion.cpp	Wed Apr 10 22:30:09 2013 +0000
+++ b/Processes/Motion/motion.cpp	Thu Apr 11 17:23:07 2013 +0000
@@ -10,10 +10,17 @@
 namespace motion
 {
 
+Mutex waypoint_flag_mutex;
+
+Waypoint *current_waypoint = NULL; 
+
+bool waypoint_reached = false; // is current waypoint reached
+
+
 void motionlayer(void const *dummy)
 {    
-    // get target waypoint from AI
-    Waypoint target_waypoint = *AI::current_waypoint;
+    // save target waypoint
+    Waypoint target_waypoint = *current_waypoint;
     
     // get current state from Kalman
     State current_state = Kalman::getState();
@@ -31,26 +38,28 @@
     bool a_reached = false;
     
     // is the waypoint reached
-    if (distance_err < target_waypoint.pos_threshold)
+    waypoint_flag_mutex.lock();
+    if (distance_err < ((checkWaypointStatus()) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold))
     {
         d_reached = true;
         distance_err = 0;
         
-        angle_err = 0.2*constrainAngle(target_waypoint.theta - current_state.theta);
+        angle_err = 0.5*constrainAngle(target_waypoint.theta - current_state.theta);
         
         if (abs(angle_err) < target_waypoint.angle_threshold)
         {
             a_reached = true;
-            angle_err = 0;
+            //angle_err = 0;
         }
     } 
+    waypoint_flag_mutex.unlock();
         
-    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
+    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 (d_reached && a_reached)
     {
-        AI::setWaypointReached();
+        setWaypointReached();
     }
-    AI::waypoint_flag_mutex.unlock();
+    waypoint_flag_mutex.unlock();
     
     // angular velocity controller
     const float p_gain_av = 0.5; //TODO: tune
@@ -92,4 +101,24 @@
     MotorControl::set_omegacmd(angular_v);
 }
 
+void setNewWaypoint(Waypoint *new_wp)
+{
+    current_waypoint = new_wp;
+}
+
+void setWaypointReached()
+{
+    waypoint_reached = true;
+}
+
+void clearWaypointReached()
+{
+    waypoint_reached = false;
+}
+
+bool checkWaypointStatus()
+{
+    return waypoint_reached;
+}
+
 } //namespace
\ No newline at end of file

--- a/Processes/Motion/motion.h	Wed Apr 10 22:30:09 2013 +0000
+++ b/Processes/Motion/motion.h	Thu Apr 11 17:23:07 2013 +0000
@@ -7,13 +7,20 @@
 #include "Kalman.h"
 #include "MotorControl.h"
 #include "supportfuncs.h"
-#include "ai.h"
 
 namespace motion
 {
 
 void motionlayer(void const *dummy);
 
+// can encapsulate mutex fully in motion with something like: bool set_new_wp_if_current_reached()
+void setNewWaypoint(Waypoint *new_wp);
+void setWaypointReached();
+void clearWaypointReached();
+bool checkWaypointStatus();
+
+extern Mutex waypoint_flag_mutex;
+
 }
 
 #endif //EUROBOT_PROCESSES_MOTION_MOTION_H_
\ No newline at end of file