This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Diff: Processes/Motion/motion.cpp
- Revision:
- 30:791739422122
- Parent:
- 25:b16f1045108f
- Child:
- 32:e3f633620816
--- a/Processes/Motion/motion.cpp Wed Apr 10 02:01:51 2013 +0000 +++ b/Processes/Motion/motion.cpp Wed Apr 10 18:03:32 2013 +0000 @@ -6,16 +6,12 @@ //////////////////////////////////////////////////////////////////////////////// #include "motion.h" -#include "supportfuncs.h" namespace motion { void motionlayer(void const *dummy) -{ - //TODO: current_waypoint global in AI layer - //TODO: threshold for deciding that the waypoint has been achieved - +{ // get target waypoint from AI Waypoint target_waypoint = *AI::current_waypoint; @@ -31,6 +27,23 @@ float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta); + // is the waypoint reached + if (distance_err < target_waypoint.pos_threshold) + { + distance_err = 0; + } + if (abs(angle_err) < 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) + { + AI::setWaypointReached(); + return; + } + AI::waypoint_flag_mutex.unlock(); // angular velocity controller const float p_gain_av = 0.3; //TODO: tune @@ -67,7 +80,7 @@ //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v); - //TODO: put into motor control + // pass values to the motor control MotorControl::set_fwdcmd(forward_v); MotorControl::set_omegacmd(angular_v); }