Leg Collisions

To prevent leg collisions I will be implementing this check.  If you assume that the top leg wants to swing down, then we need to find theta 1 which is the maximum allowable angle before a collision occurs.