Colour sensors calibrated
Dependencies: mbed-rtos mbed Servo QEI
Fork of ICRSEurobot13 by
Diff: Processes/Motion/motion.cpp
- Revision:
- 33:e3f633620816
- Parent:
- 27:7cb3a21d9a2e
- Parent:
- 31:791739422122
- Child:
- 34:a49197572737
diff -r ada943ecaceb -r e3f633620816 Processes/Motion/motion.cpp --- a/Processes/Motion/motion.cpp Wed Apr 10 18:04:47 2013 +0000 +++ b/Processes/Motion/motion.cpp Wed Apr 10 18:25:16 2013 +0000 @@ -6,17 +6,12 @@ //////////////////////////////////////////////////////////////////////////////// #include "motion.h" -#include "supportfuncs.h" -#include "Kalman.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; @@ -33,6 +28,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 @@ -69,7 +81,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); }