Aditya Mehrotra / SPIne_Plus_v2

Spine 18 motor board for Biomimetic Robotics Lab

Dependencies:   mbed-dev

math_ops.cpp@15:56a06363a698, 22 months ago (annotated)

Committer:
adimmit
Date:
Wed Sep 28 16:32:46 2022 +0000
Revision:
15:56a06363a698
Parent:
0:76c761d3caf1 
updated to work with checksum and with the 2-motor dyno;

Who changed what in which revision?

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