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@8:9817993e5df7, 2017-04-07 (annotated)
- Committer:
- jdawkins
- Date:
- Fri Apr 07 17:12:29 2017 +0000
- Revision:
- 8:9817993e5df7
- Parent:
- 7:a8c2e9d049e8
Commit for Capstone Students
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jdawkins | 7:a8c2e9d049e8 | 1 | #ifndef UTIL_FUNC_H |
jdawkins | 7:a8c2e9d049e8 | 2 | #define UTIL_FUNC_H |
jdawkins | 7:a8c2e9d049e8 | 3 | |
jdawkins | 7:a8c2e9d049e8 | 4 | #define Pi (3.14159) |
jdawkins | 7:a8c2e9d049e8 | 5 | #define DEG2RAD (Pi/180) |
jdawkins | 7:a8c2e9d049e8 | 6 | #define RAD2DEG (180/Pi) |
jdawkins | 7:a8c2e9d049e8 | 7 | float saturateCmd(float cmd, float max,float min) |
jdawkins | 7:a8c2e9d049e8 | 8 | { |
jdawkins | 7:a8c2e9d049e8 | 9 | if(cmd>max) { |
jdawkins | 7:a8c2e9d049e8 | 10 | cmd = max; |
jdawkins | 7:a8c2e9d049e8 | 11 | } |
jdawkins | 7:a8c2e9d049e8 | 12 | if(cmd < min) { |
jdawkins | 7:a8c2e9d049e8 | 13 | cmd = min; |
jdawkins | 7:a8c2e9d049e8 | 14 | } |
jdawkins | 7:a8c2e9d049e8 | 15 | return cmd; |
jdawkins | 7:a8c2e9d049e8 | 16 | } |
jdawkins | 7:a8c2e9d049e8 | 17 | float saturateCmd(float cmd) |
jdawkins | 7:a8c2e9d049e8 | 18 | { |
jdawkins | 7:a8c2e9d049e8 | 19 | if(cmd>1.0) { |
jdawkins | 7:a8c2e9d049e8 | 20 | cmd = 1.0; |
jdawkins | 7:a8c2e9d049e8 | 21 | } |
jdawkins | 7:a8c2e9d049e8 | 22 | if(cmd < -1.0) { |
jdawkins | 7:a8c2e9d049e8 | 23 | cmd = -1.0; |
jdawkins | 7:a8c2e9d049e8 | 24 | } |
jdawkins | 7:a8c2e9d049e8 | 25 | return cmd; |
jdawkins | 7:a8c2e9d049e8 | 26 | } |
jdawkins | 7:a8c2e9d049e8 | 27 | float wrapToPi(float ang) |
jdawkins | 7:a8c2e9d049e8 | 28 | { |
jdawkins | 7:a8c2e9d049e8 | 29 | |
jdawkins | 7:a8c2e9d049e8 | 30 | while(ang > Pi) { |
jdawkins | 7:a8c2e9d049e8 | 31 | |
jdawkins | 7:a8c2e9d049e8 | 32 | ang = ang - 2*Pi; |
jdawkins | 7:a8c2e9d049e8 | 33 | } |
jdawkins | 7:a8c2e9d049e8 | 34 | while(ang < -Pi) { |
jdawkins | 7:a8c2e9d049e8 | 35 | ang = ang + 2*Pi; |
jdawkins | 7:a8c2e9d049e8 | 36 | } |
jdawkins | 7:a8c2e9d049e8 | 37 | |
jdawkins | 7:a8c2e9d049e8 | 38 | return ang; |
jdawkins | 7:a8c2e9d049e8 | 39 | } |
jdawkins | 7:a8c2e9d049e8 | 40 | |
jdawkins | 7:a8c2e9d049e8 | 41 | #endif |