2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
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