/*******************************************************************
* Function: void metric(void)
* Input Variables: none
* Output Return: none
* Overview: Moves the robot to the goal
********************************************************************/
void metric (void)
{
// currentCellWorld = 0b0000;
// currentGoalWorld = 15;
// Make metric map
wavefrontMake();
// Initialize State
isGoal = 0;
unsigned char isSiren = 0;
// Metric Loop
while(!isGoal){
if(isSiren){
SPKR_beep(500);
isSiren = 0;
}
else{
SPKR_beep(250);
isSiren = 1;
}
LCD_clear();
switch(currentOrientation){
case NORTH:
LCD_printf("CurtOrent:NORTH\n");
break;
case EAST:
LCD_printf("CurtOrent:EAST\n");
break;
case SOUTH:
LCD_printf("CurtOrent:SOUTH\n");
break;
case WEST:
LCD_printf("CurtOrent:WEST\n");
break;
default:
break;
}
// Find the next orentation
isGoal = fourNeighborSearch(currentCellWorld);
if(isGoal){
break;
}
switch(nextOrientation){
case NORTH:
LCD_printf("NextOrent:NORTH\n");
break;
case EAST:
LCD_printf("NextOrent:EAST\n");
break;
case SOUTH:
LCD_printf("NextOrent:SOUTH\n");
break;
case WEST:
LCD_printf("NextOrent:WEST\n");
break;
default:
break;
}
switch(currentMove){
case MOVE_LEFT:
LCD_printf("CurMOVE:LEFT\n");
break;
case MOVE_RIGHT:
LCD_printf("CurMOVE:RIGHT\n");
break;
case MOVE_FORWARD:
LCD_printf("CurMOVE:FORWARD\n");
break;
default:
break;
}
// Plan using metric map and next orientation
planMetric();
// Act on the move
moveMap();
// Shift the map
currentCellWorld = shiftMap(currentCellWorld, currentMove, currentOrientation);
// TMRSRVC_delay(2000);//wait 1 seconds
}
SPKR_beep(0);
}
void CBOT_main( void )
{
// initialize the robot
initializeRobot();
currentOrientation = NORTH;
// Ask for Goal
char isDone = 0;
unsigned char btnHolder = UNPRESSED;
LCD_clear();
LCD_printf(" Goal?\n\n\n\n");
while(!isDone){
btnHolder = EnterTopoCommand();
switch(btnHolder){
case MOVE_LEFT:
currentGoalWorld--;
currentGoalWorld = currentGoalWorld&0b1111;
break;
case MOVE_FORWARD:
isDone = 1;
break;
case MOVE_RIGHT:
currentGoalWorld++;
currentGoalWorld = currentGoalWorld&0b1111;
break;
default:
break;
}
printMap(currentOrientation,currentGoalWorld,RESET);
TMRSRVC_delay(100);//wait .1 seconds
}
// Ask for starting orentation
isDone = 0;
btnHolder = UNPRESSED;
LCD_clear();
LCD_printf(" Orient?\n\n\n\n");
while(!isDone){
btnHolder = EnterTopoCommand();
switch(btnHolder){
case MOVE_LEFT:
// If we move left
// shift our oriention CCW
currentOrientation--;
currentOrientation = currentOrientation&0b11;
break;
case MOVE_FORWARD:
isDone = 1;
break;
case MOVE_RIGHT:
// If we move right
// shift our oriention CW
currentOrientation++;
currentOrientation = currentOrientation&0b11;
break;
default:
break;
}
printMap(currentOrientation,currentGoalWorld,RESET);
TMRSRVC_delay(100);//wait .1 seconds
}
// Ask to start
isDone = 0;
btnHolder = UNPRESSED;
LCD_clear();
LCD_printf(" Start?\n\n\n\n");
while(!isDone){
btnHolder = EnterTopoCommand();
switch(btnHolder){
case MOVE_LEFT:
break;
case MOVE_FORWARD:
isDone = 1;
break;
case MOVE_RIGHT:
break;
default:
break;
}
printMap(currentOrientation,currentGoalWorld,RESET);
TMRSRVC_delay(100);//wait .1 seconds
}
// Locilize the Robot
// localize();
// Initialize State
isLost = 1;
oldMove = MOVE_STOP;
// Localization Loop
while(isLost)
{
// Break if not isLost
if(!isLost){
break;
}
//Sense Gateway
checkIR();
checkWorld();
//Plan using the Gateway
planGateway();
//Localize from Gateways?
isLost = localizeGateway();
//Act on the Gateway
moveMap();
}
// Update the currentOrientation using currentMove
switch(currentMove){
// case MOVE_LEFT:
// // If we move left
// // shift our oriention CCW
// currentOrientation--;
// currentOrientation = currentOrientation&0b11;
// break;
case MOVE_FORWARD:
break;
// case MOVE_RIGHT:
// // If we move right
// // shift our oriention CW
// currentOrientation++;
// currentOrientation = currentOrientation&0b11;
// break;
default:
LCD_printf("Whatz2?!");
break;
}
SPKR_beep(500);
// LCD_clear();
// LCD_printf("LOLZ\nI'm found!");
// TMRSRVC_delay(3000);//wait 3 seconds
LCD_clear();
LCD_printf(" New Map\n\n\n\n");
printMap(currentOrientation,currentCellWorld,RESET);
TMRSRVC_delay(1000);//wait 1 seconds
SPKR_beep(0);
// currentCellWorld = 0;
isFire = 0;
// Go firefight
while(!isFire){
//Sense Gateway
checkIR();
checkWorld();
isFire = checkFire();
if(isFire){
break;
}
// Plan using Map
planMap();
// Shift the map
currentCellWorld = shiftMap(currentCellWorld, currentMove, currentOrientation);
// Act on the Map
moveMap();
}
// // Beep for the fire SIREN
// int ii;
// for (ii=0; ii<=3; ii++){
// SPKR_beep(250);
// TMRSRVC_delay(1000);
// SPKR_beep(500);
// TMRSRVC_delay(1000);
// }
// SPKR_beep(0);
// // Print the fire cell location
// LCD_clear();
// LCD_printf("Fire = %i\n\n\n\n", currentFireCell);
// TMRSRVC_delay(5000);
// Moves the Robot to the goal
metric();
// Print that you are at home and the fire cell location
LCD_clear();
LCD_printf("LOLZ\nI'm HOME\nFire at Cell: %i\n\n",currentFireCell);
// Stop when home is reached
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_OFF);
// Beep when home is reached
SPKR_beep(500);
TMRSRVC_delay(3000);//wait 3 seconds
SPKR_beep(0);
TMRSRVC_delay(7000);//wait 7 seconds
/**
// Enter the robot's current (starting) position
LCD_printf("START Map/nlocation\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
worldInput();
TMRSRVC_delay(1000);//wait 3 seconds
LCD_clear();
// Enter the robot's current (starting) orientation
LCD_printf("START Map/norientation\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
orientationInput();
TMRSRVC_delay(1000);//wait 3 seconds
LCD_clear();
// Print the map
LCD_clear();
printMap(currentOrientation,currentCellWorld,RESET);
TMRSRVC_delay(10000);//wait 10 seconds
LCD_clear();
// Enter the robot's current (starting) position
LCD_printf("START Path\nlocation\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
worldInput();
TMRSRVC_delay(1000);//wait 3 seconds
LCD_clear();
// Enter the robot's current (starting) orientation
LCD_printf("START Path\norientation\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
orientationInput();
TMRSRVC_delay(1000);//wait 3 seconds
LCD_clear();
// Enter the robot topological commands
LCD_printf("ENTER Path\ncommands\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
movesInput();
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
// Print the robot gateways
LCD_printf("Robot Gateways:\n\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
getGateways();
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
// Goal loop
while (1)
{
checkIR();
checkWorld();
moveWorld();
//Test arc function
// LCD_printf("Move Arc\n\n\n\n");
// TMRSRVC_delay(1000);//wait 1 seconds
// move_arc_stwt(POINT_TURN, LEFT_TURN, 10, 10, 0);
// //Test contact Sensors
// LCD_printf("Right Contact: %i\nLeft Contact: %i\n\n\n",rightContact,leftContact);
// TMRSRVC_delay(1000);//wait 1 seconds
// // Test IR Sensors
// LCD_clear();
// LCD_printf("FrontIR = %3.2f\nBackIR = %3.2f\nLeftIR = %3.2f\nRightIR = %3.2f\n", ftIR,bkIR,ltIR,rtIR);
// TMRSRVC_delay(1000);//wait 1 seconds
}
**/
}// end the CBOT_main()
//proportional control
char wallFollow(char side, float min, float max, float angle)
{
getIR();
if(ft<=2.5)
{
return 'e';
}
if(side=='r')
{
// Follow right wall
if(rt < min)
{
// move away
rotateDeg(2*angle);
forward(3*move);
return 'r';
}
else if (rt > 2*max)
{
// End of wall probably
// End routine
return 'e';
}
else if (rt > max){
// move towards
rotateDeg(-angle);
forward(move);
return 'r';
}
else
{
//move forwards
forward(move);
return 'r';
}
}else if(side=='l')
{
// Follow left wall
if(lt < min){
// too close
rotateDeg(-.75*angle);
forward(move);
return 'l';
}
else if (lt > 2*max){
// end routine
return 'e';
}
else if (lt > max){
// too far
rotateDeg(angle);
forward(3*move);
return 'l';
}
else
{
forward(move);
return 'l';
}
}
else
{
// Error
SPKR_beep(440);
SPKR_beep(440);
return 'x';
}
}
