Ben Katz / SPIne

SPI to quad CAN firmware

Dependencies:   mbed-dev_spine

Fork of Teleop_Controller by Ben Katz

math_ops.cpp@9:56747e76c9c1, 2019-10-04 (annotated)

Committer:
benkatz
Date:
Fri Oct 04 14:21:16 2019 +0000
Revision:
9:56747e76c9c1
Parent:
0:d6186b8990c5 
first revision on new mini cheetah; updated max speed to 65 rad/s; added 1s delay at startup

Who changed what in which revision?

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