Fixed a bunch of bugs
Fixed a bunch of bugs

* Fixed issue where theta was half of what it should be, causing arcs to be too short.
* Fixed intersection angle bug due to comparing prev_tc to itself.
* Fixed backward walk to be limited in depth. Hard-coded a constant for now, but eventually this should be user-controllable.
* Fixed debug output redundant definitions

araisrobo/machinekit,RunningLight/machinekit,ArcEye/MK-Qt5,bobvanderlinden/machinekit,Cid427/machinekit,ArcEye/MK-Qt5,unseenlaser/machinekit,Cid427/machinekit,strahlex/machinekit,EqAfrica/machinekit,unseenlaser/machinekit,RunningLight/machinekit,mhaberler/machinekit,kinsamanka/machinekit,ArcEye/MK-Qt5,EqAfrica/machinekit,RunningLight/machinekit,ArcEye/machinekit-testing,ArcEye/machinekit-testing,EqAfrica/machinekit,Cid427/machinekit,kinsamanka/machinekit,unseenlaser/machinekit,araisrobo/machinekit,araisrobo/machinekit,ArcEye/machinekit-testing,EqAfrica/machinekit,ArcEye/MK-Qt5,EqAfrica/machinekit,kinsamanka/machinekit,ArcEye/MK-Qt5,ArcEye/MK-Qt5,araisrobo/machinekit,araisrobo/machinekit,bobvanderlinden/machinekit,ArcEye/machinekit-testing,ArcEye/MK-Qt5,RunningLight/machinekit,bobvanderlinden/machinekit,kinsamanka/machinekit,strahlex/machinekit,kinsamanka/machinekit,unseenlaser/machinekit,Cid427/machinekit,ArcEye/MK-Qt5,unseenlaser/machinekit,strahlex/machinekit,araisrobo/machinekit,RunningLight/machinekit,mhaberler/machinekit,bobvanderlinden/machinekit,EqAfrica/machinekit,bobvanderlinden/machinekit,mhaberler/machinekit,mhaberler/machinekit,araisrobo/machinekit,RunningLight/machinekit,kinsamanka/machinekit,RunningLight/machinekit,bobvanderlinden/machinekit,Cid427/machinekit,strahlex/machinekit,araisrobo/machinekit,unseenlaser/machinekit,araisrobo/machinekit,mhaberler/machinekit,strahlex/machinekit,EqAfrica/machinekit,bobvanderlinden/machinekit,mhaberler/machinekit,Cid427/machinekit,unseenlaser/machinekit,bobvanderlinden/machinekit,ArcEye/machinekit-testing,ArcEye/machinekit-testing,RunningLight/machinekit,ArcEye/machinekit-testing,Cid427/machinekit,kinsamanka/machinekit,Cid427/machinekit,strahlex/machinekit,kinsamanka/machinekit,mhaberler/machinekit,ArcEye/machinekit-testing,strahlex/machinekit,unseenlaser/machinekit,mhaberler/machinekit,EqAfrica/machinekit