Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/Motion/motion.cpp@91:fdadfd883825, 2013-10-15 (annotated)

Committer:
rsavitski
Date:
Tue Oct 15 12:17:11 2013 +0000
Revision:
91:fdadfd883825
Parent:
88:8850373c3f0d 
2014 ICRS Eurobot codebase

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 69:4b7bb92916da 4 // Takes current state and motion command (set via accessors) and actuates it via motor layer calls
rsavitski 24:50805ef8c499 5 ////////////////////////////////////////////////////////////////////////////////
rsavitski 24:50805ef8c499 6
rsavitski 81:ef1ce4f5322b 7 //todo: check everything is properly initialised and not a race condition
rsavitski 81:ef1ce4f5322b 8
rsavitski 24:50805ef8c499 9 #include "motion.h"
rsavitski 57:d434ceab6892 10 #include "math.h"
rsavitski 57:d434ceab6892 11 #include "Kalman.h"
rsavitski 57:d434ceab6892 12 #include "MotorControl.h"
rsavitski 57:d434ceab6892 13 #include "supportfuncs.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
rsavitski 91:fdadfd883825 27 //2014: rework mutable waypoints and make them nicer
rsavitski 91:fdadfd883825 28
rsavitski 91:fdadfd883825 29
rsavitski 91:fdadfd883825 30 //volatile int collavoiden = 0; // TODO: kill oskar's code
rsavitski 91:fdadfd883825 31 //AvoidDstSensor ADS(P_FWD_DISTANCE_SENSOR); //TODO: kill oskar's hack //2014: redo collision avoidance
madcowswe 66:f1d75e51398d 32
rsavitski 57:d434ceab6892 33 // motion commands supported
rsavitski 57:d434ceab6892 34 enum motion_type_t { motion_waypoint };
rsavitski 57:d434ceab6892 35
rsavitski 57:d434ceab6892 36 struct motion_cmd
rsavitski 57:d434ceab6892 37 {
rsavitski 57:d434ceab6892 38 motion_type_t motion_type;
rsavitski 57:d434ceab6892 39 osThreadId setter_tid;
rsavitski 57:d434ceab6892 40
rsavitski 57:d434ceab6892 41 bool motion_done;
rsavitski 57:d434ceab6892 42
rsavitski 57:d434ceab6892 43 union
rsavitski 57:d434ceab6892 44 {
rsavitski 57:d434ceab6892 45 Waypoint *wp_ptr;
rsavitski 57:d434ceab6892 46 };
rsavitski 57:d434ceab6892 47
rsavitski 57:d434ceab6892 48 };
rsavitski 57:d434ceab6892 49
rsavitski 57:d434ceab6892 50 // local copy of the current active motion
rsavitski 76:532d9bc1d2aa 51 motion_cmd current_motion = {motion_waypoint, 0, false, NULL};
rsavitski 57:d434ceab6892 52
rsavitski 70:0da6ca845762 53 Waypoint target_waypoint = {0,0,0,0,0}; //local wp copy, TODO: fix and make a shared local memory pool for any movement cmd to be copied to
rsavitski 70:0da6ca845762 54
rsavitski 39:44d3dea4adcc 55 Mutex waypoint_flag_mutex;
rsavitski 39:44d3dea4adcc 56
rsavitski 57:d434ceab6892 57 // local to waypoint motion handler
rsavitski 57:d434ceab6892 58 bool wp_d_reached = false;
rsavitski 57:d434ceab6892 59 bool wp_a_reached = false;
rsavitski 39:44d3dea4adcc 60
rsavitski 39:44d3dea4adcc 61
rsavitski 24:50805ef8c499 62 void motionlayer(void const *dummy)
rsavitski 57:d434ceab6892 63 {
rsavitski 57:d434ceab6892 64 switch (current_motion.motion_type)
rsavitski 57:d434ceab6892 65 {
rsavitski 57:d434ceab6892 66 case motion_waypoint:
rsavitski 57:d434ceab6892 67 waypoint_motion_handler();
rsavitski 57:d434ceab6892 68 break;
rsavitski 57:d434ceab6892 69 default:
rsavitski 57:d434ceab6892 70 break;
rsavitski 57:d434ceab6892 71 }
rsavitski 57:d434ceab6892 72 }
rsavitski 57:d434ceab6892 73
rsavitski 57:d434ceab6892 74 void waypoint_motion_handler()
rsavitski 57:d434ceab6892 75 {
rsavitski 39:44d3dea4adcc 76 // save target waypoint
rsavitski 70:0da6ca845762 77 //Waypoint target_waypoint = *current_motion.wp_ptr;
rsavitski 70:0da6ca845762 78
rsavitski 24:50805ef8c499 79
rsavitski 24:50805ef8c499 80 // get current state from Kalman
rsavitski 24:50805ef8c499 81 State current_state = Kalman::getState();
rsavitski 24:50805ef8c499 82
rsavitski 24:50805ef8c499 83 float delta_x = target_waypoint.x - current_state.x;
rsavitski 24:50805ef8c499 84 float delta_y = target_waypoint.y - current_state.y;
rsavitski 24:50805ef8c499 85
madcowswe 25:b16f1045108f 86 //printf("motion sees deltax: %f deltay %f\r\n", delta_x, delta_y);
rsavitski 24:50805ef8c499 87
madcowswe 25:b16f1045108f 88 float distance_err = hypot(delta_x, delta_y);
madcowswe 25:b16f1045108f 89
madcowswe 25:b16f1045108f 90 float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta);
rsavitski 24:50805ef8c499 91
rsavitski 57:d434ceab6892 92 // reversing if close to target and more optimal than forward movement
rsavitski 52:bffe5f7c39a3 93 bool reversing = false;
rsavitski 57:d434ceab6892 94 if ((abs(angle_err) > PI/2) && (distance_err < 0.2)) //TODO: parameterise and tune 0.2 meters hardcoded
rsavitski 52:bffe5f7c39a3 95 {
rsavitski 52:bffe5f7c39a3 96 reversing = true;
rsavitski 52:bffe5f7c39a3 97 angle_err = constrainAngle(angle_err + PI);
rsavitski 52:bffe5f7c39a3 98 distance_err = -distance_err;
rsavitski 52:bffe5f7c39a3 99 }
rsavitski 52:bffe5f7c39a3 100
rsavitski 52:bffe5f7c39a3 101 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 102
rsavitski 30:791739422122 103 // is the waypoint reached
rsavitski 81:ef1ce4f5322b 104 waypoint_flag_mutex.lock(); //TODO: consider refactoring, mutexes suck, switch to semaphore style or more contained mutexes (think of several producers as well); race conditions not checked atm either
rsavitski 57:d434ceab6892 105 if (abs(distance_err) < ((wp_d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold))
rsavitski 30:791739422122 106 {
rsavitski 57:d434ceab6892 107 wp_d_reached = true;
rsavitski 38:c9058a401410 108
rsavitski 52:bffe5f7c39a3 109 angle_err = constrainAngle(target_waypoint.theta - current_state.theta);
rsavitski 38:c9058a401410 110
rsavitski 38:c9058a401410 111 if (abs(angle_err) < target_waypoint.angle_threshold)
madcowswe 33:a49197572737 112 {
rsavitski 57:d434ceab6892 113 wp_a_reached = true;
madcowswe 33:a49197572737 114 }
rsavitski 35:f8e7f0a72a3d 115 }
rsavitski 65:4709ff6c753c 116 // 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 117 if (wp_d_reached && wp_a_reached)
rsavitski 30:791739422122 118 {
rsavitski 39:44d3dea4adcc 119 setWaypointReached();
rsavitski 30:791739422122 120 }
rsavitski 39:44d3dea4adcc 121 waypoint_flag_mutex.unlock();
rsavitski 24:50805ef8c499 122
rsavitski 24:50805ef8c499 123 // angular velocity controller
rsavitski 70:0da6ca845762 124 const float p_gain_av = 1;//0.7; //TODO: tune
rsavitski 24:50805ef8c499 125
rsavitski 70:0da6ca845762 126 const float max_av = 1;//0.5; // radians per sec //TODO: tune
rsavitski 24:50805ef8c499 127
rsavitski 24:50805ef8c499 128 // angle error [-pi, pi]
rsavitski 24:50805ef8c499 129 float angular_v = p_gain_av * angle_err;
rsavitski 24:50805ef8c499 130
rsavitski 24:50805ef8c499 131 // constrain range
rsavitski 24:50805ef8c499 132 if (angular_v > max_av)
rsavitski 24:50805ef8c499 133 angular_v = max_av;
rsavitski 24:50805ef8c499 134 else if (angular_v < -max_av)
rsavitski 24:50805ef8c499 135 angular_v = -max_av;
rsavitski 24:50805ef8c499 136
rsavitski 24:50805ef8c499 137
rsavitski 24:50805ef8c499 138 // forward velocity controller
madcowswe 88:8850373c3f0d 139 const float p_gain_fv = 0.95;//0.7; //TODO: tune
rsavitski 24:50805ef8c499 140
rsavitski 70:0da6ca845762 141 float max_fv = 0.3;//0.2; // meters per sec //TODO: tune
rsavitski 77:8d83a0c00e66 142 float max_fv_reverse = 0.03; //TODO: tune
madcowswe 67:be3ea5450cc7 143 const float angle_envelope_exponent = 32;//512; //8.0; //TODO: tune
rsavitski 24:50805ef8c499 144
rsavitski 24:50805ef8c499 145 // control, distance_err in meters
rsavitski 24:50805ef8c499 146 float forward_v = p_gain_fv * distance_err;
rsavitski 24:50805ef8c499 147
rsavitski 52:bffe5f7c39a3 148 // if reversing, robot is less stable hence a different cap is used
rsavitski 52:bffe5f7c39a3 149 if (reversing)
rsavitski 52:bffe5f7c39a3 150 max_fv = max_fv_reverse;
rsavitski 52:bffe5f7c39a3 151
rsavitski 24:50805ef8c499 152 // control the forward velocity envelope based on angular error
rsavitski 77:8d83a0c00e66 153 max_fv = max_fv * pow(cos(angle_err_saved/2), /*angle_envelope_exponent*/target_waypoint.angle_exponent); //temp hack
rsavitski 24:50805ef8c499 154
rsavitski 24:50805ef8c499 155 // constrain range
rsavitski 24:50805ef8c499 156 if (forward_v > max_fv)
rsavitski 24:50805ef8c499 157 forward_v = max_fv;
rsavitski 24:50805ef8c499 158 else if (forward_v < -max_fv)
rsavitski 24:50805ef8c499 159 forward_v = -max_fv;
madcowswe 25:b16f1045108f 160
madcowswe 25:b16f1045108f 161 //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v);
rsavitski 24:50805ef8c499 162
rsavitski 91:fdadfd883825 163 //2014
rsavitski 69:4b7bb92916da 164 //TODO: remove oskar's avoidance hack
rsavitski 91:fdadfd883825 165 /*if(collavoiden)
rsavitski 69:4b7bb92916da 166 {
madcowswe 66:f1d75e51398d 167 float d = ADS.Distanceincm();
rsavitski 69:4b7bb92916da 168 if(d > 10)
rsavitski 69:4b7bb92916da 169 {
madcowswe 72:7996aa8286ae 170 forward_v *= max(min((d-15)*(1.0f/25.0f),1.0f),-0.1f);
madcowswe 66:f1d75e51398d 171 }
madcowswe 66:f1d75e51398d 172 }
rsavitski 91:fdadfd883825 173 */
rsavitski 69:4b7bb92916da 174 // end of Oskar hack
madcowswe 66:f1d75e51398d 175
rsavitski 30:791739422122 176 // pass values to the motor control
rsavitski 24:50805ef8c499 177 MotorControl::set_fwdcmd(forward_v);
madcowswe 25:b16f1045108f 178 MotorControl::set_omegacmd(angular_v);
rsavitski 24:50805ef8c499 179 }
rsavitski 24:50805ef8c499 180
rsavitski 57:d434ceab6892 181
rsavitski 57:d434ceab6892 182 bool checkMotionStatus()
rsavitski 57:d434ceab6892 183 {
rsavitski 57:d434ceab6892 184 return current_motion.motion_done;
rsavitski 57:d434ceab6892 185 }
rsavitski 57:d434ceab6892 186
rsavitski 57:d434ceab6892 187
rsavitski 57:d434ceab6892 188 void setNewWaypoint(osThreadId setter_tid_in, Waypoint *new_wp)
rsavitski 39:44d3dea4adcc 189 {
rsavitski 57:d434ceab6892 190 clearMotionCmd();
rsavitski 57:d434ceab6892 191
rsavitski 57:d434ceab6892 192 current_motion.setter_tid = setter_tid_in;
rsavitski 57:d434ceab6892 193 current_motion.motion_type = motion_waypoint;
rsavitski 57:d434ceab6892 194
rsavitski 70:0da6ca845762 195 target_waypoint = *new_wp;
rsavitski 39:44d3dea4adcc 196 }
rsavitski 39:44d3dea4adcc 197
rsavitski 57:d434ceab6892 198
rsavitski 39:44d3dea4adcc 199 void setWaypointReached()
rsavitski 39:44d3dea4adcc 200 {
rsavitski 57:d434ceab6892 201 current_motion.motion_done = true;
rsavitski 57:d434ceab6892 202 osSignalSet(current_motion.setter_tid,0x1);
rsavitski 39:44d3dea4adcc 203 }
rsavitski 39:44d3dea4adcc 204
rsavitski 57:d434ceab6892 205
rsavitski 57:d434ceab6892 206 void clearMotionCmd()
rsavitski 39:44d3dea4adcc 207 {
rsavitski 57:d434ceab6892 208 current_motion.motion_done = false;
rsavitski 57:d434ceab6892 209 osSignalClear(current_motion.setter_tid, 0x1);
rsavitski 57:d434ceab6892 210
rsavitski 57:d434ceab6892 211 switch (current_motion.motion_type)
rsavitski 57:d434ceab6892 212 {
rsavitski 57:d434ceab6892 213 case motion_waypoint:
rsavitski 57:d434ceab6892 214 clearWaypoint();
rsavitski 57:d434ceab6892 215 break;
rsavitski 57:d434ceab6892 216
rsavitski 57:d434ceab6892 217 default:
rsavitski 57:d434ceab6892 218 break;
rsavitski 57:d434ceab6892 219 }
rsavitski 39:44d3dea4adcc 220 }
rsavitski 39:44d3dea4adcc 221
rsavitski 57:d434ceab6892 222
rsavitski 57:d434ceab6892 223 void clearWaypoint()
rsavitski 39:44d3dea4adcc 224 {
rsavitski 57:d434ceab6892 225 wp_d_reached = false;
rsavitski 57:d434ceab6892 226 wp_a_reached = false;
rsavitski 39:44d3dea4adcc 227 }
rsavitski 39:44d3dea4adcc 228
rsavitski 24:50805ef8c499 229 } //namespace