Motor control for robots. More compact, less object-oriented revision.

Dependencies:   FastPWM3 mbed-dev-f303

Fork of Hobbyking_Cheetah_V1 by Ben Katz

Committer:
benkatz
Date:
Mon Jul 30 20:33:23 2018 +0000
Revision:
46:6cc428f3431d
Parent:
45:aadebe074af6
fixed dumb bug in float_to_uint (already fixed in SPIne)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 20:bf9ea5125d52 1
benkatz 20:bf9ea5125d52 2 #include "math_ops.h"
benkatz 20:bf9ea5125d52 3
benkatz 20:bf9ea5125d52 4 float fmaxf(float x, float y){
benkatz 25:f5741040c4bb 5 /// Returns maximum of x, y ///
benkatz 20:bf9ea5125d52 6 return (((x)>(y))?(x):(y));
benkatz 20:bf9ea5125d52 7 }
benkatz 20:bf9ea5125d52 8
benkatz 20:bf9ea5125d52 9 float fminf(float x, float y){
benkatz 25:f5741040c4bb 10 /// Returns minimum of x, y ///
benkatz 20:bf9ea5125d52 11 return (((x)<(y))?(x):(y));
benkatz 20:bf9ea5125d52 12 }
benkatz 20:bf9ea5125d52 13
benkatz 20:bf9ea5125d52 14 float fmaxf3(float x, float y, float z){
benkatz 25:f5741040c4bb 15 /// Returns maximum of x, y, z ///
benkatz 20:bf9ea5125d52 16 return (x > y ? (x > z ? x : z) : (y > z ? y : z));
benkatz 20:bf9ea5125d52 17 }
benkatz 20:bf9ea5125d52 18
benkatz 20:bf9ea5125d52 19 float fminf3(float x, float y, float z){
benkatz 25:f5741040c4bb 20 /// Returns minimum of x, y, z ///
benkatz 20:bf9ea5125d52 21 return (x < y ? (x < z ? x : z) : (y < z ? y : z));
benkatz 20:bf9ea5125d52 22 }
benkatz 34:51647c6c500d 23
benkatz 34:51647c6c500d 24 float roundf(float x){
benkatz 34:51647c6c500d 25 /// Returns nearest integer ///
benkatz 34:51647c6c500d 26
benkatz 34:51647c6c500d 27 return x < 0.0f ? ceilf(x - 0.5f) : floorf(x + 0.5f);
benkatz 34:51647c6c500d 28 }
benkatz 20:bf9ea5125d52 29
benkatz 20:bf9ea5125d52 30 void limit_norm(float *x, float *y, float limit){
benkatz 25:f5741040c4bb 31 /// Scales the lenght of vector (x, y) to be <= limit ///
benkatz 20:bf9ea5125d52 32 float norm = sqrt(*x * *x + *y * *y);
benkatz 20:bf9ea5125d52 33 if(norm > limit){
benkatz 20:bf9ea5125d52 34 *x = *x * limit/norm;
benkatz 20:bf9ea5125d52 35 *y = *y * limit/norm;
benkatz 20:bf9ea5125d52 36 }
benkatz 20:bf9ea5125d52 37 }
benkatz 26:2b865c00d7e9 38
benkatz 45:aadebe074af6 39
benkatz 45:aadebe074af6 40 int float_to_uint(float x, float x_min, float x_max, uint8_t bits){
benkatz 45:aadebe074af6 41 /// Converts a float to an unsigned int, given range and number of bits ///
benkatz 26:2b865c00d7e9 42 float span = x_max - x_min;
benkatz 26:2b865c00d7e9 43 float offset = x_min;
benkatz 45:aadebe074af6 44 return (int) ((x-offset)*((float)((1<<bits)-1))/span);
benkatz 26:2b865c00d7e9 45 }
benkatz 26:2b865c00d7e9 46
benkatz 45:aadebe074af6 47
benkatz 45:aadebe074af6 48 float uint_to_float(int x_int, float x_min, float x_max, uint8_t bits){
benkatz 45:aadebe074af6 49 /// converts unsigned int to float, given range and number of bits ///
benkatz 26:2b865c00d7e9 50 float span = x_max - x_min;
benkatz 26:2b865c00d7e9 51 float offset = x_min;
benkatz 26:2b865c00d7e9 52 return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
benkatz 45:aadebe074af6 53 }