🐛[BUG] - Path smoothing forces points to come close to the robot

[ssejrog]

Expected Behavior

Because of #152, we expect no excess points to be places within look ahead of the robot.

Actual Behavior

If the user puts a point within look ahead of the robot, path smoothing will pull points into this radius.

Steps to Reproduce

Run this and check terminal, ensure that look ahead is larger than 6. Target points will be wihtin the look ahead distance.

  chassis.pid_odom_set({{{0_in, 6_in}, fwd, 110},
                        {{0_in, 12_in}, fwd, 110},
                        {{0_in, 24_in}, fwd, 110}},
                       true);
  chassis.pid_wait();

Versions

PROS Kernel Version:
EZ-Template Version:
Other Libraries:
EZ-Template 3.2.0-beta.6
Kernel 4.1.1

OPTIONAL: Additional Information