Martin Gurtner / HKC_MiniCheetah

Fork of the Ben Katz's motor controller firmware.

math_ops.h@59:8fb0145aa933, 2021-01-22 (annotated)

Committer:
MartinGurtner
Date:
Fri Jan 22 12:44:34 2021 +0000
Revision:
59:8fb0145aa933
Parent:
48:74a40481740c 
communication improved - floats are correctly converted to uints

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 20:bf9ea5125d52 1 #ifndef MATH_OPS_H
benkatz 20:bf9ea5125d52 2 #define MATH_OPS_H
benkatz 20:bf9ea5125d52 3
benkatz 20:bf9ea5125d52 4 #define PI 3.14159265359f
benkatz 48:74a40481740c 5 #define SQRT3 1.73205080757f
benkatz 20:bf9ea5125d52 6
benkatz 20:bf9ea5125d52 7 #include "math.h"
benkatz 20:bf9ea5125d52 8
benkatz 20:bf9ea5125d52 9 float fmaxf(float x, float y);
benkatz 20:bf9ea5125d52 10 float fminf(float x, float y);
benkatz 20:bf9ea5125d52 11 float fmaxf3(float x, float y, float z);
benkatz 20:bf9ea5125d52 12 float fminf3(float x, float y, float z);
benkatz 34:51647c6c500d 13 float roundf(float x);
benkatz 20:bf9ea5125d52 14 void limit_norm(float *x, float *y, float limit);
benkatz 47:e1196a851f76 15 void limit(float *x, float min, float max);
MartinGurtner 59:8fb0145aa933 16
MartinGurtner 59:8fb0145aa933 17 inline float my_fmod(float a, float b) {
MartinGurtner 59:8fb0145aa933 18 return (a - b * floor(a / b));
MartinGurtner 59:8fb0145aa933 19 }
MartinGurtner 59:8fb0145aa933 20
MartinGurtner 59:8fb0145aa933 21 template<unsigned int bits>
MartinGurtner 59:8fb0145aa933 22 int float_to_uint(float x, const float x_max, const bool symmetric=true)
MartinGurtner 59:8fb0145aa933 23 {
MartinGurtner 59:8fb0145aa933 24 float span = x_max + symmetric*x_max;
MartinGurtner 59:8fb0145aa933 25 float quantum = span/((1<<bits)-1);
MartinGurtner 59:8fb0145aa933 26
MartinGurtner 59:8fb0145aa933 27 float offset = 0;
MartinGurtner 59:8fb0145aa933 28 if (symmetric) {
MartinGurtner 59:8fb0145aa933 29 offset = -x_max - quantum/2;
MartinGurtner 59:8fb0145aa933 30 }
MartinGurtner 59:8fb0145aa933 31
MartinGurtner 59:8fb0145aa933 32 return (int) ((x-offset)/quantum + 0.5);
MartinGurtner 59:8fb0145aa933 33 }
MartinGurtner 59:8fb0145aa933 34
MartinGurtner 59:8fb0145aa933 35 template<unsigned int bits>
MartinGurtner 59:8fb0145aa933 36 float uint_to_float(int x, const float x_max, const bool symmetric=true)
MartinGurtner 59:8fb0145aa933 37 {
MartinGurtner 59:8fb0145aa933 38 float span = x_max + symmetric*x_max;
MartinGurtner 59:8fb0145aa933 39 float quantum = span/((1<<bits)-1);
MartinGurtner 59:8fb0145aa933 40
MartinGurtner 59:8fb0145aa933 41 float offset = 0;
MartinGurtner 59:8fb0145aa933 42 if (symmetric) {
MartinGurtner 59:8fb0145aa933 43 offset = -x_max - quantum/2;
MartinGurtner 59:8fb0145aa933 44 }
MartinGurtner 59:8fb0145aa933 45
MartinGurtner 59:8fb0145aa933 46 return ((float)x)*quantum + offset;
MartinGurtner 59:8fb0145aa933 47 }
MartinGurtner 59:8fb0145aa933 48
MartinGurtner 59:8fb0145aa933 49 template<unsigned int bits>
MartinGurtner 59:8fb0145aa933 50 int float_to_uint_symmetric(float x, const float x_lim)
MartinGurtner 59:8fb0145aa933 51 {
MartinGurtner 59:8fb0145aa933 52 return float_to_uint<bits>(x, x_lim, true);
MartinGurtner 59:8fb0145aa933 53 }
MartinGurtner 59:8fb0145aa933 54
MartinGurtner 59:8fb0145aa933 55 template<unsigned int bits>
MartinGurtner 59:8fb0145aa933 56 float uint_to_float_symmetric(int x, const float x_lim)
MartinGurtner 59:8fb0145aa933 57 {
MartinGurtner 59:8fb0145aa933 58 return uint_to_float<bits>(x, x_lim, true);
MartinGurtner 59:8fb0145aa933 59 }
MartinGurtner 59:8fb0145aa933 60
MartinGurtner 59:8fb0145aa933 61 template<unsigned int bits>
MartinGurtner 59:8fb0145aa933 62 int float_to_uint_positive(float x, const float x_lim)
MartinGurtner 59:8fb0145aa933 63 {
MartinGurtner 59:8fb0145aa933 64 return float_to_uint<bits>(x, x_lim, false);
MartinGurtner 59:8fb0145aa933 65 }
MartinGurtner 59:8fb0145aa933 66
MartinGurtner 59:8fb0145aa933 67 template<unsigned int bits>
MartinGurtner 59:8fb0145aa933 68 float uint_to_float_positive(int x, const float x_lim)
MartinGurtner 59:8fb0145aa933 69 {
MartinGurtner 59:8fb0145aa933 70 return uint_to_float<bits>(x, x_lim, false);
MartinGurtner 59:8fb0145aa933 71 }
benkatz 20:bf9ea5125d52 72
benkatz 20:bf9ea5125d52 73 #endif