2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Processes/Motion/motion.cpp@39:44d3dea4adcc, 2013-04-11 (annotated)
- Committer:
- rsavitski
- Date:
- Thu Apr 11 17:23:07 2013 +0000
- Revision:
- 39:44d3dea4adcc
- Parent:
- 38:c9058a401410
- Child:
- 40:fefdbb9b5968
moved waypoint functionality from ai to motion layer
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rsavitski | 24:50805ef8c499 | 1 | //////////////////////////////////////////////////////////////////////////////// |
rsavitski | 24:50805ef8c499 | 2 | // Motion control unit |
rsavitski | 24:50805ef8c499 | 3 | //////////////////////////////////////////////////////////////////////////////// |
rsavitski | 24:50805ef8c499 | 4 | // Takes current state of the robot and target waypoint, |
rsavitski | 24:50805ef8c499 | 5 | // calculates desired forward and angular velocities and requests those from the motor control layer. |
rsavitski | 24:50805ef8c499 | 6 | //////////////////////////////////////////////////////////////////////////////// |
rsavitski | 24:50805ef8c499 | 7 | |
rsavitski | 24:50805ef8c499 | 8 | #include "motion.h" |
rsavitski | 38:c9058a401410 | 9 | |
rsavitski | 24:50805ef8c499 | 10 | namespace motion |
rsavitski | 24:50805ef8c499 | 11 | { |
rsavitski | 24:50805ef8c499 | 12 | |
rsavitski | 39:44d3dea4adcc | 13 | Mutex waypoint_flag_mutex; |
rsavitski | 39:44d3dea4adcc | 14 | |
rsavitski | 39:44d3dea4adcc | 15 | Waypoint *current_waypoint = NULL; |
rsavitski | 39:44d3dea4adcc | 16 | |
rsavitski | 39:44d3dea4adcc | 17 | bool waypoint_reached = false; // is current waypoint reached |
rsavitski | 39:44d3dea4adcc | 18 | |
rsavitski | 39:44d3dea4adcc | 19 | |
rsavitski | 24:50805ef8c499 | 20 | void motionlayer(void const *dummy) |
rsavitski | 30:791739422122 | 21 | { |
rsavitski | 39:44d3dea4adcc | 22 | // save target waypoint |
rsavitski | 39:44d3dea4adcc | 23 | Waypoint target_waypoint = *current_waypoint; |
rsavitski | 24:50805ef8c499 | 24 | |
rsavitski | 24:50805ef8c499 | 25 | // get current state from Kalman |
rsavitski | 24:50805ef8c499 | 26 | State current_state = Kalman::getState(); |
rsavitski | 24:50805ef8c499 | 27 | |
rsavitski | 24:50805ef8c499 | 28 | float delta_x = target_waypoint.x - current_state.x; |
rsavitski | 24:50805ef8c499 | 29 | float delta_y = target_waypoint.y - current_state.y; |
rsavitski | 24:50805ef8c499 | 30 | |
madcowswe | 25:b16f1045108f | 31 | //printf("motion sees deltax: %f deltay %f\r\n", delta_x, delta_y); |
rsavitski | 24:50805ef8c499 | 32 | |
madcowswe | 25:b16f1045108f | 33 | float distance_err = hypot(delta_x, delta_y); |
madcowswe | 25:b16f1045108f | 34 | |
madcowswe | 25:b16f1045108f | 35 | float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta); |
rsavitski | 24:50805ef8c499 | 36 | |
rsavitski | 37:6ecf0d21e492 | 37 | bool d_reached = false; |
rsavitski | 37:6ecf0d21e492 | 38 | bool a_reached = false; |
rsavitski | 37:6ecf0d21e492 | 39 | |
rsavitski | 30:791739422122 | 40 | // is the waypoint reached |
rsavitski | 39:44d3dea4adcc | 41 | waypoint_flag_mutex.lock(); |
rsavitski | 39:44d3dea4adcc | 42 | if (distance_err < ((checkWaypointStatus()) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold)) |
rsavitski | 30:791739422122 | 43 | { |
rsavitski | 37:6ecf0d21e492 | 44 | d_reached = true; |
rsavitski | 30:791739422122 | 45 | distance_err = 0; |
rsavitski | 38:c9058a401410 | 46 | |
rsavitski | 39:44d3dea4adcc | 47 | angle_err = 0.5*constrainAngle(target_waypoint.theta - current_state.theta); |
rsavitski | 38:c9058a401410 | 48 | |
rsavitski | 38:c9058a401410 | 49 | if (abs(angle_err) < target_waypoint.angle_threshold) |
madcowswe | 33:a49197572737 | 50 | { |
rsavitski | 37:6ecf0d21e492 | 51 | a_reached = true; |
rsavitski | 39:44d3dea4adcc | 52 | //angle_err = 0; |
madcowswe | 33:a49197572737 | 53 | } |
rsavitski | 35:f8e7f0a72a3d | 54 | } |
rsavitski | 39:44d3dea4adcc | 55 | waypoint_flag_mutex.unlock(); |
rsavitski | 30:791739422122 | 56 | |
rsavitski | 39:44d3dea4adcc | 57 | 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 |
rsavitski | 37:6ecf0d21e492 | 58 | if (d_reached && a_reached) |
rsavitski | 30:791739422122 | 59 | { |
rsavitski | 39:44d3dea4adcc | 60 | setWaypointReached(); |
rsavitski | 30:791739422122 | 61 | } |
rsavitski | 39:44d3dea4adcc | 62 | waypoint_flag_mutex.unlock(); |
rsavitski | 24:50805ef8c499 | 63 | |
rsavitski | 24:50805ef8c499 | 64 | // angular velocity controller |
madcowswe | 34:e1678450feec | 65 | const float p_gain_av = 0.5; //TODO: tune |
rsavitski | 24:50805ef8c499 | 66 | |
madcowswe | 33:a49197572737 | 67 | const float max_av = 0.5*PI; // radians per sec //TODO: tune |
rsavitski | 24:50805ef8c499 | 68 | |
rsavitski | 24:50805ef8c499 | 69 | // angle error [-pi, pi] |
rsavitski | 24:50805ef8c499 | 70 | float angular_v = p_gain_av * angle_err; |
rsavitski | 24:50805ef8c499 | 71 | |
rsavitski | 24:50805ef8c499 | 72 | // constrain range |
rsavitski | 24:50805ef8c499 | 73 | if (angular_v > max_av) |
rsavitski | 24:50805ef8c499 | 74 | angular_v = max_av; |
rsavitski | 24:50805ef8c499 | 75 | else if (angular_v < -max_av) |
rsavitski | 24:50805ef8c499 | 76 | angular_v = -max_av; |
rsavitski | 24:50805ef8c499 | 77 | |
rsavitski | 24:50805ef8c499 | 78 | |
rsavitski | 24:50805ef8c499 | 79 | // forward velocity controller |
madcowswe | 34:e1678450feec | 80 | const float p_gain_fv = 0.5; //TODO: tune |
rsavitski | 24:50805ef8c499 | 81 | |
madcowswe | 25:b16f1045108f | 82 | float max_fv = 0.2; // meters per sec //TODO: tune |
rsavitski | 24:50805ef8c499 | 83 | const float angle_envelope_exponent = 8.0; //TODO: tune |
rsavitski | 24:50805ef8c499 | 84 | |
rsavitski | 24:50805ef8c499 | 85 | // control, distance_err in meters |
rsavitski | 24:50805ef8c499 | 86 | float forward_v = p_gain_fv * distance_err; |
rsavitski | 24:50805ef8c499 | 87 | |
rsavitski | 24:50805ef8c499 | 88 | // control the forward velocity envelope based on angular error |
rsavitski | 24:50805ef8c499 | 89 | max_fv = max_fv * pow(cos(angle_err/2), angle_envelope_exponent); |
rsavitski | 24:50805ef8c499 | 90 | |
rsavitski | 24:50805ef8c499 | 91 | // constrain range |
rsavitski | 24:50805ef8c499 | 92 | if (forward_v > max_fv) |
rsavitski | 24:50805ef8c499 | 93 | forward_v = max_fv; |
rsavitski | 24:50805ef8c499 | 94 | else if (forward_v < -max_fv) |
rsavitski | 24:50805ef8c499 | 95 | forward_v = -max_fv; |
madcowswe | 25:b16f1045108f | 96 | |
madcowswe | 25:b16f1045108f | 97 | //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v); |
rsavitski | 24:50805ef8c499 | 98 | |
rsavitski | 30:791739422122 | 99 | // pass values to the motor control |
rsavitski | 24:50805ef8c499 | 100 | MotorControl::set_fwdcmd(forward_v); |
madcowswe | 25:b16f1045108f | 101 | MotorControl::set_omegacmd(angular_v); |
rsavitski | 24:50805ef8c499 | 102 | } |
rsavitski | 24:50805ef8c499 | 103 | |
rsavitski | 39:44d3dea4adcc | 104 | void setNewWaypoint(Waypoint *new_wp) |
rsavitski | 39:44d3dea4adcc | 105 | { |
rsavitski | 39:44d3dea4adcc | 106 | current_waypoint = new_wp; |
rsavitski | 39:44d3dea4adcc | 107 | } |
rsavitski | 39:44d3dea4adcc | 108 | |
rsavitski | 39:44d3dea4adcc | 109 | void setWaypointReached() |
rsavitski | 39:44d3dea4adcc | 110 | { |
rsavitski | 39:44d3dea4adcc | 111 | waypoint_reached = true; |
rsavitski | 39:44d3dea4adcc | 112 | } |
rsavitski | 39:44d3dea4adcc | 113 | |
rsavitski | 39:44d3dea4adcc | 114 | void clearWaypointReached() |
rsavitski | 39:44d3dea4adcc | 115 | { |
rsavitski | 39:44d3dea4adcc | 116 | waypoint_reached = false; |
rsavitski | 39:44d3dea4adcc | 117 | } |
rsavitski | 39:44d3dea4adcc | 118 | |
rsavitski | 39:44d3dea4adcc | 119 | bool checkWaypointStatus() |
rsavitski | 39:44d3dea4adcc | 120 | { |
rsavitski | 39:44d3dea4adcc | 121 | return waypoint_reached; |
rsavitski | 39:44d3dea4adcc | 122 | } |
rsavitski | 39:44d3dea4adcc | 123 | |
rsavitski | 24:50805ef8c499 | 124 | } //namespace |