/*
void elevate(unsigned char angle,unsigned char side)
{
if(side==0)
servo_1(angle);
else if(side==1)
servo_1((angle==45)?0:45);
}
void open(unsigned char side)
{
if(side==0)
servo_2(180);
else if(side==1)
servo_3(180);
}
void close(unsigned char side)
{
if(side==0)
servo_2(0);
else if(side==1)
servo_3(0);
}
void pickupSeQ(unsigned char side)
{
elevate(0,side);//lower
open(side);
close(side);
elevate(45,side);//mid
}
void dropSeQ(unsigned char side)
{
elevate(0,side);//lower
open(side);
elevate(45,side);//mid
close(side);
}
*/
unsigned char pickup(unsigned char side) //1=swap 0=term
{
if (arm[side] == 0)
{
if (indi[term[CT][side]] != CT && term[CT][side] != 0)
{
arm[side] = term[CT][side];
term[CT][side] = 0;
//pickupSeQ(side);
return 1;
}
}
else if (arm[!side] == 0)
{
if (indi[term[CT][side]] != CT && term[CT][side] != 0)
{
arm[!side] = term[CT][side];
term[CT][side] = 0;
uTurn();
// pickupSeQ(!side);
uTurn();
return 1;
}
}
return 0;
}
/*-----------------------------------------------------------------------------
*Function name: traverseSimpleTrack
*Description: This function navegates the maze using the simplified instructions
* to get to the desired station faster
*Inputs: maze = Maze * = the actuall maze that the robot will be traversing in this function
* robot = Robot * = the robot that will be traversing the maze
* outTurns = char * = the set of turns that the robot will be fallowing
*Outputs: none
-----------------------------------------------------------------------------*/
void traverseSimpleTrack(Maze *maze, RobotModel *robot, char *outTurns)
{
int done = 0;
int bToL, bToR, bInF, onB;
int turn = 0;
while(!done)
{
clear();
printMazePlusCurrentPos(*maze, *robot);
/*Letting the robot know where the paths are with respect to itself*/
bToL = blackToLeft(*maze, *robot);
bToR = blackToRight(*maze, *robot);
bInF = blackInFront(*maze, *robot);
onB = onBlack(*maze, *robot);
/*If there are no path on either side of the robot, then the robot just moves forward*/
if(bToL == 0 && bToR == 0 && onB == 1)
{
moveStraight(robot);
}
/*If the robot can turn, it checks the outTurns array to see what the next turn it needs to take is*/
/*the turn index turn is the incremented to let the robot know what the next turn is*/
else
{
switch(outTurns[turn])
{
case 'R':
turnRight(robot);
turn++;
break;
case 'L':
turnLeft(robot);
turn++;
break;
case 'S':
moveStraight(robot);
turn++;
break;
case 'U':
uTurn(robot);
break;
default:
break;
}
/*if the robot is not on the maze than it is at its desired station and the function is done running*/
if(!onBlack(*maze, *robot))
{
done = 1;
printw("\nYour station has arrived\n");
}
}
getch();
}
}
unsigned char drop(unsigned char side)
{
if (term[CT][side] == 0)
if (indi[arm[side]] == CT)
{
term[CT][side] = arm[side];
arm[side] = 0;
// dropSeQ(side);
count++;
return 1;
}
else if (indi[arm[!side]] == CT)
{
term[CT][side] = arm[!side];
arm[!side] = 0;
uTurn();
// dropSeQ(!side);
uTurn();
count++;
return 1;
}
return 0;
}
/*-----------------------------------------------------------------------------
* Function name: learn_track
* Description: This function navigates through the maze and solves it
* Inputs: maze = Maze * = pointer to Maze structure
* robot = RobotModel * = pointer to RobotModel
* turns = char * = array of characters to store the turns made to solve
* the maze (passed by simulated reference)
* curTurnIndex = int * = the index position where the next turn
* made by the robot will be stored in turns array
* (passed by simulated reference)
* code = int * = the last administrative code collected
* (passed by simulated reference)
* targetStation = int = the targetStation input by the user
* Output: int = 0 if the targetStation is not found
* targetStation otherwise
*----------------------------------------------------------------------------*/
int learn_track(Maze *maze, RobotModel *robot, char *turns, int *curTurnIndex, int *code, int targetStation)
{
int done = 0;
int uTurned = 0;
int traversalMode = 2; /* e.g. 0: leftHandOnWallMode, 1: rightHandOnWallMode, 2: mainLoopMode */
int station = 0; // returns the station reached
int bToL, bToR, bInF; // valiables indicating black to left(L), right(R), or frontF)
int onB, i; // valiables indicating on black
while (!done)
{
clear();
printMazePlusCurrentPos(*maze, *robot);
/*
TODO:
Determine the values of bToL, bToR, bInF, onB
Determine if the robot has reached the packing station (which means learn track is done but the targetStation was not found)
If the robot is not at a junction and can only move straight, move straight
otherwise
if traversalMode indicates mainLoopMode
decide the appropriate turn following mainLoopMode
NOTE: since the main loop has no dead-end, it is always posible to make a L, R, or S turn
apply the turn on the robot and save it into turns array
else if traversalMode indicates leftHandOnWallMode
decide the appropriate turn following leftHandOnWallMode
decide u-turn when neither of L, R, or S turn is possible (i.e., a dead-end and a new storage station has been reached)
apply the turn on the robot and save it into turns array
else if traversalMode indicates rightHandOnWallMode
decide the appropriate turn following rightHandOnWallMode
decide u-turn when neither of L, R, or S turn is possible (i.e., a dead-end and a new administrative station has been reached)
apply the turn on the robot and save it into turns array
Update the current traversalMode
*/
/*Lets the robot know where the location of the paths of the maze are with respect to itself*/
bToL = blackToLeft(*maze, *robot);
bToR = blackToRight(*maze, *robot);
bInF = blackInFront(*maze, *robot);
onB = onBlack(*maze, *robot);
/*checks to make sure that the robot is not surrouned by open paths because the only time that would happen is when it is at an invalid spot*/
/*if this is true, the program ends the learn_maze function and lets the main function know that there was an error by changing the station variable to 0*/
if(bToL && bToR && bInF && onB)
{
done = 1;
station = 0;
}
/*Checks to see if the only way that the robot can go is forward and if that is true, it moves the robot forward*/
else if(bToL == 0 && bToR == 0 && onB == 1)
{
moveStraight(robot);
}
/*If the previous comparsions are false, the robot checks to see what type of traversal mode it is in*/
/*In "Left hand mode" that robot tries to first turn left, then move straight, then turn right, and then finally U-turn to continue on the path of the maze*/
/*Each turn is recorded in the turns array at the index of curTurnIndex, which is incremented after each turn except for the last turn to let the main function know how many turns there are*/
/*Each time a u-turn is recorded, the station number is incremented unless the robot is at the target station at which point the function is complete*/
else if(traversalMode == 0)
{
if(bToL == 1)
{
turnLeft(robot);
turns[*curTurnIndex] = 'L';
*curTurnIndex += 1;
}
else if(bInF == 1)
{
moveStraight(robot);
}
else if(bToR == 1)
{
turnRight(robot);
turns[*curTurnIndex] = 'R';
*curTurnIndex += 1;
}
else
{
uTurn(robot);
station++;
if(station == targetStation)
{
done = 1;
}
turns[*curTurnIndex] = 'U';
if(!done)
{
*curTurnIndex += 1;
}
uTurned = 1;
}
}
/*In "Right hand mode" that robot tries to first turn right, then move straight, then turn left, and then finally U-turn to continue on the path of the maze*/
/*Each turn is recorded in the turns array at the index of curTurnIndex, which is incremented after each turn except for the last turn to let the main function know how many turns there are*/
/*Each time a u-turn is recorded, the function asks the user for an administrative code. It will not let the user continue until a code greater than 0 is entered. Each time a new code is entered, the old one is deleted*/
else if(traversalMode == 1)
{
if(bToR == 1)
{
turnRight(robot);
turns[*curTurnIndex] = 'R';
*curTurnIndex += 1;
}
else if(bInF == 1)
{
moveStraight(robot);
}
else if(bToL == 1)
{
turnLeft(robot);
turns[*curTurnIndex] = 'L';
*curTurnIndex += 1;
}
else
{
uTurn(robot);
turns[*curTurnIndex] = 'U';
*curTurnIndex += 1;
*code = 0;
do
{
printw("\nGive Administrative Code (>0): ");
scanw("%d", code);
}while(*code <= 0);
uTurned = 1;
}
}
/*In "Main loop mode" that robot tries to first turn left, then turn right, and then move straight the path of the maze*/
/*Each turn is recorded in the turns array at the index of curTurnIndex, which is incremented after each turn except for the last turn to let the main function know how many turns there are*/
/*Each time the robot turns, it checks to see if it is at an intersection to see if it needs to change the traversal mode for the appropriate branch of the maze*/
else if(traversalMode == 2)
{
if(bToL == 1)
{
if(bInF == 1 || bToR == 1)
{
traversalMode = 0;
}
turnLeft(robot);
turns[*curTurnIndex] = 'L';
*curTurnIndex += 1;
}
else if(bToR == 1)
{
turnRight(robot);
if(bInF == 1 || bToL ==1)
{
traversalMode = 1;
}
turns[*curTurnIndex] = 'R';
*curTurnIndex += 1;
}
else
{
moveStraight(robot);
}
}
/*This switch case checks to see if the robot needs to be put back in the main loop and there fore needs to be put back into main loop traversal mode*/
switch(traversalMode)
{
case 0:
if(bToL == 1 && bToR == 1 && uTurned == 1)
{
traversalMode = 2;
uTurned = 0;
}
/*This is a special case to check to see if the robot needs to travel straight and that needs to be recored for the program to simplifiy the instructions*/
else if(bInF == 1 && bToR == 1 && uTurned == 1)
{
traversalMode = 2;
turns[*curTurnIndex] = 'S';
(*curTurnIndex) += 1;
uTurned = 0;
}
break;
case 1:
if(bToL == 1 && bToR == 1 && uTurned == 1)
{
traversalMode = 2;
uTurned = 0;
}
break;
default:
break;
}
getch();
}
return station;
}