intMath.h

Go to the documentation of this file.

#ifndef intMath_h
#define intMath_h


/******** Defines ********/
#define __PI 3.14159

/* Angles */
#define MILLIRAD_HALF_PI                        1571
#define MILLIRAD_PI                                     (MILLIRAD_HALF_PI * 2)
#define MILLIRAD_2PI                            (MILLIRAD_HALF_PI * 4)

#define MILLIRAD_DEG_0                          0
#define MILLIRAD_DEG_15                         (MILLIRAD_HALF_PI / 6)
#define MILLIRAD_DEG_20                         (MILLIRAD_PI / 9)
#define MILLIRAD_DEG_30                         (MILLIRAD_HALF_PI / 3)
#define MILLIRAD_DEG_45                         (MILLIRAD_HALF_PI / 2)
#define MILLIRAD_DEG_90                         MILLIRAD_HALF_PI
#define MILLIRAD_DEG_180                        MILLIRAD_PI
#define MILLIRAD_DEG_270                        (MILLIRAD_PI + MILLIRAD_HALF_PI)
#define MILLIRAD_DEG_360                        MILLIRAD_2PI

#define MILLIRAD_TRIG_SCALER            32767

#define GAUSSIAN_LOOKUP_SIZE 64
#define GAUSSIAN_LOOKUP_MAX_RETURN 255


/******** Structs ********/

typedef struct Pose {
        int32 x;                
        int32 y;                
        int32 theta;    
} Pose;

extern Pose poseOrigin;



/******** Macros ********/


/******** Functions ********/

//General math

uint32 sqrtInt(uint32 val);


int32 vectorMag(int32 x, int32 y);

uint32 decToZero(uint32 val);


int32 average(int32 val1, int32 val2);


uint8 bitsCount(uint32 val);


int32 min(int32 x, int32 y);


int32 max(int32 x, int32 y);


int32 boundAbs(int32 val, int32 bound);


int32 bound(int32 val, int32 lowerBound, int32 upperBound);

// trig and angles

int16 sinMilliRad(int16 angle);


int16 cosMilliRad(int16 angle);


int16 atan2MilliRad(int32 y, int32 x);


int16 normalizeAngleMilliRad(int16 angle);


int16 normalizeAngleMilliRad2(int16 angle);


int32 normalizeAngleMicroRad(int32 angle);


//TODO: Commented out in C file, Should this be deleted
int16 normalizeAngleMilliRad3(int16 angle);


int16 smallestAngleDifference(int16 thetaGoal, int16 theta);


int32 min3(int32 valueA, int32 valueB, int32 valueC);

// Averaging and arrays

//Is this even right? Updated so that params are the same but there's a TODO:check for overflow in the actual code
int32 filterIIR(int32 sample, int32 average, int32 alpha);


int16 averageAngles(int16 angle1, int16 angle2);


int32 averageAnglesMicroRad(int32 theta1, int32 theta2);


int16 averageArrayAngle(int16 angleArray[], int32 size);


int16 averageAnglesLeftToRight(int16 angleLeft, int16 angleRight);


int16 byteToMillirad(int8 angle);


int8 milliradToByte(int16 angle);


// byte packing

void pack16(uint8 * arrayPtr, uint32 dataWord);


void pack24(uint8 * arrayPtr, uint32 dataWord);


void pack32(uint8 * arrayPtr, uint32 dataWord);


uint16 unpack16(uint8 * arrayPtr);


uint32 unpack24(uint8 * arrayPtr);


uint32 unpack32(uint8 * arrayPtr);

// Angle - Byte converting

int16 byteToMillirad(int8  angle);


int8 milliradToByte(int16  angle);


int16 byteToMilliradUnsigned(uint8 angle);


uint8 milliradToByteUnsigned(int16 angle);


//for midi parser

/*
 * @brief Converts string of bytes into a long int.
 *
 * Converts a string of 'len' bytes, MSB first, into a long int. This is
 * not to be confused with 'atoi()': 'atoi()' converts "abcdef12" into
 * '0xabcdef12L', whereas 'stol()' converts '0xab,0xcd,0xef,0x12' into
 * '0xabcdef12L'.
 *
 * @param str a string
 * @param len the number of bytes in str
 * @returns str converted into a long it
 */
uint32 stol(uint8 * str, int len);

// angles from bits

int16 angleFromBitVector(uint8 bitVector);


int16 angleFromBitVectorOffset(uint8 bitVector);


//TODO: Update comment to talk about beacons (how is this different from angle FromBItVector)
int16 angleFromBitVectorBeacon(uint8 bitVector);


uint8 bitsMaxContiguous(uint8 val);


// Pose math

int32 poseAngleDiff(Pose* poseGoalPtr, Pose* posePtr);


void poseAdd(Pose* poseResPtr, Pose* pose1Ptr, Pose* pose2Ptr);


int32 poseDistance(Pose* pose1Ptr, Pose* pose2Ptr);

// Gaussian

/*
 * @brief Calculates gaussian noise.
 *
 * Optimization errors fixed by setting roneos optimization to 0!!!!!!!
 *
 * @returns gaussian noise +/-GAUSSIAN_LOOKUP_MAX_RETURN centered at 0.
 */
int16 gaussianNoise();

#if 0
// overflow checking math
#define add32(v1, v2) add32func(v1, v2, __FILE__, __LINE__)
#define sub32(v1, v2) sub32func(v1, v2, __FILE__, __LINE__)
#define mul32(v1, v2) mul32func(v1, v2, __FILE__, __LINE__)
#define div32(v1, v2) div32func(v1, v2, __FILE__, __LINE__)

uint8 msb32(uint32 val);
int32 add32func(int32 v1, int32 v2, const char * filePathName, unsigned int lineNum);
int32 sub32func(int32 v1, int32 v2, const char * filePathName, unsigned int lineNum);
int32 mul32func(int32 v1, int32 v2, const char * filePathName, unsigned int lineNum);
int32 div32func(int32 v1, int32 v2, const char * filePathName, unsigned int lineNum);

#define add64(v1, v2) add64func(v1, v2, __FILE__, __LINE__)
#define sub64(v1, v2) sub64func(v1, v2, __FILE__, __LINE__)
#define mul64(v1, v2) mul64func(v1, v2, __FILE__, __LINE__)
#define div64(v1, v2) div64func(v1, v2, __FILE__, __LINE__)

int64 abs64(int64 val);
uint8 msb64(uint64 val);
int64 add64func(int64 v1, int64 v2, const char * filePathName, unsigned int lineNum);
int64 sub64func(int64 v1, int64 v2, const char * filePathName, unsigned int lineNum);
int64 mul64func(int64 v1, int64 v2, const char * filePathName, unsigned int lineNum);
int64 div64func(int64 v1, int64 v2, const char * filePathName, unsigned int lineNum);
#endif

#endif // #ifndef intMath_h