hormone v.1 for optimizing COT
Dependents: TurtleBot_with_AHS
Fork of calculator by
Diff: calculator.cpp
- Revision:
- 0:22ebaf70b60e
- Child:
- 1:b6345fbad095
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/calculator.cpp Mon Aug 13 11:38:52 2018 +0000 @@ -0,0 +1,171 @@ +#include "calculator.h" +#include "mbed.h" +#include <stdlib.h> + +/*------------------------< Function comment >------------------------*/ +/* NAME : constructor */ +/* PARAMETERS : - */ +/* RETURN VALUE : - */ +/* DESCRIPTION : - */ +/*--------------------------------------------------------------------*/ +calculator::calculator() +{ + +} + +/*------------------------< Function comment >------------------------*/ +/* NAME : calculate SD */ +/* PARAMETERS : - */ +/* RETURN VALUE : SD value */ +/* DESCRIPTION : - */ +/*--------------------------------------------------------------------*/ +float calculator::calculateSD(const float *sdData, int size) +{ + float sum = 0.0, mean, standardDeviation = 0.0; + int i; + + if(!sdData) + { + return NULL; + } + + for(i = 0; i < size ; i++) + { + sum += sdData[i]; + //pc.printf("sum = %.2f \n\r",sum); + } + mean = sum/size; + //pc.printf("mean = %.2f \n\r",mean); + + for(i = 0; i < size; i++) + { + standardDeviation += pow(sdData[i] - mean, 2); + //pc.printf("standardDeviation = %.2f \n\r",standardDeviation); + } + return sqrt(standardDeviation / size); +} + +/*------------------------< Function comment >------------------------*/ +/* NAME : calculate mean */ +/* PARAMETERS : - */ +/* RETURN VALUE : mean value */ +/* DESCRIPTION : - */ +/*--------------------------------------------------------------------*/ +float calculator::calculateMean(const float *meanData, int size) +{ + float sum = 0.0, mean; + int i; + + if(!meanData) + { + return NULL; + } + + for(i = 0; i < size ; ++i) + { + sum += meanData[i]; + //pc.printf("sum = %.2f \n\r",sum); + } + mean = sum/size; + //pc.printf("mean = %.2f \n\r",mean); + return mean; +} + +/*------------------------< Function comment >------------------------*/ +/* NAME : calculate size of vector A(Ax, Ay, Az) in State 2,3 */ +/* PARAMETERS : - */ +/* RETURN VALUE : size value */ +/* DESCRIPTION : - */ +/*--------------------------------------------------------------------*/ +float calculator::calcSDVectorA(const float *arrAx, const float *arrAy, const float *arrAz, int size) +{ + float sum = 0.0, mean = 0.0; + float standardDeviation = 0.0; + int i; + float *vSize; + + /* check arg is not empty */ + if(!arrAx or !arrAy or !arrAz) + { + return NULL; + } + + vSize = (float *)malloc(size*sizeof(float)); + /* check malloc is not return NULL */ + if( vSize == NULL ) + { + return NULL; + } + /*--------------------------------------------------------------------*/ + /* calculate size of vector */ + /*--------------------------------------------------------------------*/ + for(i = 0; i < size; i++) + { + *(vSize + i) = sqrt( pow(arrAx[i],2) + pow(arrAy[i],2) + pow(arrAz[i],2) ); + } + /*--------------------------------------------------------------------*/ + /* calculate mean of vector size */ + /*--------------------------------------------------------------------*/ + for(i = 0; i < size ; i++) + { + sum += *(vSize + i); + //pc.printf("sum = %.2f \n\r",sum); + } + mean = sum/size; + //pc.printf("mean = %.2f \n\r",mean); + for(i = 0; i < size; i++) + { + standardDeviation += pow(*(vSize + i) - mean, 2); + //pc.printf("standardDeviation = %.2f \n\r",standardDeviation); + } + free(vSize); + return sqrt(standardDeviation / size); +} + + +/*------------------------< Function comment >------------------------*/ +/* NAME : calculate mean size of point(roll, pitch) */ +/* PARAMETERS : - */ +/* RETURN VALUE : mean value */ +/* DESCRIPTION : - */ +/*--------------------------------------------------------------------*/ +float calculator::calcG4(const float *arrRoll, const float *arrPitch, int size) +{ + float sum = 0.0, mean = 0.0; + int i; + float *pSize; + + /* check arg is not empty */ + if(!arrRoll or !arrPitch) + { + return NULL; + } + + pSize = (float *)malloc(size*sizeof(float)); + /* check malloc is not return NULL */ + if( pSize == NULL ) + { + return NULL; + } + /*--------------------------------------------------------------------*/ + /* calculate size of point */ + /*--------------------------------------------------------------------*/ + for(i = 0; i < size; i++) + { + *(pSize + i) = sqrt( pow(arrRoll[i],2) + pow(arrPitch[i],2) ); + } + /*--------------------------------------------------------------------*/ + /* calculate mean of point size */ + /*--------------------------------------------------------------------*/ + for(i = 0; i < size ; i++) + { + sum += *(pSize + i); + //pc.printf("sum = %.2f \n\r",sum); + } + mean = sum/size; + //pc.printf("mean = %.2f \n\r",mean); + + free(pSize); + return mean; +} +/*--------------------------------------------------------------------*/ \ No newline at end of file