Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

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?

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"
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