Clara Keng / Mbed 2 deprecated FreeFlyerROS_clarakhl

Dependencies:   mbed ros_lib_kinetic

utilities.cpp

Committer:
clarakeng
Date:
2019-10-14
Revision:
7:e165f5119950
Parent:
5:864709d3eb76

File content as of revision 7:e165f5119950:


#include "utilities.h"
#include "mbed.h"

namespace utils {
    
    float smooth(float data, float filterVal, float smoothedVal) {
    
        if (filterVal > 1) {      // check to make sure param's are within range
            filterVal = .99;
        }  else if (filterVal <= 0) {
            filterVal = 0;
        }
        
        smoothedVal = (data * (1 - filterVal)) + (smoothedVal  *  filterVal);
        
        return (float)smoothedVal;
    }
    
    float min(float a, float b) {
        if (a < b)
            return a;
        else
            return b;
    }
    
    void printBits(char myByte, Serial &pc) {
        for (char mask = 0x80; mask; mask >>= 1) {
            if(mask  & myByte)
                pc.putc('1');
            else
                pc.putc('0');
        }
    }

} // end utils namespace