Skip to content

Instantly share code, notes, and snippets.

Created Jun 16, 2016
Embed
What would you like to do?
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);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment