Added fully functioning control of robot.
Deleted camera code (after making a branch for it), and reprogrammed the arms to work with new setup.
Cleaned up code, made trigger a break, added acceleration smoothing