![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
torque sensor thing
math_ops.cpp@0:f4f2a7b3364c, 2021-06-21 (annotated)
- Committer:
- adimmit
- Date:
- Mon Jun 21 19:50:05 2021 +0000
- Revision:
- 0:f4f2a7b3364c
mostly done - just need to check values
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
adimmit | 0:f4f2a7b3364c | 1 | |
adimmit | 0:f4f2a7b3364c | 2 | #include "math_ops.h" |
adimmit | 0:f4f2a7b3364c | 3 | |
adimmit | 0:f4f2a7b3364c | 4 | |
adimmit | 0:f4f2a7b3364c | 5 | float fmaxf(float x, float y){ |
adimmit | 0:f4f2a7b3364c | 6 | /// Returns maximum of x, y /// |
adimmit | 0:f4f2a7b3364c | 7 | return (((x)>(y))?(x):(y)); |
adimmit | 0:f4f2a7b3364c | 8 | } |
adimmit | 0:f4f2a7b3364c | 9 | |
adimmit | 0:f4f2a7b3364c | 10 | float fminf(float x, float y){ |
adimmit | 0:f4f2a7b3364c | 11 | /// Returns minimum of x, y /// |
adimmit | 0:f4f2a7b3364c | 12 | return (((x)<(y))?(x):(y)); |
adimmit | 0:f4f2a7b3364c | 13 | } |
adimmit | 0:f4f2a7b3364c | 14 | |
adimmit | 0:f4f2a7b3364c | 15 | float fmaxf3(float x, float y, float z){ |
adimmit | 0:f4f2a7b3364c | 16 | /// Returns maximum of x, y, z /// |
adimmit | 0:f4f2a7b3364c | 17 | return (x > y ? (x > z ? x : z) : (y > z ? y : z)); |
adimmit | 0:f4f2a7b3364c | 18 | } |
adimmit | 0:f4f2a7b3364c | 19 | |
adimmit | 0:f4f2a7b3364c | 20 | float fminf3(float x, float y, float z){ |
adimmit | 0:f4f2a7b3364c | 21 | /// Returns minimum of x, y, z /// |
adimmit | 0:f4f2a7b3364c | 22 | return (x < y ? (x < z ? x : z) : (y < z ? y : z)); |
adimmit | 0:f4f2a7b3364c | 23 | } |
adimmit | 0:f4f2a7b3364c | 24 | |
adimmit | 0:f4f2a7b3364c | 25 | void limit_norm(float *x, float *y, float limit){ |
adimmit | 0:f4f2a7b3364c | 26 | /// Scales the lenght of vector (x, y) to be <= limit /// |
adimmit | 0:f4f2a7b3364c | 27 | float norm = sqrt(*x * *x + *y * *y); |
adimmit | 0:f4f2a7b3364c | 28 | if(norm > limit){ |
adimmit | 0:f4f2a7b3364c | 29 | *x = *x * limit/norm; |
adimmit | 0:f4f2a7b3364c | 30 | *y = *y * limit/norm; |
adimmit | 0:f4f2a7b3364c | 31 | } |
adimmit | 0:f4f2a7b3364c | 32 | } |
adimmit | 0:f4f2a7b3364c | 33 | |
adimmit | 0:f4f2a7b3364c | 34 | |
adimmit | 0:f4f2a7b3364c | 35 | int float_to_uint(float x, float x_min, float x_max, int bits){ |
adimmit | 0:f4f2a7b3364c | 36 | /// Converts a float to an unsigned int, given range and number of bits /// |
adimmit | 0:f4f2a7b3364c | 37 | float span = x_max - x_min; |
adimmit | 0:f4f2a7b3364c | 38 | float offset = x_min; |
adimmit | 0:f4f2a7b3364c | 39 | return (int) ((x-offset)*((float)((1<<bits)-1))/span); |
adimmit | 0:f4f2a7b3364c | 40 | } |
adimmit | 0:f4f2a7b3364c | 41 | |
adimmit | 0:f4f2a7b3364c | 42 | |
adimmit | 0:f4f2a7b3364c | 43 | float uint_to_float(int x_int, float x_min, float x_max, int bits){ |
adimmit | 0:f4f2a7b3364c | 44 | /// converts unsigned int to float, given range and number of bits /// |
adimmit | 0:f4f2a7b3364c | 45 | float span = x_max - x_min; |
adimmit | 0:f4f2a7b3364c | 46 | float offset = x_min; |
adimmit | 0:f4f2a7b3364c | 47 | return ((float)x_int)*span/((float)((1<<bits)-1)) + offset; |
adimmit | 0:f4f2a7b3364c | 48 | } |