Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/Motion/motion.cpp@57:d434ceab6892, 2013-04-13 (annotated)

Committer:
rsavitski
Date:
Sat Apr 13 21:29:35 2013 +0000
Revision:
57:d434ceab6892
Parent:
54:99d3158c9207
Child:
64:c979fb1cd3b5
Child:
65:4709ff6c753c
refactored motion with proxy movement object, working

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