USNA WSE ES456 / Mbed 2 deprecated Madpulse_Speed_Control_temp

Push for Students

Dependencies:   BNO055_fusion Madpulse ServoIn ServoOut mavlink_emaxx mbed

Fork of Madpulse_Control_Fall2017 by USNA WSE ES456

utilityFunctions.h@1:f4c9926fb4c9, 2017-11-15 (annotated)

Committer:
jdawkins
Date:
Wed Nov 15 18:36:30 2017 +0000
Revision:
1:f4c9926fb4c9
Parent:
0:daea75c21ac1 
Committ for class

Who changed what in which revision?

UserRevisionLine numberNew 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