hormone v.1 for optimizing COT
Dependents: TurtleBot_with_AHS
Fork of calculator by
calculator.cpp
- Committer:
- worasuchad
- Date:
- 2018-08-13
- Revision:
- 0:22ebaf70b60e
- Child:
- 1:b6345fbad095
File content as of revision 0:22ebaf70b60e:
#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; } /*--------------------------------------------------------------------*/