potential_fields_snippet

potential_fields

void potential_fields() {
	// these are the coordinates of the setpoint and the current position of the robot
	// normally xr should be zero, equal to x0 (origin right in the center of the corridor)
	// and y0 must have a value

	float theta, phi, r = 1, Fatt_x, Fatt_y, Ftot_x, Ftot_y, Frep_x = 0, Frep_y = 0, Frep = 0, xr = 0, yr = 0, xs = 0.1, ys = 0;

	Fatt_x = ws * (xs - xr);
	Fatt_y = ws * (ys - yr);

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

		theta = 67.5*(scan.angle_min + scan.angle_increment*i)*pi / 180;

		// calculate the angle so that the repelling forces have the opposite direction of the obstacles
		if (theta <= 0) phi = pi + theta;
		else if (theta > 0) phi = theta - pi;

		if (scan.ranges[i] > 0) { // avoid division with 0
			r = scan.ranges[i];
			Frep = wo / (1375 * pow(r, 3) - 232.5*pow(r, 2) + 12.19*r);
			Frep_x += Frep * cos(phi);
			Frep_y += Frep * sin(phi);
		}

	}

	// sum of attractive and repulsive forces
	Ftot_x = Fatt_x + Frep_x;
	Ftot_y = Fatt_y + Frep_y;

	// Rotational speed calculation
	if (Ftot_x != 0) phi = atan2(Ftot_y, Ftot_x); // in radians
												  //    cout << "phi = " << phi << endl;

												  // calculate the total angle phi and the robot must turn until it faces the sum of the forces
	w = w_w * phi;

	vx = w_vx * Ftot_x;
	vy = w_vy * Ftot_y;

	// saturate the velocities
	if (vx >= max_v) vx = max_v;
	else if (vx <= -max_v) vx = -max_v;
	if (vy >= max_v) vy = max_v;
	else if (vy <= -max_v) vy = -max_v;
	if (w >= max_w) w = max_w;
	else if (w <= -max_w) w = -max_w;


}