// a function to draw a smiley face
void draw_smiley (void){
int num = 0;
while(checkNoBtns())
{
TMRSRVC_delay(100);
LCD_clear();
if (num % 2==0)
{
draw_face();
draw_smile();
TMRSRVC_delay(2000);
}
else
{
draw_face();
draw_frown();
TMRSRVC_delay(2000);
}
num++;
for(unsigned char row = 0; row < LCD_PIX_HEIGHT; row++)
{
for( unsigned char col = 0; col < LCD_PIX_WIDTH; col++ )
{
LCD_set_pixel(row, col, 0);
}
}
}
}
/*******************************************************************
* Function: int waitButton(void)
* Input Variables: none
* Output Return: int
* Overview: Use a comment block like this before functions
********************************************************************/
int WaitButton( void )
{
BOOL btnState1, btnState2, btnState3;//local variables - button states
//int rtnValue=0;//return the button value
int rtnValue=0;
LCD_clear();
if((ATopstat.state=SUBSYS_OPEN))
{
// Get switch states.
btnState1 = ATTINY_get_SW_state( ATTINY_SW3 );
btnState2 = ATTINY_get_SW_state( ATTINY_SW4 );
btnState3 = ATTINY_get_SW_state( ATTINY_SW5 );
//LCD_printf("btnStates: %d %d %d \n", btnState1, btnState2, btnState3);
if( btnState1 == TRUE )
{
LCD_printf( "SW1: Pushed\n");
// TMRSRVC_delay(500);//wait 1 s
// Assume the LED subsystem opened successfully.
LED_set_pattern( 0b00100000 );//turn the red LED on
TMRSRVC_delay(500);//wait 2 seconds
LED_clr_pattern( 0b01000000 );//turn the green LED off
LED_clr_pattern( 0b00100000 );//turn the red LED off
rtnValue=1;
}//end if button 1 state open
if( btnState2 == TRUE )
{
LCD_printf( "SW2: Pushed\n");
// TMRSRVC_delay(500);//wait 1 s
// Assume the LED subsystem opened successfully.
LED_set_pattern( 0b01000000 );//turn the green LED on
TMRSRVC_delay(500);//wait 2 seconds
LED_clr_pattern( 0b01000000 );//turn the green LED off
LED_clr_pattern( 0b00100000 );//turn the red LED off
rtnValue=2;
}//end if btn 2 open
if ( btnState3 == TRUE )
{
LCD_printf( "SW3: Pushed\n");
// TMRSRVC_delay(500);//wait 1 s
// Assume the LED subsystem opened successfully.
LED_set_pattern( 0b01000000 );//turn the green LED on
LED_set_pattern( 0b00100000 );//turn the red LED on
TMRSRVC_delay(500);//wait 2 seconds
LED_clr_pattern( 0b01000000 );//turn the green LED off
LED_clr_pattern( 0b00100000 );//turn the red LED off
rtnValue=3;
}//end if btn 3 open
LCD_clear();
return rtnValue;
}//end AT while
}//end the WaitButton() function
// ============================ CBOT_main =================================== //
void CBOT_main(void) {
unsigned char sensors;
// Initialize.
init_bot();
// Clear the screen.
LCD_clear();
printMainMenu();
while (1) {
// Delay a little so that the 'Tiny is not overwhelmed with requests.
TMRSRVC_delay( 100 ); // Wait 100ms.
// Get state of all sensors.
sensors = ATTINY_get_sensors();
if ( sensors & SNSR_SW3_EDGE ) {
SPKR_play_tone( SPKR_FREQ( 293.7 ), 250, 80 );
all_pixel_test();
printMainMenu();
} else if ( sensors & SNSR_SW4_EDGE ) {
SPKR_play_tone( SPKR_FREQ( 440 ), 250, 80 );
pixel_sensor_test();
printMainMenu();
} else if ( sensors & SNSR_SW5_EDGE ) {
SPKR_play_tone( SPKR_FREQ( 659.3 ), 250, 80 );
draw_smiley();
printMainMenu();
}
}
}
/*******************************************************************
* Function: char moveTrackLight(void)
* Input Variables: void
* Output Return: char
* Overview: Tracks the light using only light lover behavior.
When it is right infront of the light, it outputs a unique
flag value that suppresses this behavior and inhibits
the moveRetreat behavior.
********************************************************************/
char moveTrackLight(void)
{
// make a variable that keeps track of the light tracking behavior
char isTracking = 0;
// check to see if there is too much light (is the robot in front of the light?)
if((leftLightVolt >= LIGHT_L_MAX)&&(rightLightVolt >= LIGHT_R_MAX))
{
// set global flags to 1
lightFlagStatus = 1;
retreatFlagStatus = 1;
// Act as a docking robot
LCD_printf("Arkin = Docked.\nPreparing to retreat.\n\n");
TMRSRVC_delay(3000);//wait 3 seconds
LCD_clear;
}
// else run moveLight(Lover) behavior
else
{
// inhibit LOVER behavior of move light
isTracking = moveLight(LIGHT_LOVER);
}
return isTracking;
}
void wander(char type)
{
if(type == 'a')
{
while(1){
char robostate='w';
updateLCD();
while (robostate=='w')
{
robostate = aggressiveKid(1,6,3);
getIR();
updateLCD();
TMRSRVC_delay(100);
}
LCD_clear();
LCD_printf("Reason for Stop: %c",robostate);
if(ATTINY_get_SW_state(ATTINY_SW3))
break;
}
}
if(type=='s')
{
while(1){
char robostate='w';
updateLCD();
while (robostate=='w')
{
robostate = shyGuy(1,7,4,4,6);
getIR();
updateLCD();
TMRSRVC_delay(100);
}
LCD_clear();
LCD_printf("Reason for Stop: %c",robostate);
if(ATTINY_get_SW_state(ATTINY_SW3))
break;
}
}
}
/*
* a function that tests that all the pixels in the LCD
* can be turned on an off with the LCD_set_pixel function
*/
void all_pixel_test(void) {
while(checkNoBtns()) {
for(unsigned char row = 0; row < LCD_PIX_HEIGHT; row++)
{
for( unsigned char col = 0; col < LCD_PIX_WIDTH; col++ )
{
LCD_set_pixel(row, col, 1);
}
}
TMRSRVC_delay(2000);
for(unsigned char row = 0; row < LCD_PIX_HEIGHT; row++)
{
for( unsigned char col = 0; col < LCD_PIX_WIDTH; col++ )
{
LCD_set_pixel(row, col, 0);
}
}
TMRSRVC_delay(2000);
}
}
/*******************************************************************
* Function: void moveMap(void)
* Input Variables: void
* Output Return: void
* Overview: moves the robot through the map
********************************************************************/
void moveMap( void )
{
char isDone = 0;
pidController(0,RESET);
switch(currentMove){
case MOVE_LEFT:
move_arc_stwt(POINT_TURN, LEFT_TURN, 30, 30, 0);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_ON);
TMRSRVC_delay(BRAKE_DELAY);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_OFF);
break;
case MOVE_FORWARD:
setOdometry(WALL_STEP);
while(!isDone){
checkIR();
isDone = moveWall();
}
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_ON);
TMRSRVC_delay(BRAKE_DELAY);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_OFF);
// move_arc_stwt(NO_TURN, 45, 10, 10, 0);
break;
case MOVE_RIGHT:
move_arc_stwt(POINT_TURN, RIGHT_TURN, 30, 30, 0);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_ON);
TMRSRVC_delay(BRAKE_DELAY);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_OFF);
break;
default:
LCD_printf("Whatz2?!");
break;
}
}
/*
* a function that can test that sensor data can be output
* to the LCD display in a meaningful manner and the normal
* printf still works correctly
*/
void pixel_sensor_test(void) {
while(checkNoBtns()) {
TMRSRVC_delay(100);
LCD_clear();
float leftIR = getLeftIR();
float rightIR = getRightIR();
float trimLeftIR = trim(2*leftIR, 0, LCD_PIX_WIDTH/2);
float trimRightIR = trim(2*rightIR, 0, LCD_PIX_WIDTH/2);
for(unsigned char i = 0; i < LCD_PIX_HEIGHT; i++) {
LCD_set_pixel(i, LCD_PIX_WIDTH/2, i & 1);
LCD_set_pixel(i, LCD_PIX_WIDTH/2 - trimLeftIR, 1);
LCD_set_pixel(i, LCD_PIX_WIDTH/2-1 + trimRightIR, 1);
}
LCD_set_RC( 0, 0 );
LCD_printf("%.1f", (double)leftIR);
LCD_set_RC( 0, 16 );
LCD_printf("%.1f", (double)rightIR);
}
}
void levelZero(char type,float frontDist)
{
if(type == 'a')
{
while(1){
char robostate='w';
updateLCD();
while (robostate=='w')
{
robostate = aggressiveKid(0,6,3);
getIR();
updateLCD();
TMRSRVC_delay(100);
}
LCD_clear();
LCD_printf("Reason for Stop: %c",robostate);
if(ATTINY_get_SW_state(ATTINY_SW3))
break;
}
}
}
/*******************************************************************
* Function: void map (void)
* Input Variables: none
* Output Return: none
* Overview: Makes the robot map the world
********************************************************************/
void map (void)
{
// Initialize State
isMapping = 1;
// Mapping Loop
while(isMapping)
{
//Sense
checkIR();
checkWorld();
//Record
setGateways();
//Plan using the Map
planMap();
//Act on the Map
moveMap();
//Shift the Map
currentCellWorld = shiftMap(currentCellWorld, currentMove, currentOrientation);
//Break?
isMapping = !((currentCellWorldStart == currentCellWorld)&&(currentOrientationStart == currentOrientation));
if(!isMapping){
break;
}
//Print Map
LCD_clear();
LCD_printf(" Move"BYTETOBINARYPATTERN"\n Cell"BYTETOBINARYPATTERN"\n Ornt"BYTETOBINARYPATTERN"\n\n",BYTETOBINARY(currentMove),BYTETOBINARY(currentCellWorld),BYTETOBINARY(currentOrientation));
printMap(currentOrientation,currentCellWorld,RESET);
TMRSRVC_delay(500);//wait 3 seconds
}
}
void CBOT_main( void )
{
// Initialize Robot
ATopstat = ATTINY_open();//open the tiny microcontroller
LEopstat = LED_open(); //open the LED module
LCopstat = LCD_open(); //open the LCD module
STEPPER_open(); // Open STEPPER module for use
SPKR_open(SPKR_BEEP_MODE);//open the speaker in beep mode
LED_open();
I2C_open();
ADC_open();//open the ADC module
ADC_set_VREF( ADC_VREF_AVCC );// Set the Voltage Reference first so VREF=5V.
// // Print a debug statement
// LCD_printf("It's ALIVE\n\n\n\n");
// TMRSRVC_delay(3000);//wait 3 seconds
// LCD_clear();
// Print the metric Map
// printMetricMap();
// LCD_printf("%i %i %i %i \n%i %i %i %i \n%i %i %i %i \n%i %i %i %i \n",ROBOT_METRIC_WORLD[0][0],ROBOT_METRIC_WORLD[0][1],ROBOT_METRIC_WORLD[0][2],ROBOT_METRIC_WORLD[0][3],ROBOT_METRIC_WORLD[1][0],ROBOT_METRIC_WORLD[1][1],ROBOT_METRIC_WORLD[1][2],ROBOT_METRIC_WORLD[1][3],ROBOT_METRIC_WORLD[2][0],ROBOT_METRIC_WORLD[2][1],ROBOT_METRIC_WORLD[2][2],ROBOT_METRIC_WORLD[2][3],ROBOT_METRIC_WORLD[3][0],ROBOT_METRIC_WORLD[3][1],ROBOT_METRIC_WORLD[3][2],ROBOT_METRIC_WORLD[3][3]);
// TMRSRVC_delay(10000);//wait 10 seconds
// wavefrontMake();
// LCD_printf("%i %i %i %i \n%i %i %i %i \n%i %i %i %i \n%i %i %i %i \n",ROBOT_METRIC_WORLD[0][0],ROBOT_METRIC_WORLD[0][1],ROBOT_METRIC_WORLD[0][2],ROBOT_METRIC_WORLD[0][3],ROBOT_METRIC_WORLD[1][0],ROBOT_METRIC_WORLD[1][1],ROBOT_METRIC_WORLD[1][2],ROBOT_METRIC_WORLD[1][3],ROBOT_METRIC_WORLD[2][0],ROBOT_METRIC_WORLD[2][1],ROBOT_METRIC_WORLD[2][2],ROBOT_METRIC_WORLD[2][3],ROBOT_METRIC_WORLD[3][0],ROBOT_METRIC_WORLD[3][1],ROBOT_METRIC_WORLD[3][2],ROBOT_METRIC_WORLD[3][3]);
// TMRSRVC_delay(10000);//wait 10 seconds
// LCD_clear();
// Test the 4-Neighbor search
// LCD_printf("%i %i %i %i \n%i %i %i %i \n%i %i %i %i \n%i %i %i %i \n",ROBOT_METRIC_WORLD[0][0],ROBOT_METRIC_WORLD[0][1],ROBOT_METRIC_WORLD[0][2],ROBOT_METRIC_WORLD[0][3],ROBOT_METRIC_WORLD[1][0],ROBOT_METRIC_WORLD[1][1],ROBOT_METRIC_WORLD[1][2],ROBOT_METRIC_WORLD[1][3],ROBOT_METRIC_WORLD[2][0],ROBOT_METRIC_WORLD[2][1],ROBOT_METRIC_WORLD[2][2],ROBOT_METRIC_WORLD[2][3],ROBOT_METRIC_WORLD[3][0],ROBOT_METRIC_WORLD[3][1],ROBOT_METRIC_WORLD[3][2],ROBOT_METRIC_WORLD[3][3]);
// TMRSRVC_delay(5000);
// LCD_clear();
currentCellWorld = WORLD_CELL[0][0];
wavefrontMake();
while(currentCellWorld!=reachedEnd){
nextOrientation = fourNeighborSearch(currentCellWorld);
metricMove();
moveMap();
currentCellWorld = shiftMap(currentCellWorld, currentMove, currentOrientation);
}
LCD_printf("LOOOOOOOOOOOOLZ");
TMRSRVC_delay(5000);
LCD_clear();
// Unit test the function
// sqrt function
// int xDelta, yDelta;
// float distance;
// xDelta = abs(-3);
// yDelta = abs(0);
// distance = sqrt(((xDelta*xDelta)+(yDelta*yDelta)));
// LCD_printf("xDel = %d\nyDel = %d\ndist = %f\n\n",xDelta,yDelta,distance);
// TMRSRVC_delay(5000);
// LCD_clear();
// cell values
}// end the CBOT_main()
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()
/*******************************************************************
* Function: char getGateways(void)
* Input Variables: void
* Output Return: void
* Overview: Interpolates the list of gateways in the path by
using the map and initial conditions
********************************************************************/
void getGateways(void)
{
// Get the start location of the robot
// unsigned char curRow = (currentCellWorld>>2) & 0b1100;
// unsigned char curCol = currentCellWorld & 0b0011;
unsigned char curRow = currentCellWorld >> 2;
unsigned char curCol = currentCellWorld & 0b0011;
// Git the start orientation of the robot
unsigned char curOrient = currentOrientation;
// This will be the gatway the robot will look for
unsigned char curCell;
// This will be the move the robot will preform
unsigned char curMove;
// This is the index of the move we are looking at
unsigned char j;
for (j = 0; j<=MAX_MOVE_SIZE; j++)
{ // Get the current move
curMove = moveCommands[j];
// Get the current cell
curCell = ROBOT_WORLD[curRow][curCol];
// Rotate the cell with reference to the robot
curCell = rotateCell(curCell,curOrient,1);
// Store the cell as a searchable gateway
moveGateways[j] = curCell;
// If we are moving forward
// move to the next cell with respect to our orientation
if (curMove == MOVE_FORWARD){
switch(curOrient){
case NORTH:
curRow -= 1;
break;
case EAST:
curCol += 1;
break;
case SOUTH:
curRow += 1;
break;
case WEST:
curCol -= 1;
break;
default:
break;
}
}
// If we are turning right
// then rotate our map orientation appropriately
else if (curMove == MOVE_RIGHT){
curOrient++;
curOrient = curOrient&0b11;
// LCD_clear();
// LCD_printf("Num:\n%i\curOrient:\n"BYTETOBINARYPATTERN,j,BYTETOBINARY(curOrient));
// TMRSRVC_delay(500);//wait 1/2 seconds
}
// If we are turning left
// then rotate our map orientation appropriately
else if (curMove == MOVE_LEFT){
// if(curOrient == 0){
// curOrient = 0b0011;
// }
curOrient--;
curOrient = curOrient&0b11;
}
}
for (j = 0; j<=MAX_MOVE_SIZE; j++)
{
curCell = moveGateways[j];
LCD_clear();
LCD_printf("Num:\n%i\nCurCell:\n"BYTETOBINARYPATTERN,j,BYTETOBINARY(curCell));
TMRSRVC_delay(1000);//wait 1 second
}
}
