// 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 } }