potentialFieldFunction.cpp

void movement::potFunction(emc::IO &io, emc::LaserData scan, Point target) {

    for (int i=1;i<scan.ranges.size();i++){ //Avoid infinity measurements
        if (fabs(scan.ranges[i])<0.01){
            scan.ranges[i]=10;
        }
    }

    double x_force = 0;
    double y_force = 0;

    double r_m = 1; //scaling constant
    double resultant = 0;

    double v_x = 0;
    double v_y = 0;
    double v_a = 0;

    double radius, absolute_v;
    double targetForce = 3; // Higher means pico is more attracted to the target to the target
    double rotFactor = 0.5; // Higher means pico is more eager to rotate


    double total = 0;
    double minimum = 10;
    double minimumAng = 0;
    double correctY = 0;

    double pi = 3.14159;

    // Normalize target
    radius = sqrt(pow(target.x,2)+pow(target.y,2));
    target.x = target.x/radius;
    target.y = target.y/radius;

    // calculate the direction of the target
    // transform to laser point
    // cumulative x and y vector calculation

    for(unsigned int i = 0; i < scan.ranges.size(); ++i) {

        if (scan.ranges[i]<0.3){ // if there is a measurement point within 30 cm
            x_force = x_force + ( 1/(r_m*pow( (scan.ranges[i]-0.05) ,5)) ) * cos(scan.angle_min + i*scan.angle_increment); // Add force in the x direction
            y_force = y_force + ( 1/(r_m*pow( (scan.ranges[i]-0.05) ,5)) ) * sin(scan.angle_min + i*scan.angle_increment); // Add force in the y direction
            total++; // Keep track of how many points are detected in the 30 cm range
        }

        if (i<(scan.ranges.size()/2)){ //look on the right side of PICO for wall following
            if (scan.ranges[i]<minimum){ // keep track of where the minimum value is detected and what size this is
                minimumAng = scan.angle_min + i*scan.angle_increment;
                minimum = scan.ranges[i];
            }
        }
    }


    if (total>0){ // if there was anything within the 30 cm range

        resultant = -y_force/fabs(x_force)*rotFactor + target.y/fabs(target.x); // calculate the rotation
        std::cout << "yforce " << y_force << std::endl;
        x_force = target.x*targetForce - (x_force/total); // Compute the total force in x direction including target
        y_force = target.y*targetForce - (y_force/total); // Compute the total force in x direction including target

    }

    else{

        if (target.y == 0){ // target.y is only practically exactly 0 if PICO is not going towards a target
            if (fabs(minimumAng+1.57)>0.1){ //adjust the angle to keep perpendicular to the minimum on the wall
                resultant = (minimumAng+1.57)*10;
                //std::cout << "minimum " << minimum << std::endl;
            }
            if ((0.3>minimum)||(0.4<minimum)) { // move towards the wall if too far away and away from the wall if too close
                correctY = (0.4-minimum)*0.25;
                //std::cout << "correctY " << correctY << std::endl;
            }
        }
        else{ //ensures rotating towards the target in any case
            resultant = target.y/fabs(target.x);
        }
        x_force = target.x*targetForce;
        y_force = target.y*targetForce;
    }

    v_a = resultant;
    v_y = y_force;
    v_x = x_force;

    // Normalizing the rotation
    if (fabs(v_a)>1.2)
        v_a = 1.2*fabs(v_a)/v_a; // rotation is 1.2 times the direction

    absolute_v = sqrt(pow(v_y,2)+pow(v_x,2));
    if (absolute_v>0.5){ // normalizing the speed
        v_y = 0.5*v_y/absolute_v;
        v_x = 0.5*v_x/absolute_v;
    }

    // set speed
    io.sendBaseReference( v_x, correctY, v_a);
}