harvester.c

typedef enum {
	NO_BALLS, BIG_BALLS, ALL_BALLS,
} HarvestState;
 
typedef enum {
	NO_BALLS, BIG_BALLS, ALL_BALLS,
} HarvestPreState;
 
typedef enum {
	STOPPER_OPEN, STOPPER_CLOSE, WINCH_BIG_ALL, WINCH_BIG_START,
	WINCH_ALL_BIG, WINCH_ALL_START, WINCH_START_BIG, WINCH_START_ALL,
	WINCH_DECIDE, NOTHING,
} HarvestMovement;
 
long harvestTime = nPgmTime;
HarvestState harvestState = NO_BALLS; //0 - no balls, 1 - big balls, 2 - all balls
HarvestPreState harvestPreState = NO_BALLS;
HarvestMovement harvestMode = NOTHING;
long timerCapture = 0;
 
void joyHarvesterState(TJoystick *joyState) {
	harvestTime = nPgmTime;
	switch(harvestMode){
		case NOTHING:
			if (joyButtonPressed(joyState, JOY2, BUTTON_X)) {
					harvestPreState = BIG_BALLS;
			} else if (joyButtonPressed(joyState, JOY2, BUTTON_Y)) {
					harvestPreState = ALL_BALLS;
			}else if (joyButtonPressed(joyState, JOY2, BUTTON_B)) {
					harvestPreState = NO_BALLS;
			}
			if (harvestPreState != harvestState){
				harvestMode = STOPPER_OPEN;
			}
			break;
		case STOPPER_OPEN:
			stopOpen();
			harvestMode = WINCH_DECIDE;
			break;
		case WINCH_DECIDE:
			if((harvestState == BIG_BALLS) && (harvestPreState == ALL_BALLS)){
				winchDown();
				timerCapture = nPgmTime;
				harvestMode = WINCH_BIG_ALL;
			}{ /* "...{HANDLES THE REST OF THE CASES AS WELL} */	
			}
			break;
		case STOPPER_CLOSE:
			stopClose();
			harvestState = harvestPreState;
			harvestMode = NOTHING;
			break;
		case WINCH_BIG_ALL,WINCH_BIG_START,WINCH_START_ALL,WINCH_START_BIG,WINCH_ALL_START,WINCH_ALL_BIG:
		//same for all states except some different values
			if ((harvestTime - timerCapture) > 2500){
				clearServos();
				harvestMode = STOPPER_CLOSE;
			}
			break;
	}