zebra
/
ZebraJointController
zebra joint controller firmware (originally from MIT HKC motor controller by benkatz)
Embed:
(wiki syntax)
Show/hide line numbers
math_ops.cpp
00001 00002 #include "../math_ops.h" 00003 00004 00005 float fmaxf(float x, float y){ 00006 /// Returns maximum of x, y /// 00007 return (((x)>(y))?(x):(y)); 00008 } 00009 00010 float fminf(float x, float y){ 00011 /// Returns minimum of x, y /// 00012 return (((x)<(y))?(x):(y)); 00013 } 00014 00015 float fmaxf3(float x, float y, float z){ 00016 /// Returns maximum of x, y, z /// 00017 return (x > y ? (x > z ? x : z) : (y > z ? y : z)); 00018 } 00019 00020 float fminf3(float x, float y, float z){ 00021 /// Returns minimum of x, y, z /// 00022 return (x < y ? (x < z ? x : z) : (y < z ? y : z)); 00023 } 00024 00025 float roundf(float x){ 00026 /// Returns nearest integer /// 00027 00028 return x < 0.0f ? ceilf(x - 0.5f) : floorf(x + 0.5f); 00029 } 00030 00031 void limit_norm(float *x, float *y, float limit){ 00032 /// Scales the lenght of vector (x, y) to be <= limit /// 00033 float norm = sqrt(*x * *x + *y * *y); 00034 if(norm > limit){ 00035 *x = *x * limit/norm; 00036 *y = *y * limit/norm; 00037 } 00038 } 00039 00040 void limit(float *x, float min, float max){ 00041 *x = fmaxf(fminf(*x, max), min); 00042 } 00043 00044 int float_to_uint(float x, float x_min, float x_max, int bits){ 00045 /// Converts a float to an unsigned int, given range and number of bits /// 00046 float span = x_max - x_min; 00047 float offset = x_min; 00048 return (int) ((x-offset)*((float)((1<<bits)-1))/span); 00049 } 00050 00051 00052 float uint_to_float(int x_int, float x_min, float x_max, int bits){ 00053 /// converts unsigned int to float, given range and number of bits /// 00054 float span = x_max - x_min; 00055 float offset = x_min; 00056 return ((float)x_int)*span/((float)((1<<bits)-1)) + offset; 00057 }
Generated on Sun Jul 17 2022 04:31:50 by 1.7.2