Added constrain and map functions, fixed gyro code