A (small) helper library for handling common tasks when programming in RobotC.

libvexhelper.c

/* vexhelper library version 0.2 */
/*

Dictionary:

-variable -> a name which can contain a value (i.e 6, "hello", 3.14)
-constant -> a variable whose value cannot be modified
-function -> a block of reusable code

Description:

This library contains a collection of useful constants and functions
to make programming the VEX much more simpler. So far, it supports the
following features:

1. Input Processing
2. Motor Processing (constant values only)
3. Sensor Processing
*/

const int DEFAULT_THRESHOLD = 10;
int THRESHOLD = DEFAULT_THRESHOLD;

const int X_AXIS = 0;
const int Y_AXIS = 1;

const int P1 = 0;
const int P2 = 1;

const int LEFT 	= 0;
const int RIGHT = 1;
const int UP 	= 2;
const int DOWN 	= 4;

const int FULL_FORWARD = 127;
const int HALF_FORWARD = FULL_FORWARD / 2;
const int FULL_REVERSE = -127;
const int HALF_REVERSE = FULL_REVERSE / 2;

const int GYRO_SENSOR = 1000;
const int ACCEL_SENSOR = 1001;

short LeftMotorFront, LeftMotorBack, RightMotorFront, RightMotoBack;
short LeftMotor, RightMotor;

short LeftChannel, RightChannel;
short xAxisChannel, yAxisChannel;

/**********************
 ** Config Functions **
 **********************/
void configureFourMotor(short lmf, short lmb, short rmf, short rmb) {
	LeftMotorFront = lmf;
	LeftMotorBack = lmb;
	RightMotorFront = rmf;
	RightMotorBack = rmb;
}

void configureTwoMotor(short lMotor, short rMotor) {
	LeftMotor = lMotor;
	RightMotor = rMotor;
}

void configureTank(int player) {
	LeftChannel = (player == P1) ? Ch3 : Ch3Xmrt2;
	RightChannel = (player == P1) ? Ch2 : Ch2Xmrt2;
}

void configureArcade(int player, int side) {
	if (player == P1) {
		if (side == RIGHT) {
			xAxisChannel = Ch1;
			yAxisChannel = Ch2;
		} else {
			xAxisChannel = Ch4;
			yAxisChannel = Ch3;
		}
	} else {
		if (side == RIGHT) {
			xAxisChannel = Ch1Xmrt2;
			yAxisChannel = Ch2Xmrt2;
		} else {
			xAxisChannel = Ch4Xmrt2;
			yAxisChannel = Ch3Xmrt2;
		}
	}
}

/***********************
 ** Utility Functions **
 ***********************/
void setThreshold(int value) {
	THRESHOLD = value;
}

void threshify(int value) {
	return (abs(value) > THRESHOLD ? value : 0);
}

/**********************
 ** Input Processing **
 **********************/

// Joystick Functions
short getJoystick(int player, int side, int axisType) {
	if (player == P1) {
		if (side == RIGHT) {
			if (axisType == X_AXIS) { //Ch3 - horizontal
				return threshify(vexRT[Ch3]);
			} else if (axisType == Y_AXIS) { //Ch4 - vertical
				return threshify(vexRT[Ch4]);
			} else {
				return 0;
			}
		} else {
			if (axisType == X_AXIS) { //Ch1 - horizontal
				return threshify(vexRT[Ch1]);
			} else if (axisType == Y_AXIS) { //Ch2 - vertical
				return threshify(vexRT[Ch2]);
			} else {
				return 0;
			}
		}
	} else {
		if (side == RIGHT) {
			if (axisType == X_AXIS) { //Ch3 - horizontal
				return threshify(vexRT[Ch3Xmrt2]);
			} else if (axisType == Y_AXIS) { //Ch4 - vertical
				return threshify(vexRT[Ch4Xmrt2]);
			} else {
				return 0;
			}
		} else {
			if (axisType == X_AXIS) { //Ch1 - horizontal
				return threshify(vexRT[Ch1Xmrt2]);
			} else if (axisType == Y_AXIS) { //Ch2 - vertical
				return threshify(vexRT[Ch2Xmrt2]);
			} else {
				return 0;
			}
		}
	}
}

short getP1Joystick(int side, int axisType) {
	return getJoystick(P1, side, axisType);
}

short getP2Joystick(int side, int axisType) {
	return getJoystick(P2, side, axisType);
}

