File content as of revision 30:44676e1b38f8:

#include "purePursuit.h"

purePursuit::purePursuit()
{

}


void purePursuit::GenVW(float* purePursuit_velocity, float* purePursuit_omega,
                        float target_waypoint[2], float target_velocity,
                        float current_position[2], float current_heading)
{
    //rotate world to robot frame
    robotFrame_targetPos[0] = target_waypoint[0] * cos(current_heading) - target_waypoint[1] * sin(current_heading);
    robotFrame_targetPos[1] = target_waypoint[0] * sin(current_heading) + target_waypoint[1] * cos(current_heading);

    //distance to go
    robotFrame_targetDistance= sqrt(pow((target_waypoint[0] - current_position[0]),2) + pow((target_waypoint[1] - current_position[1]),2));

    //target_velocity velocity in mm/s

    //pure pursuit look ahead distance
    purePursuit_ld = generalFunctions::map_f(robotFrame_targetDistance, ld_min, ld_max, trackingLimit, ld_max);
    purePursuit_ld = generalFunctions::constrain_f(purePursuit_ld, trackingLimit, ld_max);

    //pure-pursuit tracking path radius
    purePursuit_r = pow(purePursuit_ld, 2) / (2 * (robotFrame_targetPos[0] - current_position[0]) );

    //calculate heading error
    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

    //add robot heading to compute global error
    purePursuit_headingE -= current_heading;

    //convert angle to 0 to 2*pi
    if(purePursuit_headingE > 2*M_PI)   purePursuit_headingE -= 2*M_PI;
    if(purePursuit_headingE < 0)        purePursuit_headingE += 2*M_PI;

    //convert back heading error into -pi -> +pi  from 0 to 2*pi
    if(purePursuit_headingE > M_PI)   purePursuit_headingE -= 2*M_PI;

    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
        //rotate towards the target heading on the spot
        *purePursuit_velocity = 150.0;//50.0;
        *purePursuit_omega = generalFunctions::sign_f(purePursuit_headingE)*1.0*M_PI;
    }

    else {
        if(robotFrame_targetDistance < trackingLimit) {
            *purePursuit_velocity = 0;
            *purePursuit_omega = 0;
        }

        if(robotFrame_targetDistance >= trackingLimit) {
            //regulate velocity depending on distance to be travelled
            *purePursuit_velocity = generalFunctions::map_f(robotFrame_targetDistance, trackingLimit, ld_max, drive_minV, target_velocity);
            *purePursuit_velocity = generalFunctions::constrain_f(*purePursuit_velocity, drive_minV, drive_maxV);

            //omega calculated from generated path radius and command velocity
            //*purePursuit_omega = *purePursuit_velocity / purePursuit_r;


            //omega calculated from heading error and command velocity
            *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.


            //check to see if omega calculated is too high
            if(generalFunctions::abs_f(*purePursuit_omega) > purePursuit_omega_limit) {
                *purePursuit_velocity= *purePursuit_velocity * (purePursuit_omega_limit/generalFunctions::abs_f(*purePursuit_omega)); //scale speed to keep omega within normal limits
                //purePursuit_omega = 1.0*purePursuit_headingE * (*purePursuit_velocity / robotFrame_targetDistance);
            }
        }
    }
}


void purePursuit:: GenVGW(float* purePursuit_velocity, float* purePursuit_gamma, float* purePursuit_omega,
                          float target_waypoint[2], float target_velocity,
                          float current_position[2], float current_heading)
{
    float gamma_out;
    float gamma_error;

    //distance to go
    robotFrame_targetDistance= sqrt(pow((target_waypoint[0] - current_position[0]),2) + pow((target_waypoint[1] - current_position[1]),2));

    //target_velocity velocity in mm/s

    //pure pursuit look ahead distance
    purePursuit_ld = generalFunctions::map_f(robotFrame_targetDistance, ld_min, ld_max, trackingLimit, ld_max);
    purePursuit_ld = generalFunctions::constrain_f(purePursuit_ld, trackingLimit, ld_max);


    //calculate global heading to track, frame needs to be rotated by pi/2 rad for compilmentry angle about measured from Y axis
    gamma_out = atan2((target_waypoint[1] - current_position[1]) , (target_waypoint[0] - current_position[0])) - (M_PI/2);

    //convert angle to 0 to 2*pi
    if(gamma_out > 2*M_PI)   gamma_out -= 2*M_PI;
    if(gamma_out < 0)        gamma_out += 2*M_PI;
    
    ////convert back heading error into -pi -> +pi  from 0 to 2*pi
    //if(gamma_out > M_PI)   gamma_out -= 2*M_PI;
    
    gamma_error = gamma_out - current_heading;
    //convert heading error into -pi -> +pi 
    if(gamma_error < -1*M_PI)   gamma_error += 2*M_PI;
    if(gamma_error > M_PI)   gamma_error -= 2*M_PI;
    
    
    if(robotFrame_targetDistance < trackingLimit) {
        *purePursuit_velocity = 0;
    }

    else {
        //regulate velocity depending on distance to be travelled
        *purePursuit_velocity = generalFunctions::map_f(robotFrame_targetDistance, trackingLimit, ld_max, drive_minV, target_velocity);
        *purePursuit_velocity = generalFunctions::constrain_f(*purePursuit_velocity, drive_minV, drive_maxV);

        //omega calculated from heading error and command velocity
        *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.


        //check to see if omega calculated is too high
        if(generalFunctions::abs_f(*purePursuit_omega) > purePursuit_omega_limit) {
            *purePursuit_velocity= *purePursuit_velocity * (purePursuit_omega_limit/generalFunctions::abs_f(*purePursuit_omega)); //scale speed to keep omega within normal limits
        }
        
        //convert angle to 0 to 2*pi
        if(gamma_out > 2*M_PI)   gamma_out -= 2*M_PI;
        if(gamma_out < 0)        gamma_out += 2*M_PI;

        *purePursuit_gamma = gamma_out;
    }
}