Marble Sorter

marble_sorter.c

#pragma config(Sensor, in1,    light,          sensorReflection)
#pragma config(Sensor, dgtl1,  led,            sensorLEDtoVCC)
#pragma config(Motor,  port4,           hopper,        tmotorServoStandard, openLoop)
#pragma config(Motor,  port5,           sorter,        tmotorServoStandard, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// Good luck reading this....just kidding!!!

#define CLEAR 127
#define WHITE 0
#define STEEL -127

void dropMarble();
int checkMarble();
void setSortServo(int type);


task main(){
	int currentMarble;
	while(true){
		turnLEDOn(led);
		wait(1);
		currentMarble = checkMarble();
		wait(0.5);
		setSortServo(currentMarble);
		wait(1);
		dropMarble();
		wait(0.5);
		setSortServo(WHITE);
		turnLEDOff(led);
	}
	turnLEDOff(led);

}

void dropMarble(){
	setServo(hopper, -127);
	wait(0.15);
	setServo(hopper, 10);
}


int checkMarble(){
	wait(0.5);
	int color = SensorValue[light];
	if(color < 400){
		return CLEAR;
	} else if(color < 550){
		return WHITE;
	} else {
		return STEEL;
	}
}

void setSortServo(int pos){
	setServo(sorter, pos)
}