Final Code

Code still crashes when deploying to cRIO