Robotics Fundamentals 1, Chapter 8: Challenge 2 solution (line following, obstacle avoidance, wall following and homing)

Formulas: line follower:  e = -x ;  u = Kp e ;  v = vmax / ( 1 + c |e| ) ;  vR = v + u L / 2 ,  vL = v - u L / 2
          potential field:  F(rep) = k ( 1/d - 1/d0 ) / d^2  for d < d0 ;  F(att) = ka x ( distance to the goal )
          wall following (wall on the right):  theta = atan( ( dF - dR ) / s ) ;  d(perp) = ( ( dF + dR ) / 2 ) cos( theta ) ;  u = -( Kp ( d(perp) - d(ref) ) + Ktheta theta )
          homing:  w = Kb beta ;  v = min( vmax , kv r )

(a) vmax = 0.40, c = 0.04 per mm, Kp = 0.06 rad/s per mm, L = 0.18 m, line at x = +15 mm (to the right).
        e = -15 mm
        u = 0.06 x ( -15 ) = -0.90 rad/s                          clockwise: the robot turns RIGHT, toward the line
        v = 0.40 / ( 1 + 0.04 x 15 ) = 0.40 / 1.6 = 0.25 m/s
        vR = 0.25 + ( -0.90 ) x 0.09 = 0.25 - 0.081 = 0.169 m/s        vL = 0.25 - ( -0.90 ) x 0.09 = 0.25 + 0.081 = 0.331 m/s      (the left wheel faster: the robot swings right)
    Line lost, last seen at x = +15 mm (the right), so search toward the right: clockwise at 1.0 rad/s.
        Turn in 3 s = 1.0 x 3 = 3.0 rad = 3.0 x 57.296 = 171.9 degrees, clockwise, before the 3 s limit sends the machine to STOP.
        (Less than half a turn: a line that left the array on the right is almost always found within that angle, if it is there.)

(b) k = 0.02, d0 = 0.60 m, ka = 0.30.  The robot faces +x, y is positive to the left.
        Goal 1.5 m ahead:        F(att) = 0.30 x 1.5 = 0.450 along +x
        Obstacle A, d = 0.30 m, straight ahead:   F = 0.02 x ( 1/0.30 - 1/0.60 ) / 0.30^2 = 0.02 x ( 3.333 - 1.667 ) / 0.09 = 0.02 x 18.52 = 0.370, pushing along -x
        Obstacle B, d = 0.45 m, to the RIGHT (at y = -0.45):   F = 0.02 x ( 2.222 - 1.667 ) / 0.2025 = 0.02 x 2.743 = 0.0549, pushing away from it: along +y (to the left)
        Sum:   Fx = 0.450 - 0.370 = +0.0796        Fy = +0.0549
        Size = sqrt( 0.0796^2 + 0.0549^2 ) = sqrt( 0.006336 + 0.003014 ) = sqrt( 0.009350 ) = 0.0967
        Direction = atan2( 0.0549, 0.0796 ) = +34.6 degrees:  34.6 degrees to the LEFT of straight ahead, so the robot veers left (away from B, round A).
        (The two forces on the x axis nearly cancel, 0.450 against 0.370: with an obstacle a little nearer, or a goal a little closer, they would cancel completely and the robot would stall in a local minimum.)

(c) Wall on the right, d(ref) = 0.35 m, s = 0.12 m, dF = 0.28, dR = 0.36, Kp = 2.5, Ktheta = 1.2, v = 0.20, L = 0.14 (so L/2 = 0.07).
        theta = atan( ( 0.28 - 0.36 ) / 0.12 ) = atan( -0.6667 ) = -33.69 degrees = -0.5880 rad
              negative: the FRONT is CLOSER to the wall than the rear, so the robot is heading TOWARD the wall.
        d(perp) = ( ( 0.28 + 0.36 ) / 2 ) x cos( 33.69 degrees ) = 0.32 x 0.8321 = 0.2663 m
        error = 0.2663 - 0.35 = -0.0837 m                          (too close, and closing)
        u = -( 2.5 x ( -0.0837 ) + 1.2 x ( -0.5880 ) ) = -( -0.2094 - 0.7056 ) = +0.915 rad/s       counter-clockwise: turn LEFT, away from the wall on the right
        vR = 0.20 + 0.915 x 0.07 = 0.264 m/s       vL = 0.20 - 0.915 x 0.07 = 0.136 m/s              (the right wheel faster: the robot swings left)

(d) Kb = 1.2 per second, vmax = 0.20, kv = 0.4 per second; the beacon is 35 degrees to the right (beta = -35 degrees = -0.6109 rad), range 0.8 m.
        w = 1.2 x ( -0.6109 ) = -0.733 rad/s        clockwise: turn right, toward the beacon
        v = min( 0.20 , 0.4 x 0.8 = 0.32 ) = 0.20 m/s
        At range 0.2 m:  v = min( 0.20 , 0.4 x 0.2 = 0.08 ) = 0.08 m/s     (the robot slows for the last stretch)