Teleop_v111

*Major issues, if else needed
*Update psuedocode and documentation