Fork of Emaxx Navigation code with dynamic vehicle model and HIL support. Strange bugs with mbed locking up upon receiving certain messages.
Dependencies: BNO055_fusion Vehicle_Model MODSERIAL ServoIn ServoOut mbed
Fork of Emaxx_Navigation by
utilityFunctions.h
- Committer:
- jdawkins
- Date:
- 2016-12-16
- Revision:
- 7:a8c2e9d049e8
File content as of revision 7:a8c2e9d049e8:
#ifndef UTIL_FUNC_H #define UTIL_FUNC_H #define Pi (3.14159) #define DEG2RAD (Pi/180) #define RAD2DEG (180/Pi) float saturateCmd(float cmd, float max,float min) { if(cmd>max) { cmd = max; } if(cmd < min) { cmd = min; } return cmd; } float saturateCmd(float cmd) { if(cmd>1.0) { cmd = 1.0; } if(cmd < -1.0) { cmd = -1.0; } return cmd; } float wrapToPi(float ang) { while(ang > Pi) { ang = ang - 2*Pi; } while(ang < -Pi) { ang = ang + 2*Pi; } return ang; } #endif