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