Push for Students
Dependencies: BNO055_fusion Madpulse ServoIn ServoOut mavlink_emaxx mbed
Fork of Madpulse_Speed_Control_temp by
utilityFunctions.h@0:daea75c21ac1, 2017-11-14 (annotated)
- Committer:
- jdawkins
- Date:
- Tue Nov 14 13:51:29 2017 +0000
- Revision:
- 0:daea75c21ac1
Initial commit of Madpulse Control Instructors Solution
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jdawkins | 0:daea75c21ac1 | 1 | #ifndef UTIL_FUNC_H |
jdawkins | 0:daea75c21ac1 | 2 | #define UTIL_FUNC_H |
jdawkins | 0:daea75c21ac1 | 3 | |
jdawkins | 0:daea75c21ac1 | 4 | #define Pi (3.14159) |
jdawkins | 0:daea75c21ac1 | 5 | #define DEG2RAD (Pi/180) |
jdawkins | 0:daea75c21ac1 | 6 | #define RAD2DEG (180/Pi) |
jdawkins | 0:daea75c21ac1 | 7 | inline float saturateVal(float cmd, float max,float min) |
jdawkins | 0:daea75c21ac1 | 8 | { |
jdawkins | 0:daea75c21ac1 | 9 | if(cmd>max) { |
jdawkins | 0:daea75c21ac1 | 10 | cmd = max; |
jdawkins | 0:daea75c21ac1 | 11 | } |
jdawkins | 0:daea75c21ac1 | 12 | if(cmd < min) { |
jdawkins | 0:daea75c21ac1 | 13 | cmd = min; |
jdawkins | 0:daea75c21ac1 | 14 | } |
jdawkins | 0:daea75c21ac1 | 15 | return cmd; |
jdawkins | 0:daea75c21ac1 | 16 | } |
jdawkins | 0:daea75c21ac1 | 17 | inline float saturateCmd(float cmd) |
jdawkins | 0:daea75c21ac1 | 18 | { |
jdawkins | 0:daea75c21ac1 | 19 | if(cmd>1.0) { |
jdawkins | 0:daea75c21ac1 | 20 | cmd = 1.0; |
jdawkins | 0:daea75c21ac1 | 21 | } |
jdawkins | 0:daea75c21ac1 | 22 | if(cmd < -1.0) { |
jdawkins | 0:daea75c21ac1 | 23 | cmd = -1.0; |
jdawkins | 0:daea75c21ac1 | 24 | } |
jdawkins | 0:daea75c21ac1 | 25 | return cmd; |
jdawkins | 0:daea75c21ac1 | 26 | } |
jdawkins | 0:daea75c21ac1 | 27 | inline float wrapToPi(float ang) |
jdawkins | 0:daea75c21ac1 | 28 | { |
jdawkins | 0:daea75c21ac1 | 29 | |
jdawkins | 0:daea75c21ac1 | 30 | while(ang > Pi) { |
jdawkins | 0:daea75c21ac1 | 31 | |
jdawkins | 0:daea75c21ac1 | 32 | ang = ang - 2*Pi; |
jdawkins | 0:daea75c21ac1 | 33 | } |
jdawkins | 0:daea75c21ac1 | 34 | while(ang < -Pi) { |
jdawkins | 0:daea75c21ac1 | 35 | ang = ang + 2*Pi; |
jdawkins | 0:daea75c21ac1 | 36 | } |
jdawkins | 0:daea75c21ac1 | 37 | |
jdawkins | 0:daea75c21ac1 | 38 | return ang; |
jdawkins | 0:daea75c21ac1 | 39 | } |
jdawkins | 0:daea75c21ac1 | 40 | //float norm3D(float x,float y,float z){ |
jdawkins | 0:daea75c21ac1 | 41 | |
jdawkins | 0:daea75c21ac1 | 42 | //return sqrt((double)(x*x) + (double)(y*y) + (double)(z*z)); |
jdawkins | 0:daea75c21ac1 | 43 | //return sqrt(0.2); |
jdawkins | 0:daea75c21ac1 | 44 | //} |
jdawkins | 0:daea75c21ac1 | 45 | |
jdawkins | 0:daea75c21ac1 | 46 | |
jdawkins | 0:daea75c21ac1 | 47 | |
jdawkins | 0:daea75c21ac1 | 48 | inline float firstOrderFilter(float var_new, float var_old,float dt,float tau){ |
jdawkins | 0:daea75c21ac1 | 49 | |
jdawkins | 0:daea75c21ac1 | 50 | float Kdc = 1.0; |
jdawkins | 0:daea75c21ac1 | 51 | float var_dot = -(1/tau)*var_old + (Kdc/tau)*var_new; // first order filter |
jdawkins | 0:daea75c21ac1 | 52 | float var_out = var_old + var_dot*dt; |
jdawkins | 0:daea75c21ac1 | 53 | |
jdawkins | 0:daea75c21ac1 | 54 | return var_out; |
jdawkins | 0:daea75c21ac1 | 55 | } |
jdawkins | 0:daea75c21ac1 | 56 | |
jdawkins | 0:daea75c21ac1 | 57 | #endif |