-
-
Save anonymous/6a277e705f8080349451f8360e67ea76 to your computer and use it in GitHub Desktop.
Potential fields
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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; // Attraction force in the x-direction | |
float F_attract_y = y_setpoint; // Attraction force in the y-direction | |
float F_repulse = 0, F_repulse_x = 0, F_repulse_y = 0; // Initiate some values | |
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]; // Distance of the laserbeam | |
float theta = (67.5 * (laser->angle_min + laser->angle_increment * (float)i)) * M_PI / 180; // Calculate angle of laser beam i | |
if (theta <= 0) { // Pi-correction | |
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); // Calculate the repulsive forces with a fine-tuned formula | |
F_repulse_x += F_repulse * cos(theta); // Calculate the repuslive force in the x-direction | |
F_repulse_y += F_repulse * sin(theta); // Calculate the repuslive force in the y-direction | |
} | |
} | |
F_total_x = F_attract_x + 0.1*F_repulse_x; | |
if (turning){ // Tweak some forces of PICO is turning | |
F_total_y = F_attract_y + 0.5*F_repulse_y; | |
} | |
else{ // Tweak some forces of PICO is not turning | |
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) { // Maximize the velocities | |
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; // Create a forces class for the visualization | |
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){ // Tweak some final values if PICO is 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; // Return the forces class for the visualization | |
} | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment