Rearranged Debug section, removed Diegos section from arm

moved Diegos output to motorController from arm
~ activated by using simulation mode
~ Motor controller now keeps track of current position

Debug parts have been moved to two sections in arm
~ 1 if statement in the waypoint loop
~ 1 if statement at the end of Autonomous function