fast integer math (no floating point processor on rone) More...

Data Structures
struct	Pose
	The pose of a robot, it's position and orientation. More...

Typedefs
typedef struct Pose	Pose
	The pose of a robot, it's position and orientation.

Functions
uint32	sqrtInt (uint32 val)
	Compute the integer square root of a number. More...

int32	vectorMag (int32 x, int32 y)
	Computes the magnitude of the input vector. More...

uint32	decToZero (uint32 val)
	Continually decrements the input value by one until it is closest to zero. More...

int32	average (int32 val1, int32 val2)
	Average two values. More...

uint8	bitsCount (uint32 val)
	Counts how many bits the value has. More...

int32	min (int32 x, int32 y)
	Finds the min of the two arguments. More...

int32	max (int32 x, int32 y)
	Finds the min of the two arguments. More...

int32	boundAbs (int32 val, int32 bound)
	Bounds the value with one specified bound as both lower and upper bound. More...

int32	bound (int32 val, int32 lowerBound, int32 upperBound)
	Bounds the value with specified lower and upper bound. More...

int16	sinMilliRad (int16 angle)
	Interprets the angle as milli-radian of sine. More...

int16	cosMilliRad (int16 angle)

int16	atan2MilliRad (int32 y, int32 x)
	Gets atan2 approximation in miiliradians. More...

int16	normalizeAngleMilliRad (int16 angle)
	Normalizes the angle. More...

int16	normalizeAngleMilliRad2 (int16 angle)
	Normalizes the angle. More...

int32	normalizeAngleMicroRad (int32 angle)
	Normalizes the angle. More...

int16	smallestAngleDifference (int16 thetaGoal, int16 theta)
	Calculates the smallest angle difference between the two input angles. More...

int32	min3 (int32 valueA, int32 valueB, int32 valueC)
	Finds the min of 3 input values. More...

int32	filterIIR (int32 sample, int32 average, int32 alpha)
	IIR (low pass) filter for integer quantities. More...

int16	averageAngles (int16 angle1, int16 angle2)
	Calculates the average of the two angles in milli-radians. More...

int32	averageAnglesMicroRad (int32 theta1, int32 theta2)
	Calculates the average of the two angles in micro-radians. More...

int16	averageArrayAngle (int16 angleArray[], int32 size)
	Calculates the average of the angles in the array. More...

int16	averageAnglesLeftToRight (int16 angleLeft, int16 angleRight)
	Calculates the average of the two angles in millirad. More...

int16	byteToMillirad (int8 angle)
	unit conversion (16bit to 8bit) More...

int8	milliradToByte (int16 angle)
	unit conversion (16bit to 8bit) More...

void	pack16 (uint8 *arrayPtr, uint32 dataWord)
	Pack a 16-bit dataWord into 8-bit, pointed to by pointer arrayPtr. More...

void	pack24 (uint8 *arrayPtr, uint32 dataWord)
	Pack a 24-bit dataWord into 8-bit, pointed to by char pointer arrayPtr. More...

void	pack32 (uint8 *arrayPtr, uint32 dataWord)
	Pack a 32-bit dataWord into 8-bit, pointed to by char pointer arrayPtr. More...

uint16	unpack16 (uint8 *arrayPtr)
	Unpacks an 8-bit data into 16-bit. More...

uint32	unpack24 (uint8 *arrayPtr)
	Unpacks an 8-bit data into 24-bit. More...

uint32	unpack32 (uint8 *arrayPtr)
	Unpacks an 8-bit data into 32 bit. Implemented in a pedantic way to avoid assumptions of endianness. More...

int16	byteToMilliradUnsigned (uint8 angle)
	unit conversion (8bit to 16bit) More...

uint8	milliradToByteUnsigned (int16 angle)
	unit conversion (16bit to 8bit) More...

int16	angleFromBitVector (uint8 bitVector)
	Calculates the resultant angle from the bit vector. This assumes that bit0 = 0 rad. More...

int16	angleFromBitVectorOffset (uint8 bitVector)
	Calculates the resultant angle from the bit vector. This assumes that bit0 = 383 rad. More...

int16	angleFromBitVectorBeacon (uint8 bitVector)
	Calculates the resultant angle from the bit vector. This assumes that bit0 = 0 rad. More...

uint8	bitsMaxContiguous (uint8 val)
	Determines the number of bits that reflect against an obstacle. More...

int32	poseAngleDiff (Pose poseGoalPtr, Pose posePtr)
	Calculates the smallestAngleDifference between two poses. More...

void	poseAdd (Pose poseResPtr, Pose pose1Ptr, Pose *pose2Ptr)
	Adds two Poses and places result in a Pose. More...

int32	poseDistance (Pose pose1Ptr, Pose pose2Ptr)
	Calculates distance between two poses. More...

Detailed Description

fast integer math (no floating point processor on rone)

Since: Apr 2, 2012

Author: James McLurkin

