Potential fields

potential_fields.cpp

#include "config.h"
#include "classes.h"
#include "iostream"

    emc::IO io;
    DriveControl pico_drive(&io);

Forces potential_fields(emc::LaserData* laser, float x_setpoint, float y_setpoint, bool turning)
{

    float F_attract_x = x_setpoint;
    float F_attract_y = y_setpoint;

    float F_repulse = 0, F_repulse_x = 0, F_repulse_y = 0;
    float F_total_x = 0, F_total_y = 0;

    float dist = 0;

    for(int i = 0; i < laser->ranges.size(); i++) {
        dist = laser->ranges[i];

        float theta = (67.5 * (laser->angle_min + laser->angle_increment * (float)i)) * M_PI / 180;

        if (theta <= 0) {
            theta += M_PI;
        } else if (theta > 0) {
            theta -= M_PI;
        }

        if ( (dist >= laser->range_min) &&  (dist <= 0.5) ){

            F_repulse = 1 / (1375 * pow(dist, 3) - 232.5*pow(dist, 2) + 12.19*dist);

            F_repulse_x += F_repulse * cos(theta);
            F_repulse_y += F_repulse * sin(theta);
        }
    }
    F_total_x = F_attract_x + 0.1*F_repulse_x;
    if (turning){
        F_total_y = F_attract_y + 0.5*F_repulse_y;
    }
    else{
        F_total_y = F_attract_y + 0.01*F_repulse_y;
    }

    float phi = 0;
    if (F_total_x != 0) {
        phi = 0.4*atan2(F_total_y, F_total_x);
    }

    float v_x; v_x = F_total_x;
    float v_y; v_y = F_total_y;

    float v_trans_max = MAX_TRANS_VELOCITY;
    float v_rot_max = MAX_ROT_VELOCITY;

    if (v_x > v_trans_max) {
        v_x = v_trans_max;
    }
    else if (-v_x > v_trans_max) {
        v_x = -v_trans_max;
    }

    if (v_y > v_trans_max) {
        v_y = v_trans_max;
    }
    else if (-v_y > v_trans_max) {
        v_y = -v_trans_max;
    }

    if (phi > v_rot_max) {
        phi = v_rot_max;
    }
    else if (-phi > v_trans_max) {
        phi = -v_trans_max;
    }

    Forces forces;
    forces.F_a.end.x = F_attract_x;
    forces.F_a.end.y = F_attract_y;
    forces.F_r.end.x = 0.1*F_repulse_x;
    forces.F_r.end.y = 0.01*F_repulse_y;
    forces.F_t.end.x = F_total_x;
    forces.F_t.end.y = F_total_y;

    if (turning){
    io.sendBaseReference(v_x, 0.75*v_y, 3*phi);
    forces.F_t.end.y = 0.75*v_y;

    }
    else{
    io.sendBaseReference(v_x, v_y, phi);
    }

    return forces;

}