// Direction Pad Functions
short getDPad(int player, int side, int side) {
	if (player == P1) {
		if (side == LEFT) { //Ch7
			if (side == UP) {
				return vexRT[Btn7U];
			} else if (side == DOWN) {
				return vexRT[Btn7D];
			} else if (side == LEFT) {
				return vexRT[Btn7L];
			} else if (side == RIGHT) {
				return vexRT[Btn7R];
			} else {
				return 0;
			}
		} else {
			if (side == UP) {
				return vexRT[Btn8U];
			} else if (side == DOWN) {
				return vexRT[Btn8D];
			} else if (side == LEFT) {
				return vexRT[Btn8L];
			} else if (side == RIGHT) {
				return vexRT[Btn8R];
			} else {
				return 0;
			}
		}
	} else {
		if (side == LEFT) { //Ch7
			if (side == UP) {
				return vexRT[Btn7UXmrt2];
			} else if (side == DOWN) {
				return vexRT[Btn7DXmrt2];
			} else if (side == LEFT) {
				return vexRT[Btn7LXmrt2];
			} else if (side == RIGHT) {
				return vexRT[Btn7RXmrt2];
			} else {
				return 0;
			}
		} else {
			if (side == UP) {
				return vexRT[Btn8UXmrt2];
			} else if (side == DOWN) {
				return vexRT[Btn8DXmrt2];
			} else if (side == LEFT) {
				return vexRT[Btn8LXmrt2];
			} else if (side == RIGHT) {
				return vexRT[Btn8RXmrt2];
			} else {
				return 0;
			}
		}
	}
}

short getDPadP1(int side, int side) {
	getDPad(P1, side, side);
}

short getDPadP2(int side, int side) {
	getDPad(P2, side, side);
}

// Trigger Button Functions
short getTrigger(int player, int side, int side) {
	if (player == P1) {
		if (side == LEFT) { //Ch5
			if (side == UP) {
				return vexRT[Btn5U];
			} else if (side == DOWN) {
				return vexRT[Btn5D];
			} else {
				return 0;
			}
		} else { //Ch6
			if (side == UP) {
				return vexRT[Btn6U];
			} else if (side == DOWN) {
				return vexRT[Btn6D];
			} else {
				return 0;
			}
		}
	} else {
		if (side == LEFT) { //Ch5
			if (side == UP) {
				return vexRT[Btn5UXmrt2];
			} else if (side == DOWN) {
				return vexRT[Btn5DXmrt2];
			} else {
				return 0;
			}
		} else { //Ch6
			if (side == UP) {
				return vexRT[Btn6UXmrt2];
			} else if (side == DOWN) {
				return vexRT[Btn6DXmrt2];
			} else {
				return 0;
			}
		}
	}
}

short getTriggerP1(int side, int side) {
	getTrigger(P1, side, side);
}

short getTriggerP2(int side, int side) {
	getTrigger(P2, side, side);
}

/**********************
 ** Motor Processing **
 **********************/

// User Control Functions
void processFourMotorTank() {
	motor[LeftMotorFront] = threshify(vexRT[LeftChannel]);
	motor[LeftMotorBack] = threshify(vexRT[LeftChannel]);
	
	motor[RightMotorBack] = threshify(vexRT[RightChannel]);
	motor[RightMotorFront] = threshify(vexRT[RightChannel]);
}

void processTwoMotorTank() {
	motor[LeftMotor] = threshify(vexRT[LeftChannel]);
	motor[RightMotor] = threshify(vexRT[RightChannel]);
}

void processFourMotorArcade() {
	motor[LeftMotorFront] = threshify(vexRT[xAxisChannel]);
	motor[RightMotorFront] = threshify(vexRT[yAxisChannel]);
	motor[LeftMotorBack] = threshify(vexRT[xAxisChannel]);
	motor[RightMotorBack] = threshify(vexRT[yAxisChannel]);
}

void processTwoMotorArcade() {
	motor[LeftMotor] = threshify(vexRT[xAxisChannel]);
	motor[RightMotor] = threshify(vexRT[yAxisChannel]);
}

// Autonomous Control Functions
/*
 * WORK IN PROGRESS
 */

/***********************
 ** Sensor Processing **
 ***********************/

tSensors gyroscope, accelerometer;
int xAxis = -1, yAxis = -1, zAxis = -1;
int xBias = 0, yBias = 0, zBias = 0;

// Initialize Sensor Functions
void initGyro(tSensors analogPort) {
	gyroscope = analogPort;
	SensorType[gyroscope] = sensorNone;
	wait1Msec(1000);
	SensorType[gyroscope] = sensorGyro;
	wait1Msec(2000);
}

void initAccelerometer(tSensors analogPort, short xPlane, short yPlane, short zPlane) {
	accelerometer = analogPort;
	xAxis = xPlane;
	yAxis = yPlane;
	zAxis = zPlane;

	wait1Msec(500);

	if (xAxis != -1) xBias = SensorBias[xAxis];
	if (yAxis != -1) yBias = SensorBias[yAxis];
	if (zAxis != -1) zBias = SensorBias[zAxis];
}

int getGyro() {
	if (SensorValue[gyroscope] < 0) return -SensorValue[gyroscope];
	else if (SensorValue[gyroscope] > 0) return 3600-SensorValue[gyroscope];
	else return 0;
}

int getAccel(int axisType) {
	if (axisType == X_AXIS && xAxis != -1) {
		return SensorValue[xAxis] - xBias;
	} else if (axisType == Y_AXIS && yAxis != -1) {
		return SensorValue[yAxis] - yBias;
	} else if (axisType == Z_AXIS && zAxis != -1) {
		return SensorValue[zAxis] - zBias;
	} else {
		return 0;
	}
}