Function Documentation

int16 angleFromBitVector ( uint8 bitVector)

Calculates the resultant angle from the bit vector. This assumes that bit0 = 0 rad.

Parameters

bitVector is the vector of bits

Returns: the average angle

int16 angleFromBitVectorBeacon ( uint8 bitVector)

Calculates the resultant angle from the bit vector. This assumes that bit0 = 0 rad.

Parameters

bitVector is the vector of bits

Returns: the average angle

int16 angleFromBitVectorOffset ( uint8 bitVector)

Calculates the resultant angle from the bit vector. This assumes that bit0 = 383 rad.

Parameters

bitVector is the vector of bits

Returns: the average angle

int16 atan2MilliRad	(	int32	y,
		int32	x
	)

Gets atan2 approximation in miiliradians.

Originally developed by John Aspinal at iRobot. It is quite good.

Parameters

y	y-coordinate of the point to be calculated
x	x-coordinate of the point to be calculated

Returns: atan2 approximation of the input point, specified by (x,y) coordinate

int32 average	(	int32	val1,
		int32	val2
	)

Average two values.

Parameters

val1	is the first value
val2	is the second value

Returns: the average of val1 and val2

int16 averageAngles	(	int16	angle1,
		int16	angle2
	)

Calculates the average of the two angles in milli-radians.

Parameters

angle1	is the first angle to be averaged
angle2	is the second angle to be averaged

Returns: the average angle

int16 averageAnglesLeftToRight	(	int16	angleLeft,
		int16	angleRight
	)

Calculates the average of the two angles in millirad.

Parameters

angleLeft	is the left angle to be averaged
angleRight	is the right angle to be averaged

Returns: the average angle

int32 averageAnglesMicroRad	(	int32	theta1,
		int32	theta2
	)

Calculates the average of the two angles in micro-radians.

Parameters

theta1	is the first angle to be averaged
theta2	is the second angle to be averaged

Returns: the average angle

int16 averageArrayAngle	(	int16	angleArray[],
		int32	size
	)

Calculates the average of the angles in the array.

Calculates the average of the first "size (a number)" of angles in angleArray.

Parameters

angleArray[]	is the array of angles to be averaged
size	specifies how many elements in the array (starting from the first) should be averaged

Returns: the average of the angles in the array (returns 0 if given a nonpositive size)

uint8 bitsCount ( uint32 val)

Counts how many bits the value has.

Ignores leading zeros.

Parameters

val	is the value to be counted

Returns: the number of bits of the input value

uint8 bitsMaxContiguous ( uint8 val)

Determines the number of bits that reflect against an obstacle.

The number of bits increases as the robot gets closer to the obstacle.

Parameters

val	a uint8 value

Returns: the number of bits that reflect against an obstacle

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

Bounds the value with specified lower and upper bound.

Bounds the value so that it stays within the range of lowerBound <= value <= upperBound. If it exceeds the bound, set it to the lower/upper bound.

Parameters

val	is the value to be bounded
lowerBound	is the lower bound
upperBound	is the upper bound

Returns: the bounded value

int32 boundAbs	(	int32	val,
		int32	bound
	)

Bounds the value with one specified bound as both lower and upper bound.

Bounds the input value so that it stays within the range of -bound <= value <= bound. If it exceeds the bound, set it to the bound.

Parameters

val	is the value to be bounded
bound	is the lower and upper bound

Returns: the bounded value

int16 byteToMillirad ( int8 angle)

unit conversion (16bit to 8bit)

unit conversion (8bit to 16bit)

Parameters

angle -angle that needs to be changed

Returns: int16 angle in milli-radians

Parameters

angle as a 8 Byte

Returns: a int16 angle in milli-radians

unit conversion (16bit to 8bit)

Parameters

angle as a 8 Byte

Returns: a int16 angle in milli-radians

int16 byteToMilliradUnsigned ( uint8 angle)

unit conversion (8bit to 16bit)

Parameters

angle as a 8 Byte

Returns: a int16 angle in milli-radians

int16 cosMilliRad ( int16 angle)

Interprets the angle as milli-randian of cosine.

Parameters

angle the angle to be interpreted

Returns: angle as milli-radian of cosine

uint32 decToZero ( uint32 val)

Continually decrements the input value by one until it is closest to zero.

Parameters

val	the value to be decremented

Returns: the decremented value (within the range of 0 <= val < 1)

int32 filterIIR	(	int32	sample,
		int32	average,
		int32	alpha
	)

IIR (low pass) filter for integer quantities.

runs an IIR filter for integer quantities.

Parameters

sample	new measurement
average
alpha	IIR constant. This is divided by 100, so 100 = all newVal and 0 = all currentVal

Returns: filtered value

int32 max	(	int32	x,
		int32	y
	)

Finds the min of the two arguments.

Finds the min of two arguments.

Parameters

x	is the value to be compared
y	is the value to be compared

Returns: the min value

