vex_marblesorter.c

#pragma config(Sensor, in1,    l,              sensorReflection)
#pragma config(Motor,  port1,           f,             tmotorVexFlashlight, openLoop, reversed)
#pragma config(Motor,  port2,           g,             tmotorServoStandard, openLoop)
#pragma config(Motor,  port3,           b,             tmotorServoStandard, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// CONFIG
int kBinSteel     = 100; // The servo angle for the steel marble bin
int kBinYellow    = -80; // The servo angle for the yellow marble bin

int kGateClosed   = -14; // The servo angle for the gate's "closed" position
int kGateOpen     = 127; // The servo angle for the gate's "open" position
int kGateDelay    = 170; // The number of milliseconds for the gate to remain open

int kSteelLower   = 200; // The lower light sensor value for steel marbles
int kSteelUpper   = 250; // The upper light sensor value for steel marbles
int kYellowLower  = 100; // The lower light sensor value for yellow marbles
int kYellowUpper  = 190; // The upper light sensor value for yellow marbles
// END CONFIG

/**
 * An enumeration of all expected marble types.
 */
enum marble {
	kSteel,
	kYellow
};

/**
 * Gets the sign of a number.
 */
int signum(int n);

/**
 * Checks whether a value is within two bounds, inclusive.
 */
bool withinBounds(int n, int lower, int upper);

/**
 * Checks whether the value is within the color threshold for a given marble type.
 */
bool withinThreshold(marble type, int value);

/**
 * Gets the type of marble currently detected by the light sensor.
 */
marble senseType();

/**
 * Instructs the gate to allow one marble through.
 */
void gate();

/**
 * Drives the bin motor to the given position in increments.
 */
void incrementalDrive(int pos);

/**
 * Rotates to the given bin.
 */
void bin(marble type);

task main() {
	motor[f] = 127; // Turn on the flashlight
	motor[g] = kGateClosed; // Move the gate to the "closed" position
	wait(0.8); // Sleep 0.8 seconds
	for (int i = 0; i < 20; i++) { // For 20 times...
		marble type = senseType(); // Read the marble type
		bin(type); // Rotate to the correct bin
		gate(); // Release one marble
		wait(4); // Sleep 4 seconds before next iteration
	}
	bin(-1); // Finally, rotate to 0 before exiting the program
}

int signum(int n) {
	return n == 0 ? 1 : (n / abs(n)); // Calculates signum by dividing a number by its absolute value
}

bool withinBounds(int n, int lower, int upper) {
	return n < upper && n > lower;
}

bool withinThreshold(marble type, int value) {
	switch (type) { // For the given marble type...
		case kSteel: // If it's steel...
			return withinBounds(value, kSteelLower, kSteelUpper); // Check whether the value is valid for steel
		case kYellow: // If it's yellow...
			return withinBounds(value, kYellowLower, kYellowUpper); // Check whether the value is valid for yellow
		default: // If it's neither (for some reason)...
			return false; // Just return false for the check
	}
}

marble senseType() {
	int value = SensorValue[l]; // Read the light sensor value
	int finalOrdinal = (long)kYellow; // Set a variable for the final element in the set of marble types
	for (long i = (long)kSteel; i <= finalOrdinal; i++) { // Iterating through each marble type...
		if (withinThreshold(i, value)) // Check whether the value matches that type
			return (marble)i; // And return that type if it does
	}
	return kSteel; // If no matches are found, default to steel
}

void gate() {
	motor[g] = kGateOpen; // Open the gate
	waitInMilliseconds(kGateDelay); // Wait for a bit
	motor[g] = kGateClosed; // Close the gate
	waitInMilliseconds(kGateDelay); // Wait for a bit again to make sure it's closed
}

void incrementalDrive(int pos) {
	while(abs(motor[b] - pos) > 5) { // While the bin servo's position is not within 5 of the target...
		motor[b] = motor[b] + signum(pos - motor[b]) * 8; // Rotate 8 units in the direction of the target
		waitInMilliseconds(25); // Wait 25 milliseconds before the next iteration
	}
}

void bin(marble type) {
	switch (type) { // For the given marble type...
		case kSteel: // If it's steel...
			incrementalDrive(kBinSteel); // Rotate to the steel bin
			break;
		case kYellow: // If it's yellow...
			incrementalDrive(kBinYellow); // Rotate to the yellow bin
			break;
		default: // Otherwise...
			incrementalDrive(0); // Rotate to zero
			break;
	}
}