purePursuit.cpp

00001 #include "purePursuit.h"
00002 
00003 purePursuit::purePursuit()
00004 {
00005 
00006 }
00007 
00008 
00009 void purePursuit::GenVW(float* purePursuit_velocity, float* purePursuit_omega,
00010                         float target_waypoint[2], float target_velocity,
00011                         float current_position[2], float current_heading)
00012 {
00013     //rotate world to robot frame
00014     robotFrame_targetPos[0] = target_waypoint[0] * cos(current_heading) - target_waypoint[1] * sin(current_heading);
00015     robotFrame_targetPos[1] = target_waypoint[0] * sin(current_heading) + target_waypoint[1] * cos(current_heading);
00016 
00017     //distance to go
00018     robotFrame_targetDistance= sqrt(pow((target_waypoint[0] - current_position[0]),2) + pow((target_waypoint[1] - current_position[1]),2));
00019 
00020     //target_velocity velocity in mm/s
00021 
00022     //pure pursuit look ahead distance
00023     purePursuit_ld = generalFunctions::map_f(robotFrame_targetDistance, ld_min, ld_max, trackingLimit, ld_max);
00024     purePursuit_ld = generalFunctions::constrain_f(purePursuit_ld, trackingLimit, ld_max);
00025 
00026     //pure-pursuit tracking path radius
00027     purePursuit_r = pow(purePursuit_ld, 2) / (2 * (robotFrame_targetPos[0] - current_position[0]) );
00028 
00029     //calculate heading error
00030     purePursuit_headingE = atan2((target_waypoint[1] - current_position[1]) , (target_waypoint[0] - current_position[0])) - (M_PI/2); //frame rotated by pi/2 rad, compilmentry angle about measured from Y axis
00031 
00032     //add robot heading to compute global error
00033     purePursuit_headingE -= current_heading;
00034 
00035     //convert angle to 0 to 2*pi
00036     if(purePursuit_headingE > 2*M_PI)   purePursuit_headingE -= 2*M_PI;
00037     if(purePursuit_headingE < 0)        purePursuit_headingE += 2*M_PI;
00038 
00039     //convert back heading error into -pi -> +pi  from 0 to 2*pi
00040     if(purePursuit_headingE > M_PI)   purePursuit_headingE -= 2*M_PI;
00041 
00042     if((generalFunctions::abs_f(purePursuit_headingE) >= M_PI/2) && (robotFrame_targetDistance > trackingLimit)) { //pure pursuit-implementation requires heading error to be under +/- 90 degree to work efficiently, lets bring it down to within 80 deg using zero turn radius
00043         //rotate towards the target heading on the spot
00044         *purePursuit_velocity = 150.0;//50.0;
00045         *purePursuit_omega = generalFunctions::sign_f(purePursuit_headingE)*1.0*M_PI;
00046     }
00047 
00048     else {
00049         if(robotFrame_targetDistance < trackingLimit) {
00050             *purePursuit_velocity = 0;
00051             *purePursuit_omega = 0;
00052         }
00053 
00054         if(robotFrame_targetDistance >= trackingLimit) {
00055             //regulate velocity depending on distance to be travelled
00056             *purePursuit_velocity = generalFunctions::map_f(robotFrame_targetDistance, trackingLimit, ld_max, drive_minV, target_velocity);
00057             *purePursuit_velocity = generalFunctions::constrain_f(*purePursuit_velocity, drive_minV, drive_maxV);
00058 
00059             //omega calculated from generated path radius and command velocity
00060             //*purePursuit_omega = *purePursuit_velocity / purePursuit_r;
00061 
00062 
00063             //omega calculated from heading error and command velocity
00064             *purePursuit_omega = 10.0*purePursuit_headingE * (*purePursuit_velocity / robotFrame_targetDistance); // for very large command distances, small heading errors cause huge amplification on omega, and sending the robot into oscillations. Hence an adaptive scaler to check this condition.
00065 
00066 
00067             //check to see if omega calculated is too high
00068             if(generalFunctions::abs_f(*purePursuit_omega) > purePursuit_omega_limit) {
00069                 *purePursuit_velocity= *purePursuit_velocity * (purePursuit_omega_limit/generalFunctions::abs_f(*purePursuit_omega)); //scale speed to keep omega within normal limits
00070                 //purePursuit_omega = 1.0*purePursuit_headingE * (*purePursuit_velocity / robotFrame_targetDistance);
00071             }
00072         }
00073     }
00074 }
00075 
00076 
00077 void purePursuit:: GenVGW(float* purePursuit_velocity, float* purePursuit_gamma, float* purePursuit_omega,
00078                           float target_waypoint[2], float target_velocity,
00079                           float current_position[2], float current_heading)
00080 {
00081     float gamma_out;
00082     float gamma_error;
00083 
00084     //distance to go
00085     robotFrame_targetDistance= sqrt(pow((target_waypoint[0] - current_position[0]),2) + pow((target_waypoint[1] - current_position[1]),2));
00086 
00087     //target_velocity velocity in mm/s
00088 
00089     //pure pursuit look ahead distance
00090     purePursuit_ld = generalFunctions::map_f(robotFrame_targetDistance, ld_min, ld_max, trackingLimit, ld_max);
00091     purePursuit_ld = generalFunctions::constrain_f(purePursuit_ld, trackingLimit, ld_max);
00092 
00093 
00094     //calculate global heading to track, frame needs to be rotated by pi/2 rad for compilmentry angle about measured from Y axis
00095     gamma_out = atan2((target_waypoint[1] - current_position[1]) , (target_waypoint[0] - current_position[0])) - (M_PI/2);
00096 
00097     //convert angle to 0 to 2*pi
00098     if(gamma_out > 2*M_PI)   gamma_out -= 2*M_PI;
00099     if(gamma_out < 0)        gamma_out += 2*M_PI;
00100     
00101     ////convert back heading error into -pi -> +pi  from 0 to 2*pi
00102     //if(gamma_out > M_PI)   gamma_out -= 2*M_PI;
00103     
00104     gamma_error = gamma_out - current_heading;
00105     //convert heading error into -pi -> +pi 
00106     if(gamma_error < -1*M_PI)   gamma_error += 2*M_PI;
00107     if(gamma_error > M_PI)   gamma_error -= 2*M_PI;
00108     
00109     
00110     if(robotFrame_targetDistance < trackingLimit) {
00111         *purePursuit_velocity = 0;
00112     }
00113 
00114     else {
00115         //regulate velocity depending on distance to be travelled
00116         *purePursuit_velocity = generalFunctions::map_f(robotFrame_targetDistance, trackingLimit, ld_max, drive_minV, target_velocity);
00117         *purePursuit_velocity = generalFunctions::constrain_f(*purePursuit_velocity, drive_minV, drive_maxV);
00118 
00119         //omega calculated from heading error and command velocity
00120         *purePursuit_omega = 1.0*(gamma_error) * (*purePursuit_velocity / robotFrame_targetDistance); // for very large command distances, small heading errors cause huge amplification on omega, and sending the robot into oscillations. Hence an adaptive scaler to check this condition.
00121 
00122 
00123         //check to see if omega calculated is too high
00124         if(generalFunctions::abs_f(*purePursuit_omega) > purePursuit_omega_limit) {
00125             *purePursuit_velocity= *purePursuit_velocity * (purePursuit_omega_limit/generalFunctions::abs_f(*purePursuit_omega)); //scale speed to keep omega within normal limits
00126         }
00127         
00128         //convert angle to 0 to 2*pi
00129         if(gamma_out > 2*M_PI)   gamma_out -= 2*M_PI;
00130         if(gamma_out < 0)        gamma_out += 2*M_PI;
00131 
00132         *purePursuit_gamma = gamma_out;
00133     }
00134 }