int8 milliradToByte ( int16 angle)

unit conversion (16bit to 8bit)

Parameters

angle

Returns: int8 the angle as a Byte

uint8 milliradToByteUnsigned ( int16 angle)

unit conversion (16bit to 8bit)

Parameters

angle a int16

Returns: int8 the angle as a Byte

int32 min	(	int32	x,
		int32	y
	)

Finds the min of the two arguments.

Finds the min of two arguments.

Parameters

x	is the value to be compared
y	is the value to be compared *

Returns: the min value

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

Finds the min of 3 input values.

Parameters

valueA	first int32 value
valueB	second int32 value
valueC	third int32 value

Returns: minimum of the 3 input values

int32 normalizeAngleMicroRad ( int32 angle)

Normalizes the angle.

Normalizes the angle to make it stay in the range of 0 <= angle < microrad_2PI.

Parameters

angle the angle to be normalized

Returns

the normalized angle in micro-radians

int16 normalizeAngleMilliRad ( int16 angle)

Normalizes the angle.

Normalizes the angle to make it stay in the range of 0 <= angle < millirad_2PI

Parameters

angle the angle to be normalized

Returns: the normalized angle

int16 normalizeAngleMilliRad2 ( int16 angle)

Normalizes the angle.

Normalizes the angle to make it stay in the range of -millirad_PI < angle <= millirad_PI.

Parameters

angle the angle to be normalized

Returns: the normalized angle in milli-radians

void pack16	(	uint8 *	arrayPtr,
		uint32	dataWord
	)

Pack a 16-bit dataWord into 8-bit, pointed to by pointer arrayPtr.

Parameters

arrayPtr	points to the packed 8-bit dataWord
dataWord	16-bit data to be packed

Returns: void

void pack24	(	uint8 *	arrayPtr,
		uint32	dataWord
	)

Pack a 24-bit dataWord into 8-bit, pointed to by char pointer arrayPtr.

Parameters

arrayPtr	points to the packed 8-bit dataWord
dataWord	24-bit data to be packed

Returns: void

void pack32	(	uint8 *	arrayPtr,
		uint32	dataWord
	)

Pack a 32-bit dataWord into 8-bit, pointed to by char pointer arrayPtr.

Parameters

arrayPtr	points to the packed 8-bit dataWord
dataWord	32-bit data to be packed

Returns: void

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

Adds two Poses and places result in a Pose.

Parameters

poseResPtr	pointer to Pose to hold result
pose1Ptr	pointer to first Pose
pose2Ptr	pointer to second Pose

Returns: void

int32 poseAngleDiff	(	Pose *	poseGoalPtr,
		Pose *	posePtr
	)

Calculates the smallestAngleDifference between two poses.

Parameters

poseGoalPtr	pointer to goal pose
posePtr	pointer to a pose

Returns: smallest angle difference between two poses

int32 poseDistance	(	Pose *	pose1Ptr,
		Pose *	pose2Ptr
	)

Calculates distance between two poses.

Parameters

pose1Ptr	pointer to first Pose
pose2Ptr	pointer to second Pose

Returns: distance

int16 sinMilliRad ( int16 angle)

Interprets the angle as milli-radian of sine.

Parameters

angle the angle to be interpreted

Returns: if angle is greater than pi/4, angle as milli-radian of sine. else, 0.

int16 smallestAngleDifference	(	int16	thetaGoal,
		int16	theta
	)

Calculates the smallest angle difference between the two input angles.

The difference will be within the range of -MILLIRAD_PI <- difference <= MILLIRAD_PI.

Parameters

thetaGoal	is first angle
theta	is second angle

Returns: the difference between thetaGoal and theta

uint32 sqrtInt ( uint32 val)

Compute the integer square root of a number.

Based on Microchip app note TB040. Can't take the root of numbers higher than MAX_INT32.

Parameters

val	is the number to be computed

Returns: the computed integer square root

uint16 unpack16 ( uint8 * arrayPtr)

Unpacks an 8-bit data into 16-bit.

Parameters

arrayPtr points to data with 8-bit wordlength

Returns: unpacked input data with 16-bit wordlength

uint32 unpack24 ( uint8 * arrayPtr)

Unpacks an 8-bit data into 24-bit.

Parameters

arrayPtr points to data with 8-bit wordlength

Returns: unpacked input data with 24-bit wordlength

uint32 unpack32 ( uint8 * arrayPtr)

Unpacks an 8-bit data into 32 bit. Implemented in a pedantic way to avoid assumptions of endianness.

Parameters

arrayPtr points to data with 8-bit wordlength

Returns: unpacked input data with 32-bit wordlength

int32 vectorMag	(	int32	x,
		int32	y
	)

Computes the magnitude of the input vector.

Parameters

x	the x component of the vector
y	the y component of the vector

Returns: the magnitude of the vector with components x and y

Data Structures

Typedefs

Functions

Detailed Description

Function Documentation