2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Processes/Motion/motion.cpp@26:7cb3a21d9a2e, 2013-04-10 (annotated)
- Committer:
- madcowswe
- Date:
- Wed Apr 10 03:46:23 2013 +0000
- Revision:
- 26:7cb3a21d9a2e
- Parent:
- 25:b16f1045108f
- Child:
- 32:e3f633620816
Update loop rewritten, not tested not set to run
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" |
madcowswe | 25:b16f1045108f | 9 | #include "supportfuncs.h" |
madcowswe | 26:7cb3a21d9a2e | 10 | #include "Kalman.h" |
rsavitski | 24:50805ef8c499 | 11 | |
rsavitski | 24:50805ef8c499 | 12 | namespace motion |
rsavitski | 24:50805ef8c499 | 13 | { |
rsavitski | 24:50805ef8c499 | 14 | |
rsavitski | 24:50805ef8c499 | 15 | void motionlayer(void const *dummy) |
rsavitski | 24:50805ef8c499 | 16 | { |
rsavitski | 24:50805ef8c499 | 17 | //TODO: current_waypoint global in AI layer |
rsavitski | 24:50805ef8c499 | 18 | //TODO: threshold for deciding that the waypoint has been achieved |
rsavitski | 24:50805ef8c499 | 19 | |
rsavitski | 24:50805ef8c499 | 20 | // get target waypoint from AI |
rsavitski | 24:50805ef8c499 | 21 | Waypoint target_waypoint = *AI::current_waypoint; |
rsavitski | 24:50805ef8c499 | 22 | |
rsavitski | 24:50805ef8c499 | 23 | // get current state from Kalman |
madcowswe | 26:7cb3a21d9a2e | 24 | using Kalman::State; |
rsavitski | 24:50805ef8c499 | 25 | State current_state = Kalman::getState(); |
rsavitski | 24:50805ef8c499 | 26 | |
rsavitski | 24:50805ef8c499 | 27 | float delta_x = target_waypoint.x - current_state.x; |
rsavitski | 24:50805ef8c499 | 28 | float delta_y = target_waypoint.y - current_state.y; |
rsavitski | 24:50805ef8c499 | 29 | |
madcowswe | 25:b16f1045108f | 30 | //printf("motion sees deltax: %f deltay %f\r\n", delta_x, delta_y); |
rsavitski | 24:50805ef8c499 | 31 | |
madcowswe | 25:b16f1045108f | 32 | float distance_err = hypot(delta_x, delta_y); |
madcowswe | 25:b16f1045108f | 33 | |
madcowswe | 25:b16f1045108f | 34 | float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta); |
rsavitski | 24:50805ef8c499 | 35 | |
rsavitski | 24:50805ef8c499 | 36 | |
rsavitski | 24:50805ef8c499 | 37 | // angular velocity controller |
madcowswe | 25:b16f1045108f | 38 | const float p_gain_av = 0.3; //TODO: tune |
rsavitski | 24:50805ef8c499 | 39 | |
madcowswe | 25:b16f1045108f | 40 | const float max_av = 0.2*PI; // radians per sec //TODO: tune |
rsavitski | 24:50805ef8c499 | 41 | |
rsavitski | 24:50805ef8c499 | 42 | // angle error [-pi, pi] |
rsavitski | 24:50805ef8c499 | 43 | float angular_v = p_gain_av * angle_err; |
rsavitski | 24:50805ef8c499 | 44 | |
rsavitski | 24:50805ef8c499 | 45 | // constrain range |
rsavitski | 24:50805ef8c499 | 46 | if (angular_v > max_av) |
rsavitski | 24:50805ef8c499 | 47 | angular_v = max_av; |
rsavitski | 24:50805ef8c499 | 48 | else if (angular_v < -max_av) |
rsavitski | 24:50805ef8c499 | 49 | angular_v = -max_av; |
rsavitski | 24:50805ef8c499 | 50 | |
rsavitski | 24:50805ef8c499 | 51 | |
rsavitski | 24:50805ef8c499 | 52 | // forward velocity controller |
madcowswe | 25:b16f1045108f | 53 | const float p_gain_fv = 0.3; //TODO: tune |
rsavitski | 24:50805ef8c499 | 54 | |
madcowswe | 25:b16f1045108f | 55 | float max_fv = 0.2; // meters per sec //TODO: tune |
rsavitski | 24:50805ef8c499 | 56 | const float angle_envelope_exponent = 8.0; //TODO: tune |
rsavitski | 24:50805ef8c499 | 57 | |
rsavitski | 24:50805ef8c499 | 58 | // control, distance_err in meters |
rsavitski | 24:50805ef8c499 | 59 | float forward_v = p_gain_fv * distance_err; |
rsavitski | 24:50805ef8c499 | 60 | |
rsavitski | 24:50805ef8c499 | 61 | // control the forward velocity envelope based on angular error |
rsavitski | 24:50805ef8c499 | 62 | max_fv = max_fv * pow(cos(angle_err/2), angle_envelope_exponent); |
rsavitski | 24:50805ef8c499 | 63 | |
rsavitski | 24:50805ef8c499 | 64 | // constrain range |
rsavitski | 24:50805ef8c499 | 65 | if (forward_v > max_fv) |
rsavitski | 24:50805ef8c499 | 66 | forward_v = max_fv; |
rsavitski | 24:50805ef8c499 | 67 | else if (forward_v < -max_fv) |
rsavitski | 24:50805ef8c499 | 68 | forward_v = -max_fv; |
madcowswe | 25:b16f1045108f | 69 | |
madcowswe | 25:b16f1045108f | 70 | //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v); |
rsavitski | 24:50805ef8c499 | 71 | |
rsavitski | 24:50805ef8c499 | 72 | //TODO: put into motor control |
rsavitski | 24:50805ef8c499 | 73 | MotorControl::set_fwdcmd(forward_v); |
madcowswe | 25:b16f1045108f | 74 | MotorControl::set_omegacmd(angular_v); |
rsavitski | 24:50805ef8c499 | 75 | } |
rsavitski | 24:50805ef8c499 | 76 | |
rsavitski | 24:50805ef8c499 | 77 | } //namespace |