Changed home position assembly to prevent self collision, but allow for collision checking once robot is out of home position
Added set zero configuration after confirming it doubles angles; halve angles first, then set