Diff: Processes/Motion/motion.cpp

Revision:

39:44d3dea4adcc

Parent:

38:c9058a401410

Child:

40:fefdbb9b5968

--- 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