Last active
August 29, 2015 14:14
-
-
Save jimmyli97/f904c9b2c052c605dec0 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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: | |
writeDebugStream("Harvester not doing anything \n"); | |
if (joyButtonPressed(joyState, JOY2, BUTTON_X)) { | |
writeDebugStream("moving harvester position->Big\n"); | |
harvestPreState = BIG_BALLS; | |
} else if (joyButtonPressed(joyState, JOY2, BUTTON_Y)) { | |
writeDebugStream("moving harvester position->All\n"); | |
harvestPreState = ALL_BALLS; | |
}else if (joyButtonPressed(joyState, JOY2, BUTTON_B)) { | |
writeDebugStream("moving harvester position->START\n"); | |
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)){ | |
writeDebugStream("moving harvester BIG->All\n"); | |
winchDown(); | |
timerCapture = nPgmTime; | |
harvestMode = WINCH_BIG_ALL; | |
}else if((harvestState == BIG_BALLS) && (harvestPreState == NO_BALLS)){ | |
writeDebugStream("moving harvester BIG->START\n"); | |
winchUp(); | |
timerCapture = nPgmTime; | |
harvestMode = WINCH_BIG_START; | |
}else if((harvestState == ALL_BALLS) && (harvestPreState == NO_BALLS)){ | |
writeDebugStream("moving harvester ALL->START\n"); | |
winchUp(); | |
timerCapture = nPgmTime; | |
harvestMode = WINCH_ALL_START; | |
}else if((harvestState == ALL_BALLS) && (harvestPreState == BIG_BALLS)){ | |
writeDebugStream("moving harvester ALL->BIG\n"); | |
winchUp(); | |
timerCapture = nPgmTime; | |
harvestMode = WINCH_ALL_BIG; | |
}else if((harvestState == NO_BALLS) && (harvestPreState == BIG_BALLS)){ | |
writeDebugStream("moving harvester START->BIG\n"); | |
winchDown(); | |
timerCapture = nPgmTime; | |
harvestMode = WINCH_START_BIG; | |
}else if((harvestState == ALL_BALLS) && (harvestPreState == NO_BALLS)){ | |
writeDebugStream("moving harvester ALL->START\n"); | |
winchDown(); | |
timerCapture = nPgmTime; | |
harvestMode = WINCH_ALL_START; | |
}else if((harvestState == BIG_BALLS) && (harvestPreState == NO_BALLS)){ | |
writeDebugStream("moving harvester BIG->START\n"); | |
winchDown(); | |
timerCapture = nPgmTime; | |
harvestMode = WINCH_BIG_START; | |
}else if((harvestState == NO_BALLS) && (harvestPreState == ALL_BALLS)){ | |
writeDebugStream("moving harvester START->ALL\n"); | |
winchDown(); | |
timerCapture = nPgmTime; | |
harvestMode = WINCH_START_ALL; | |
} | |
break; | |
case STOPPER_CLOSE: | |
stopClose(); | |
writeDebugStream("Stopper Close\n"); | |
harvestState = harvestPreState; | |
harvestMode = NOTHING; | |
break; | |
case WINCH_BIG_ALL: | |
writeDebugStream("Big to All"); | |
writeDebugStream("%d, %d \n", harvestTime, timerCapture); | |
if ((harvestTime - timerCapture) > 2500){ | |
clearServos(); | |
harvestMode = STOPPER_CLOSE; | |
} | |
break; | |
case WINCH_BIG_START: | |
writeDebugStream("Big to Start"); | |
writeDebugStream("%d, %d \n", harvestTime, timerCapture); | |
if ((harvestTime - timerCapture) > 2500){ | |
clearServos(); | |
harvestMode = STOPPER_CLOSE; | |
} | |
break; | |
case WINCH_START_ALL: | |
writeDebugStream("Start to ALL"); | |
writeDebugStream("%d, %d \n", harvestTime, timerCapture); | |
if ((harvestTime - timerCapture) > 2500){ | |
clearServos(); | |
harvestMode = STOPPER_CLOSE; | |
} | |
break; | |
case WINCH_START_BIG: | |
writeDebugStream("Start to Big"); | |
writeDebugStream("%d, %d \n", harvestTime, timerCapture); | |
if ((harvestTime - timerCapture) > 2500){ | |
clearServos(); | |
harvestMode = STOPPER_CLOSE; | |
} | |
break; | |
case WINCH_ALL_BIG: | |
writeDebugStream("All to Big"); | |
writeDebugStream("%d, %d \n", harvestTime, timerCapture); | |
if ((harvestTime - timerCapture) > 2500){ | |
clearServos(); | |
harvestMode = STOPPER_CLOSE; | |
} | |
break; | |
case WINCH_ALL_START: | |
writeDebugStream("All to Start"); | |
writeDebugStream("%d, %d \n", harvestTime, timerCapture); | |
if ((harvestTime - timerCapture) > 2500){ | |
clearServos(); | |
harvestMode = STOPPER_CLOSE; | |
} | |
break; | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment