Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

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?

UserRevisionLine numberNew 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