Cooper Liu / Mbed 2 deprecated ICRSEurobot13

Colour sensors calibrated

Dependencies:   mbed-rtos mbed Servo QEI

Fork of ICRSEurobot13 by Thomas Branch

Files at this revision

API Documentation at this revision

Revision 41:fefdbb9b5968, committed 2013-04-11

Comitter:
rsavitski
Date:
Thu Apr 11 18:49:11 2013 +0000
Parent:
40:44d3dea4adcc
Child:
43:6504d85d85b4
Commit message:
hysteresis

Changed in this revision

Processes/Motion/motion.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/Processes/Motion/motion.cpp	Thu Apr 11 17:23:07 2013 +0000
+++ b/Processes/Motion/motion.cpp	Thu Apr 11 18:49:11 2013 +0000
@@ -15,7 +15,8 @@
 Waypoint *current_waypoint = NULL; 
 
 bool waypoint_reached = false; // is current waypoint reached
-
+bool d_reached = false;
+bool a_reached = false;
 
 void motionlayer(void const *dummy)
 {    
@@ -34,12 +35,9 @@
     
     float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta);
     
-    bool d_reached = false;
-    bool a_reached = false;
-    
     // is the waypoint reached
     waypoint_flag_mutex.lock();
-    if (distance_err < ((checkWaypointStatus()) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold))
+    if (distance_err < ((d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold))
     {
         d_reached = true;
         distance_err = 0;
@@ -114,6 +112,8 @@
 void clearWaypointReached()
 {
     waypoint_reached = false;
+    d_reached = false;
+    a_reached = false;
 }
 
 bool checkWaypointStatus()