// FILE: potField1_0.sce
// DATE: 11/21/16 13:11
// AUTH: P.Oh
// DESC: 1_1: implement potential field in Scilab

clear // clear all memory

// Variables
 iterations = 100; // total number of iterations
 T = 0.1; // sampling time [sec]
 xTarget = 10;  // Target location
 yTarget = 7;
 xObstacle = 5; // Obstable location
 yObstacle = 4;
 rho0 = 2; // repulsive field maximum
 nRepulse = 1; // repulsive field constant
 nAttract = 2; // attractive field constant
 t(1) = 0;
 rhoTarget(1) = 0;
 x(1) = 0;
 y(1) = 0;
 rho(1) = 0;

// Begin update

 for i=2:iterations  // Run algorithm for 100 sample periods
    t(i-1) = T*(i-1); // time in seconds
    // Distance to target (Eqn 5)
    rhoTarget(i-1) = sqrt(((yTarget-y(i-1))^2) + ((xTarget-x(i-1))^2));

   // Distance to an obstacle (Eqn 8)
   rho(i-1) = sqrt( ((yObstacle-y(i-1))^2) + ((xObstacle-x(i-1))^2) );
 
   // Calculate gradient (Eqns 11 and 13)
   if rho(i-1) < rho0 then
      // ur_x = nRepulse*(xObstacle-x(i-1))*((1/rho(i-1))-1/rho0))/(rho(i-1)^3);
      // ur_y = nRepulse*(yObstacle-y(i-1))*((1/rho(i-1))-(1/rho0))/(rho(i-1)^3);
      ur_x = nRepulse*( (1/rho(i-1))-(1/rho0) ) * (xObstacle - x(i-1)) / rho(i-1)^3;
      ur_y = nRepulse*( (1/rho(i-1))-(1/rho0) ) * (yObstacle - y(i-1)) / rho(i-1)^3;
      // ur_x = 0;
      // ur_y = 0;
   else
      ur_x = 0;
      ur_y = 0;
   end

   // Calculate new robot position (Eqns 15 and 16)
   x(i) = x(i-1) - T*nAttract*(x(i-1)-xTarget) - T*ur_x;
   y(i) = y(i-1) - T*nAttract*(y(i-1)-yTarget) - T*ur_y;
 
 end // end of for

 // Plot results
 clf; // refresh plot
 xlabel("Robot x position"); // caption x-axis
 ylabel("Robot y position"); // caption y-axis
 title("Potential Field example"); // title the plot
 plot(x(1), y(1), "ob"); // plot start position in blue circle
 plot(xTarget, yTarget, "og"); // plot target position in green circle
 plot(xObstacle, yObstacle, "xr"); // obstacles in red x
 plot(x,y, "k"); // plot solution path in black solid line
