fixed errors in setlatched.
Merge branch 'develop' of https://github.com/2729StormRobotics/Storm2014 into develop