Changed Test Program

Changed main_teleop body and movement_test configs
fixed math.c

Still does not work