Josh harvester code

joshharvester.c

/* The winch has three settings:
1. The starting setting, this doesn't allow any balls to be selected.
2. The middle position which only picks up big balls.
3. The last and closest to the floor position that collects big and small balls.
In all these positions the stopper will be in a closed position in case that any
other robot hits our robot, the robot won't break our stopper.
 
The enum HarvestState represents the current setting the winch is on.
*/
 
typedef enum {
NO_BALLS,
BIG_BALLS,
ALL_BALLS,
} HarvestState;
 
/*
The enum HarvestPreState represents the setting we want the winch to change
into.
*/
 
typedef enum {
NO_BALLS,
BIG_BALLS,
ALL_BALLS,
} HarvestPreState;
 
/*
The enum HarvestMovement is a statemachine for which direction to run the winch
and the stopper.
*/
 
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;
 
/*
To know how long to run the winch (to a certain setting) we use nPgmTime, which
allows us to not have to reset the timer every 30 seconds, like the nxtTimers
force us to. timerCapture is for when the winch starts to run and when the winch
setting is changed timerCapture becomes nPgmTime.
 
The program starts by setting harvestTime as nPgmTime. Then the program begins
with the state NOTHING and goes through the states accordingly.
*/
 
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;
 
/*
States:
- NOTHING: The program is waiting for the driver to send it a common, if the
buttons X, Y or B are pressed and the robot isn't already at that position then
the robot will move to the state STOPPER_OPEN.
 
- STOPPER_OPEN: Opens the stopper by running the servo to the preset value and
then continues on to WINCH_DECIDE.
 
- WINCH_DECIDE: In this state the program determines how long to run the winch
because the winch doesn't have an encoder and it needs to go to a preset
position it is necessary to time how long it takes the motor to reach that
position. The time varies with different positions and WINCH_DECIDE chooses
which timer to use based on the current position and the change in position.
 
- WINCH_BIG/START/ALL_BIG/START/ALL: All these starts and variation of the
states just run the motor for the specified timer value. The first BIG/START/ALL
specifies the current position of the winch (starting position, collecting only
big balls position, collecing all balls position). The second one specifies the
position the driver wants the winch to go to. After this the state becomes
STOPPER_CLOSE.
 
- STOPPER_CLOSE: closes the stopper by running the servo to the preset value.
Then it changes the position wanted to the current position and sets the state
back to NOTHING.
*/
 
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;
}