Emaxx Navigation code ported for MBED
Dependencies: BNO055_fusion Emaxx_Navigation_Dynamic_HIL MODSERIAL ServoIn ServoOut Vehicle_Model mbed
Fork of Emaxx_Navigation_Dynamic_HIL by
utilityFunctions.h
- Committer:
- jdawkins
- Date:
- 2018-04-03
- Revision:
- 9:3aa9b689bda5
- Parent:
- 7:a8c2e9d049e8
File content as of revision 9:3aa9b689bda5:
#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