Improved Test program, added new test program

Improved old test program to be best yet by playing with params. Made
new test program to implement diffrent tracking logic. rather than
determine direction, will change angle to decrease diff, if diff
increase go other way. Needs implemented