2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Processes/Motion/motion.cpp@67:be3ea5450cc7, 2013-04-14 (annotated)
- Committer:
- madcowswe
- Date:
- Sun Apr 14 18:47:17 2013 +0000
- Revision:
- 67:be3ea5450cc7
- Parent:
- 66:f1d75e51398d
- Parent:
- 65:4709ff6c753c
- Child:
- 69:4b7bb92916da
Tuned merge. Testing needed
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 | 57:d434ceab6892 | 9 | #include "math.h" |
rsavitski | 57:d434ceab6892 | 10 | #include "Kalman.h" |
rsavitski | 57:d434ceab6892 | 11 | #include "MotorControl.h" |
rsavitski | 57:d434ceab6892 | 12 | #include "supportfuncs.h" |
madcowswe | 66:f1d75e51398d | 13 | #include "AvoidDstSensor.h" |
rsavitski | 57:d434ceab6892 | 14 | |
rsavitski | 57:d434ceab6892 | 15 | namespace motion |
rsavitski | 57:d434ceab6892 | 16 | { |
rsavitski | 57:d434ceab6892 | 17 | // private function prototypes |
rsavitski | 57:d434ceab6892 | 18 | |
rsavitski | 57:d434ceab6892 | 19 | void setWaypointReached(); |
rsavitski | 57:d434ceab6892 | 20 | void clearMotionCmd(); |
rsavitski | 57:d434ceab6892 | 21 | void clearWaypoint(); |
rsavitski | 57:d434ceab6892 | 22 | } |
rsavitski | 38:c9058a401410 | 23 | |
rsavitski | 24:50805ef8c499 | 24 | namespace motion |
rsavitski | 24:50805ef8c499 | 25 | { |
rsavitski | 24:50805ef8c499 | 26 | |
madcowswe | 66:f1d75e51398d | 27 | volatile int collavoiden = 0; |
madcowswe | 66:f1d75e51398d | 28 | AvoidDstSensor ADS(P_FWD_DISTANCE_SENSOR); |
madcowswe | 66:f1d75e51398d | 29 | |
rsavitski | 57:d434ceab6892 | 30 | // motion commands supported |
rsavitski | 57:d434ceab6892 | 31 | enum motion_type_t { motion_waypoint }; |
rsavitski | 57:d434ceab6892 | 32 | |
rsavitski | 57:d434ceab6892 | 33 | struct motion_cmd |
rsavitski | 57:d434ceab6892 | 34 | { |
rsavitski | 57:d434ceab6892 | 35 | motion_type_t motion_type; |
rsavitski | 57:d434ceab6892 | 36 | osThreadId setter_tid; |
rsavitski | 57:d434ceab6892 | 37 | |
rsavitski | 57:d434ceab6892 | 38 | bool motion_done; |
rsavitski | 57:d434ceab6892 | 39 | |
rsavitski | 57:d434ceab6892 | 40 | union |
rsavitski | 57:d434ceab6892 | 41 | { |
rsavitski | 57:d434ceab6892 | 42 | Waypoint *wp_ptr; |
rsavitski | 57:d434ceab6892 | 43 | }; |
rsavitski | 57:d434ceab6892 | 44 | |
rsavitski | 57:d434ceab6892 | 45 | }; |
rsavitski | 57:d434ceab6892 | 46 | |
rsavitski | 57:d434ceab6892 | 47 | // local copy of the current active motion |
rsavitski | 57:d434ceab6892 | 48 | motion_cmd current_motion; |
rsavitski | 57:d434ceab6892 | 49 | |
rsavitski | 39:44d3dea4adcc | 50 | Mutex waypoint_flag_mutex; |
rsavitski | 39:44d3dea4adcc | 51 | |
rsavitski | 57:d434ceab6892 | 52 | // local to waypoint motion handler |
rsavitski | 57:d434ceab6892 | 53 | bool wp_d_reached = false; |
rsavitski | 57:d434ceab6892 | 54 | bool wp_a_reached = false; |
rsavitski | 39:44d3dea4adcc | 55 | |
rsavitski | 39:44d3dea4adcc | 56 | |
rsavitski | 24:50805ef8c499 | 57 | void motionlayer(void const *dummy) |
rsavitski | 57:d434ceab6892 | 58 | { |
rsavitski | 57:d434ceab6892 | 59 | switch (current_motion.motion_type) |
rsavitski | 57:d434ceab6892 | 60 | { |
rsavitski | 57:d434ceab6892 | 61 | case motion_waypoint: |
rsavitski | 57:d434ceab6892 | 62 | waypoint_motion_handler(); |
rsavitski | 57:d434ceab6892 | 63 | break; |
rsavitski | 57:d434ceab6892 | 64 | default: |
rsavitski | 57:d434ceab6892 | 65 | break; |
rsavitski | 57:d434ceab6892 | 66 | } |
rsavitski | 57:d434ceab6892 | 67 | } |
rsavitski | 57:d434ceab6892 | 68 | |
rsavitski | 57:d434ceab6892 | 69 | void waypoint_motion_handler() |
rsavitski | 57:d434ceab6892 | 70 | { |
rsavitski | 39:44d3dea4adcc | 71 | // save target waypoint |
rsavitski | 57:d434ceab6892 | 72 | Waypoint target_waypoint = *current_motion.wp_ptr; |
rsavitski | 24:50805ef8c499 | 73 | |
rsavitski | 24:50805ef8c499 | 74 | // get current state from Kalman |
rsavitski | 24:50805ef8c499 | 75 | State current_state = Kalman::getState(); |
rsavitski | 24:50805ef8c499 | 76 | |
rsavitski | 24:50805ef8c499 | 77 | float delta_x = target_waypoint.x - current_state.x; |
rsavitski | 24:50805ef8c499 | 78 | float delta_y = target_waypoint.y - current_state.y; |
rsavitski | 24:50805ef8c499 | 79 | |
madcowswe | 25:b16f1045108f | 80 | //printf("motion sees deltax: %f deltay %f\r\n", delta_x, delta_y); |
rsavitski | 24:50805ef8c499 | 81 | |
madcowswe | 25:b16f1045108f | 82 | float distance_err = hypot(delta_x, delta_y); |
madcowswe | 25:b16f1045108f | 83 | |
madcowswe | 25:b16f1045108f | 84 | float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta); |
rsavitski | 24:50805ef8c499 | 85 | |
rsavitski | 57:d434ceab6892 | 86 | // reversing if close to target and more optimal than forward movement |
rsavitski | 52:bffe5f7c39a3 | 87 | bool reversing = false; |
rsavitski | 57:d434ceab6892 | 88 | if ((abs(angle_err) > PI/2) && (distance_err < 0.2)) //TODO: parameterise and tune 0.2 meters hardcoded |
rsavitski | 52:bffe5f7c39a3 | 89 | { |
rsavitski | 52:bffe5f7c39a3 | 90 | reversing = true; |
rsavitski | 52:bffe5f7c39a3 | 91 | angle_err = constrainAngle(angle_err + PI); |
rsavitski | 52:bffe5f7c39a3 | 92 | distance_err = -distance_err; |
rsavitski | 52:bffe5f7c39a3 | 93 | } |
rsavitski | 52:bffe5f7c39a3 | 94 | |
rsavitski | 52:bffe5f7c39a3 | 95 | float angle_err_saved = angle_err; // actuated angle error can be overriden by the final turning code, but forward speed envelope should be controlled with the atan angle |
rsavitski | 52:bffe5f7c39a3 | 96 | |
rsavitski | 30:791739422122 | 97 | // is the waypoint reached |
rsavitski | 57:d434ceab6892 | 98 | waypoint_flag_mutex.lock(); //TODO: consider refactoring |
rsavitski | 57:d434ceab6892 | 99 | if (abs(distance_err) < ((wp_d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold)) |
rsavitski | 30:791739422122 | 100 | { |
rsavitski | 57:d434ceab6892 | 101 | wp_d_reached = true; |
rsavitski | 38:c9058a401410 | 102 | |
rsavitski | 52:bffe5f7c39a3 | 103 | angle_err = constrainAngle(target_waypoint.theta - current_state.theta); |
rsavitski | 38:c9058a401410 | 104 | |
rsavitski | 38:c9058a401410 | 105 | if (abs(angle_err) < target_waypoint.angle_threshold) |
madcowswe | 33:a49197572737 | 106 | { |
rsavitski | 57:d434ceab6892 | 107 | wp_a_reached = true; |
madcowswe | 33:a49197572737 | 108 | } |
rsavitski | 35:f8e7f0a72a3d | 109 | } |
rsavitski | 65:4709ff6c753c | 110 | // 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 | 57:d434ceab6892 | 111 | if (wp_d_reached && wp_a_reached) |
rsavitski | 30:791739422122 | 112 | { |
rsavitski | 39:44d3dea4adcc | 113 | setWaypointReached(); |
rsavitski | 30:791739422122 | 114 | } |
rsavitski | 39:44d3dea4adcc | 115 | waypoint_flag_mutex.unlock(); |
rsavitski | 24:50805ef8c499 | 116 | |
rsavitski | 24:50805ef8c499 | 117 | // angular velocity controller |
madcowswe | 67:be3ea5450cc7 | 118 | const float p_gain_av = 0.7; //TODO: tune |
rsavitski | 24:50805ef8c499 | 119 | |
madcowswe | 64:c979fb1cd3b5 | 120 | const float max_av = 0.5; // radians per sec //TODO: tune |
rsavitski | 24:50805ef8c499 | 121 | |
rsavitski | 24:50805ef8c499 | 122 | // angle error [-pi, pi] |
rsavitski | 24:50805ef8c499 | 123 | float angular_v = p_gain_av * angle_err; |
rsavitski | 24:50805ef8c499 | 124 | |
rsavitski | 24:50805ef8c499 | 125 | // constrain range |
rsavitski | 24:50805ef8c499 | 126 | if (angular_v > max_av) |
rsavitski | 24:50805ef8c499 | 127 | angular_v = max_av; |
rsavitski | 24:50805ef8c499 | 128 | else if (angular_v < -max_av) |
rsavitski | 24:50805ef8c499 | 129 | angular_v = -max_av; |
rsavitski | 24:50805ef8c499 | 130 | |
rsavitski | 24:50805ef8c499 | 131 | |
rsavitski | 24:50805ef8c499 | 132 | // forward velocity controller |
madcowswe | 67:be3ea5450cc7 | 133 | const float p_gain_fv = 0.7; //TODO: tune |
rsavitski | 24:50805ef8c499 | 134 | |
madcowswe | 25:b16f1045108f | 135 | float max_fv = 0.2; // meters per sec //TODO: tune |
rsavitski | 52:bffe5f7c39a3 | 136 | float max_fv_reverse = 0.05; //TODO: tune |
madcowswe | 67:be3ea5450cc7 | 137 | const float angle_envelope_exponent = 32;//512; //8.0; //TODO: tune |
rsavitski | 24:50805ef8c499 | 138 | |
rsavitski | 24:50805ef8c499 | 139 | // control, distance_err in meters |
rsavitski | 24:50805ef8c499 | 140 | float forward_v = p_gain_fv * distance_err; |
rsavitski | 24:50805ef8c499 | 141 | |
rsavitski | 52:bffe5f7c39a3 | 142 | // if reversing, robot is less stable hence a different cap is used |
rsavitski | 52:bffe5f7c39a3 | 143 | if (reversing) |
rsavitski | 52:bffe5f7c39a3 | 144 | max_fv = max_fv_reverse; |
rsavitski | 52:bffe5f7c39a3 | 145 | |
rsavitski | 24:50805ef8c499 | 146 | // control the forward velocity envelope based on angular error |
rsavitski | 52:bffe5f7c39a3 | 147 | max_fv = max_fv * pow(cos(angle_err_saved/2), angle_envelope_exponent); |
rsavitski | 24:50805ef8c499 | 148 | |
rsavitski | 24:50805ef8c499 | 149 | // constrain range |
rsavitski | 24:50805ef8c499 | 150 | if (forward_v > max_fv) |
rsavitski | 24:50805ef8c499 | 151 | forward_v = max_fv; |
rsavitski | 24:50805ef8c499 | 152 | else if (forward_v < -max_fv) |
rsavitski | 24:50805ef8c499 | 153 | forward_v = -max_fv; |
madcowswe | 25:b16f1045108f | 154 | |
madcowswe | 25:b16f1045108f | 155 | //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v); |
rsavitski | 24:50805ef8c499 | 156 | |
madcowswe | 66:f1d75e51398d | 157 | if(collavoiden){ |
madcowswe | 66:f1d75e51398d | 158 | float d = ADS.Distanceincm(); |
madcowswe | 66:f1d75e51398d | 159 | if(d > 10){ |
madcowswe | 66:f1d75e51398d | 160 | forward_v *= max(min((d-15)*(1.0f/20.0f),1.0f),-0.1f); |
madcowswe | 66:f1d75e51398d | 161 | } |
madcowswe | 66:f1d75e51398d | 162 | } |
madcowswe | 66:f1d75e51398d | 163 | |
rsavitski | 30:791739422122 | 164 | // pass values to the motor control |
rsavitski | 24:50805ef8c499 | 165 | MotorControl::set_fwdcmd(forward_v); |
madcowswe | 25:b16f1045108f | 166 | MotorControl::set_omegacmd(angular_v); |
rsavitski | 24:50805ef8c499 | 167 | } |
rsavitski | 24:50805ef8c499 | 168 | |
rsavitski | 57:d434ceab6892 | 169 | |
rsavitski | 57:d434ceab6892 | 170 | bool checkMotionStatus() |
rsavitski | 57:d434ceab6892 | 171 | { |
rsavitski | 57:d434ceab6892 | 172 | return current_motion.motion_done; |
rsavitski | 57:d434ceab6892 | 173 | } |
rsavitski | 57:d434ceab6892 | 174 | |
rsavitski | 57:d434ceab6892 | 175 | |
rsavitski | 57:d434ceab6892 | 176 | void setNewWaypoint(osThreadId setter_tid_in, Waypoint *new_wp) |
rsavitski | 39:44d3dea4adcc | 177 | { |
rsavitski | 57:d434ceab6892 | 178 | clearMotionCmd(); |
rsavitski | 57:d434ceab6892 | 179 | |
rsavitski | 57:d434ceab6892 | 180 | current_motion.setter_tid = setter_tid_in; |
rsavitski | 57:d434ceab6892 | 181 | current_motion.motion_type = motion_waypoint; |
rsavitski | 57:d434ceab6892 | 182 | |
rsavitski | 57:d434ceab6892 | 183 | current_motion.wp_ptr = new_wp; |
rsavitski | 39:44d3dea4adcc | 184 | } |
rsavitski | 39:44d3dea4adcc | 185 | |
rsavitski | 57:d434ceab6892 | 186 | |
rsavitski | 39:44d3dea4adcc | 187 | void setWaypointReached() |
rsavitski | 39:44d3dea4adcc | 188 | { |
rsavitski | 57:d434ceab6892 | 189 | current_motion.motion_done = true; |
rsavitski | 57:d434ceab6892 | 190 | osSignalSet(current_motion.setter_tid,0x1); |
rsavitski | 39:44d3dea4adcc | 191 | } |
rsavitski | 39:44d3dea4adcc | 192 | |
rsavitski | 57:d434ceab6892 | 193 | |
rsavitski | 57:d434ceab6892 | 194 | void clearMotionCmd() |
rsavitski | 39:44d3dea4adcc | 195 | { |
rsavitski | 57:d434ceab6892 | 196 | current_motion.motion_done = false; |
rsavitski | 57:d434ceab6892 | 197 | osSignalClear(current_motion.setter_tid, 0x1); |
rsavitski | 57:d434ceab6892 | 198 | |
rsavitski | 57:d434ceab6892 | 199 | switch (current_motion.motion_type) |
rsavitski | 57:d434ceab6892 | 200 | { |
rsavitski | 57:d434ceab6892 | 201 | case motion_waypoint: |
rsavitski | 57:d434ceab6892 | 202 | clearWaypoint(); |
rsavitski | 57:d434ceab6892 | 203 | break; |
rsavitski | 57:d434ceab6892 | 204 | |
rsavitski | 57:d434ceab6892 | 205 | default: |
rsavitski | 57:d434ceab6892 | 206 | break; |
rsavitski | 57:d434ceab6892 | 207 | } |
rsavitski | 39:44d3dea4adcc | 208 | } |
rsavitski | 39:44d3dea4adcc | 209 | |
rsavitski | 57:d434ceab6892 | 210 | |
rsavitski | 57:d434ceab6892 | 211 | void clearWaypoint() |
rsavitski | 39:44d3dea4adcc | 212 | { |
rsavitski | 57:d434ceab6892 | 213 | wp_d_reached = false; |
rsavitski | 57:d434ceab6892 | 214 | wp_a_reached = false; |
rsavitski | 39:44d3dea4adcc | 215 | } |
rsavitski | 39:44d3dea4adcc | 216 | |
rsavitski | 24:50805ef8c499 | 217 | } //namespace |