motion.cpp

00001 ////////////////////////////////////////////////////////////////////////////////
00002 // Motion control unit
00003 ////////////////////////////////////////////////////////////////////////////////
00004 // Takes current state and motion command (set via accessors) and actuates it via motor layer calls
00005 ////////////////////////////////////////////////////////////////////////////////
00006 
00007 //todo: check everything is properly initialised and not a race condition
00008 
00009 #include "motion.h"
00010 #include "math.h"
00011 #include "Kalman.h"
00012 #include "MotorControl.h"
00013 #include "supportfuncs.h"
00014 
00015 namespace motion
00016 {
00017     // private function prototypes
00018     
00019     void setWaypointReached();
00020     void clearMotionCmd();
00021     void clearWaypoint();
00022 }
00023 
00024 namespace motion
00025 {
00026 
00027 //2014: rework mutable waypoints and make them nicer
00028 
00029 
00030 //volatile int collavoiden = 0; // TODO: kill oskar's code
00031 //AvoidDstSensor ADS(P_FWD_DISTANCE_SENSOR);  //TODO: kill oskar's hack //2014: redo collision avoidance
00032 
00033 // motion commands supported
00034 enum motion_type_t { motion_waypoint };
00035 
00036 struct motion_cmd
00037 {
00038     motion_type_t motion_type;    
00039     osThreadId setter_tid;    
00040     
00041     bool motion_done;
00042     
00043     union 
00044     {
00045         Waypoint *wp_ptr;
00046     };
00047 
00048 };
00049 
00050 // local copy of the current active motion
00051 motion_cmd current_motion = {motion_waypoint, 0, false, NULL};
00052 
00053 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
00054 
00055 Mutex waypoint_flag_mutex;
00056 
00057 // local to waypoint motion handler
00058 bool wp_d_reached = false;
00059 bool wp_a_reached = false;
00060 
00061 
00062 void motionlayer(void const *dummy)
00063 {   
00064     switch (current_motion.motion_type)
00065     {
00066         case motion_waypoint:
00067             waypoint_motion_handler();
00068         break;
00069         default:
00070         break;
00071     }
00072 }
00073 
00074 void waypoint_motion_handler()
00075 {
00076     // save target waypoint
00077     //Waypoint target_waypoint = *current_motion.wp_ptr;
00078     
00079     
00080     // get current state from Kalman
00081     State current_state = Kalman::getState();
00082     
00083     float delta_x = target_waypoint.x - current_state.x;
00084     float delta_y = target_waypoint.y - current_state.y;
00085     
00086     //printf("motion sees deltax: %f deltay %f\r\n", delta_x, delta_y);
00087     
00088     float distance_err = hypot(delta_x, delta_y);
00089     
00090     float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta);
00091     
00092     // reversing if close to target and more optimal than forward movement
00093     bool reversing = false;
00094     if ((abs(angle_err) > PI/2) && (distance_err < 0.2)) //TODO: parameterise and tune 0.2 meters hardcoded
00095     {
00096         reversing = true;
00097         angle_err = constrainAngle(angle_err + PI);
00098         distance_err = -distance_err;
00099     }
00100     
00101     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
00102     
00103     // is the waypoint reached
00104     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
00105     if (abs(distance_err) < ((wp_d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold))
00106     {
00107         wp_d_reached = true;
00108         
00109         angle_err = constrainAngle(target_waypoint.theta - current_state.theta);
00110         
00111         if (abs(angle_err) < target_waypoint.angle_threshold)
00112         {
00113             wp_a_reached = true;
00114         }
00115     } 
00116      // 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
00117     if (wp_d_reached && wp_a_reached)
00118     {
00119         setWaypointReached();
00120     }
00121     waypoint_flag_mutex.unlock();
00122     
00123     // angular velocity controller
00124     const float p_gain_av = 1;//0.7; //TODO: tune
00125     
00126     const float max_av = 1;//0.5; // radians per sec //TODO: tune
00127     
00128     // angle error [-pi, pi]
00129     float angular_v = p_gain_av * angle_err;
00130     
00131     // constrain range
00132     if (angular_v > max_av)
00133         angular_v = max_av;
00134     else if (angular_v < -max_av)
00135         angular_v = -max_av;
00136     
00137     
00138     // forward velocity controller
00139     const float p_gain_fv = 0.95;//0.7; //TODO: tune
00140     
00141     float max_fv = 0.3;//0.2; // meters per sec //TODO: tune
00142     float max_fv_reverse = 0.03; //TODO: tune
00143     const float angle_envelope_exponent = 32;//512; //8.0; //TODO: tune
00144     
00145     // control, distance_err in meters
00146     float forward_v = p_gain_fv * distance_err;
00147     
00148     // if reversing, robot is less stable hence a different cap is used
00149     if (reversing)
00150         max_fv = max_fv_reverse;
00151     
00152     // control the forward velocity envelope based on angular error
00153     max_fv = max_fv * pow(cos(angle_err_saved/2), /*angle_envelope_exponent*/target_waypoint.angle_exponent); //temp hack
00154     
00155     // constrain range
00156     if (forward_v > max_fv)
00157         forward_v = max_fv;
00158     else if (forward_v < -max_fv)
00159         forward_v = -max_fv;
00160         
00161     //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v);
00162     
00163     //2014
00164     //TODO: remove oskar's avoidance hack
00165     /*if(collavoiden)
00166     {
00167         float d = ADS.Distanceincm();
00168         if(d > 10)
00169         {
00170             forward_v *= max(min((d-15)*(1.0f/25.0f),1.0f),-0.1f);
00171         }
00172     }
00173     */
00174     // end of Oskar hack
00175     
00176     // pass values to the motor control
00177     MotorControl::set_fwdcmd(forward_v);
00178     MotorControl::set_omegacmd(angular_v);
00179 }
00180 
00181 
00182 bool checkMotionStatus()
00183 {
00184     return current_motion.motion_done;
00185 }
00186 
00187 
00188 void setNewWaypoint(osThreadId setter_tid_in, Waypoint *new_wp)
00189 {
00190     clearMotionCmd();
00191 
00192     current_motion.setter_tid = setter_tid_in;
00193     current_motion.motion_type = motion_waypoint;
00194 
00195     target_waypoint = *new_wp;
00196 }
00197 
00198 
00199 void setWaypointReached()
00200 {
00201     current_motion.motion_done = true;
00202     osSignalSet(current_motion.setter_tid,0x1);
00203 }
00204 
00205 
00206 void clearMotionCmd()
00207 {
00208     current_motion.motion_done = false;
00209     osSignalClear(current_motion.setter_tid, 0x1);
00210 
00211     switch (current_motion.motion_type)
00212     {
00213         case motion_waypoint:
00214             clearWaypoint();
00215         break;
00216         
00217         default:
00218         break;   
00219     }
00220 }
00221 
00222 
00223 void clearWaypoint()
00224 {
00225     wp_d_reached = false;
00226     wp_a_reached = false;
00227 }
00228 
00229 } //namespace