| Revised IgORElib | Hardware Routines | Main Sumo Program |
You can download the files from here
//************************ Function Deffinitions ****************
void main( void );
void Wakeup_Routine( void );
uint8 Wait_For_Target( void );
uint8 Scan_For_Target( uint8 method );
void Angry_Dance( void );
#ifdef NC_DEBUG
void int_to_hex( uint16 , uint8 , char * );
// Needed for ANSI debugging purposes
#endif
//************************ Globals ******************************
extern uint8 Display_Bitmap;
extern uint8 display_index;
extern uint8 Meter_Scan;
extern uint8 Meter_Mode;
//*************************************************************** // Guardian Mode //*************************************************************** // This is a program, mainly being created for demonstrating // social interaction between the Silver Sumo and co-workers. // // Silver Sumo has been dubbed my mascott at work, and as such // should be programmed accordinly. I have created a temporary // arena for the Silver Sumo to roam around in while at work. // This arena is directly infront of the common printer for my // group. While the Silver Sumo is located near the printer, // it will also be dubbed the "Guardian of the Printer". // // This is the purpose of this program. // // Nicholas Cogghe - March, 2003 //***************************************************************
#define NC_DEBUG // Comment this line if you don't want a debug data stream coming out the RS232 port
#include "igorelib.h"
#if !defined( IGORE_LIB_VID ) || ( IGORE_LIB_VID != 102 ) #error "You must use IgoreLib Version 1.02 for this Program" #endif
#include "SS_Hardware.h" #include "Guardian2.h"
#include <stdlib.h> #include <string.h> #include <ctype.h>
#define BACK_LEFT INSIDE_LEFT #define BACK_RIGHT INSIDE_RIGHT #define FRONT_LEFT OUTSIDE_LEFT #define FRONT_RIGHT OUTSIDE_RIGHT
#define HAPPY_APPROACH 1 #define WAG_TAIL_WAIT 2 #define TOO_CLOSE_BAD 3 #define LOST_FRIEND 4
// *********************** Main Global Variables **************** #ifdef NC_DEBUG char szBuffer[20]; // Used for terminal streaming of data #endif
uint8 general_counter = 0; // used mainly for for loops and such
// *********************** Main Application Thread **************
void main ()
{
uint8 Attitude = 0;
uint8 sharp_distance = 0;
// ************************* Startup **************************** igore_init();
if ( read_switch(1) ) // This was added to bypass the warmup sequence because sometimes it just gets annoying =)
{
Reset_Hardware();
Wakeup_Routine(); }
// ************************* Body ******************************
clear_bumper();
set_status_led( FLASH );
while(!(was_bumper_hit(TRUE) == RIGHT))
{
#ifdef NC_DEBUG
putrsUSART( (rom char *)"\n\n\r Guardian Mode Activated" );
putrsUSART( (rom char *)"\n\r -----------------------\n" );
#endif // NC_BEUG
Attitude = HAPPY_APPROACH; //Reset Attitude... SS not scared/angry anymore
Light_Display(0xAA); Check_Battery();
#ifdef NC_DEBUG putrsUSART( (rom char *)"\n\r Finding Line" ); #endif // NC_BEUG
while( is_not_line(FRONT_RIGHT) | is_not_line(FRONT_LEFT) )
{
if( is_not_line(FRONT_LEFT) )
set_motor(LEFT, FORWARD, 70);
else
set_motor(LEFT, BACKWARD, 70);
if( is_not_line(FRONT_RIGHT) ) set_motor(RIGHT, FORWARD, 70); else set_motor(RIGHT, BACKWARD, 70); }
#ifdef NC_DEBUG putrsUSART( (rom char *)"\n\r Line Found, Centering\n" ); #endif // NC_BEUG
set_motor(BOTH, BACKWARD, 100); // SS has found white line, and is moving delay_ms(250);delay_ms(250); // back to centre of ring set_motor(BOTH, FORWARD, 0); // Stop in centre
if ( !Wait_For_Target() )
{
#ifdef NC_DEBUG
putrsUSART( (rom char *)"\n\r Stupid Bumper" );
#endif // NC_BEUG
continue; // Bumper triggered a FALSE target lock, reset }
#ifdef NC_DEBUG putrsUSART( (rom char *)"\n\r Target Acquired" ); #endif // NC_BEUG
Sound_Ramp(2000, 4000, 500); delay_ms(100); Sound_Ramp(2000, 4000, 500);
while(Attitude != 0 & (!(was_bumper_hit(FALSE) == RIGHT)))
{
switch(Attitude)
{
case HAPPY_APPROACH: // Close in till 15cm away(or line) then wag_tail_wait
if( read_sharp_cm(LEFT) > 20 & read_sharp_cm(LEFT) < 30 )
{
#ifdef NC_DEBUG
putrsUSART( (rom char *)"\r Approaching Target" );
#endif // NC_BEUG
if ( is_not_line(FRONT_RIGHT) ) set_motor(RIGHT, FORWARD, 80); else set_motor(RIGHT, FORWARD, 0);
if ( is_not_line(FRONT_LEFT) ) set_motor(LEFT, FORWARD, 80); else set_motor(LEFT, FORWARD, 0);
}
else
{
Attitude = WAG_TAIL_WAIT;
set_motor(BOTH, FORWARD, 0);
}
sharp_distance = read_sharp_cm(LEFT);
if( sharp_distance < 10 )
{
#ifdef NC_DEBUG
putrsUSART( (rom char *)"\n\r Target Approaching Too Fast, Guardian Protect!" );
#endif // NC_BEUG
Attitude = TOO_CLOSE_BAD;
}
else if ( sharp_distance < 20 )
{
#ifdef NC_DEBUG
putrsUSART( (rom char *)"\n\r Safe Distance Attained" );
#endif // NC_BEUG
Attitude = WAG_TAIL_WAIT; } break;
case WAG_TAIL_WAIT: // Little noise (purring??)/stay still/dancing lights
for(general_counter = 0; general_counter < 3 & Attitude == WAG_TAIL_WAIT; general_counter++)
{
delay_ms(150);
switch(general_counter)
{
case 0:
Light_Display(0x88);
break;
case 1:
Light_Display(0x83);
break;
case 2:
Light_Display(0xE0);
//Sound(800);delay_ms(150);
#ifdef NC_DEBUG
putrsUSART( (rom char *)"\r Happy Happy, Joy Joy =)" );
#endif // NC_BEUG
break;
}
Check_Sharp();
sharp_distance = read_sharp_cm(LEFT);
if (sharp_distance < 15)
{
#ifdef NC_DEBUG
putrsUSART( (rom char *)"\n\r Target Approaching Too Fast, Guardian Protect!\n" );
#endif // NC_BEUG
Attitude = TOO_CLOSE_BAD;
}
else if (sharp_distance > 25)
{
#ifdef NC_DEBUG
putrsUSART( (rom char *)"\n\r Target Lost!" );
#endif // NC_BEUG
Attitude = LOST_FRIEND;
}
}
//delay_ms(250);delay_ms(250);
break;
case TOO_CLOSE_BAD: //growl/back away
#ifdef NC_DEBUG putrsUSART( (rom char *)"\r FEND OFF TARGET!" ); #endif // NC_BEUG
if ( is_not_line(BACK_RIGHT) ) set_motor(RIGHT, BACKWARD, 80); else set_motor(RIGHT, FORWARD, 0);
if ( is_not_line(BACK_LEFT) ) set_motor(LEFT, BACKWARD, 80); else set_motor(LEFT, FORWARD, 0);
if ( is_line(BACK_LEFT) & is_line(BACK_RIGHT) )
{
//Angry_Dance(); //TODO: Get Angry Here
Attitude = WAG_TAIL_WAIT;
}
sharp_distance = read_sharp_cm(LEFT);
if (sharp_distance > 25)
{
set_motor(BOTH, FORWARD, 0);
#ifdef NC_DEBUG
putrsUSART( (rom char *)"\n\r Target Lost!" );
#endif // NC_BEUG
Attitude = LOST_FRIEND;
}
else if (sharp_distance > 15)
{
set_motor(BOTH, FORWARD, 0);
#ifdef NC_DEBUG
putrsUSART( (rom char *)"\n\r Resume Approach To Target\n" );
#endif // NC_BEUG
Attitude = WAG_TAIL_WAIT;
}
break;
case LOST_FRIEND: // whimper/howl
//TODO:Whimper here Sound_Ramp(9500, 6000, 450); delay_ms(150);
#ifdef NC_DEBUG
putrsUSART( (rom char *)"\n\r Scanning for Target..." );
#endif // NC_BEUG
if ( Scan_For_Target(SPIN) == TRUE )
{
#ifdef NC_DEBUG
putrsUSART( (rom char *)" TARGET FOUND!\n\r Resume Tracking\n" );
#endif // NC_BEUG
Attitude = HAPPY_APPROACH;
}
else
{
#ifdef NC_DEBUG
putrsUSART( (rom char *)" Target Lost.\n\r Returning to Sentry Position.\n\r ********************\n\n\n\n\n\r" );
#endif // NC_BEUG
Light_Display(0x55);
Attitude = 0; // Lost friend, start over.
}
break;
}
}
}
}
// ************************ Application Routines *****************************
//****************************************************************************
// Wait_For_Target()
//
// This routine will have the robot sit and wait for something to cross it's
// path.
//
//****************************************************************************
uint8 Wait_For_Target( void )
{
uint16 sharp_average = 0;
clear_bumper();
do
{
Check_Sharp();
Check_Battery();
//delay_ms(50);
sharp_average = read_sharp_cm(LEFT); // This was origionally added so that I could average out 2 or 4 consecutive readings to make sure that I was getting correct results.
#ifdef NC_DEBUG putrsUSART( (rom char *)"\r Waiting for Target Acquisition" ); #endif // NC_BEUG
for(general_counter = 0; general_counter < 3 & sharp_average < 30; general_counter++)
{
#ifdef NC_DEBUG
int_to_hex( sharp_average, 4, szBuffer );
putrsUSART( (rom char *)"\n\r Old Sharp Reading : " );
putsUSART( szBuffer );
putrsUSART( (rom char *)" cm" );
#endif //NC_DEBUG
sharp_average = sharp_average + (read_sharp_cm(LEFT) - sharp_average);
// Checking 3 more times to make sure that we have a true target lock
#ifdef NC_DEBUG int_to_hex( sharp_average, 4, szBuffer ); putrsUSART( (rom char *)" Corrected to : " ); putsUSART( szBuffer ); putrsUSART( (rom char *)" cm\n" ); #endif //NC_DEBUG
}
if ( was_bumper_hit(FALSE) == RIGHT ) return FALSE;
}while ( sharp_average > 30 );
#ifdef NC_DEBUG int_to_hex( sharp_average, 4, szBuffer ); putrsUSART( (rom char *)"\n\r Target Acquired at : " ); putsUSART( szBuffer ); putrsUSART( (rom char *)" cm" ); #endif //NC_DEBUG
return TRUE; }
//****************************************************************************
// Scan_For_Target()
//
// This routine will search for a target by scanning back and forth, making
// longer sweeps each time until something is found.
//
// If something is found, the function returns TRUE, if nothing is found
// after a few sweeps, then the function will return FALSE.
//
//****************************************************************************
uint8 Scan_For_Target( uint8 method )
{
int spin_index;
switch(method)
{
case SPIN:
for(general_counter = 0; general_counter < 5; general_counter++)
{
if (general_counter & 0x01)
{
Light_Display(0xF8);
Meter_Set(0x1F, 2);
Meter_Set(0x00, 3);
if ( is_not_line(FRONT_LEFT) ) set_motor(LEFT, FORWARD, 75); else set_motor(LEFT, FORWARD, 0);
if ( is_not_line(BACK_RIGHT) ) set_motor(RIGHT, BACKWARD, 75); else set_motor(RIGHT, FORWARD, 0);
}
else
{
Light_Display(0x8F);
Meter_Set(0x1F, 3);
Meter_Set(0x00, 2);
if ( is_not_line(FRONT_RIGHT) ) set_motor(RIGHT, FORWARD, 75); else set_motor(RIGHT, FORWARD, 0);
if ( is_not_line(BACK_LEFT) ) set_motor(LEFT, BACKWARD, 75); else set_motor(LEFT, FORWARD, 0); }
switch(general_counter)
{
case 0:
spin_index = 5;
break;
case 1:
spin_index = 10;
break;
case 2:
spin_index = 20;
break;
case 3:
spin_index = 30;
break;
case 4:
spin_index = 15; // Back to centre
break;
}
for(; spin_index > 0; spin_index--)
{
if ( read_sharp_cm(LEFT) < 25 )
{
set_motor(BOTH, FORWARD, 0);
return TRUE;
}
delay_ms(50); //TODO: Is Timing Appropriate??
}
set_motor(BOTH, FORWARD, 0);
Check_Sharp();
}
break;
case TACK: // This case is still under development
for(general_counter = 0; general_counter < 5; general_counter++)
{
if (general_counter & 0x01)
{
Light_Display(0xF8);
Meter_Set(0x1F, 2);
Meter_Set(0x00, 3);
if ( is_not_line(FRONT_LEFT) ) set_motor(LEFT, FORWARD, 75); else set_motor(LEFT, FORWARD, 0);
if ( is_not_line(BACK_RIGHT) ) set_motor(RIGHT, BACKWARD, 75); else set_motor(RIGHT, FORWARD, 0);
}
else
{
Light_Display(0x8F);
Meter_Set(0x1F, 3);
Meter_Set(0x00, 2);
if ( is_not_line(FRONT_RIGHT) ) set_motor(RIGHT, FORWARD, 75); else set_motor(RIGHT, FORWARD, 0);
if ( is_not_line(BACK_LEFT) ) set_motor(LEFT, BACKWARD, 75); else set_motor(LEFT, FORWARD, 0); }
switch(general_counter)
{
case 0:
spin_index = 5;
break;
case 1:
spin_index = 10;
break;
case 2:
spin_index = 20;
break;
case 3:
spin_index = 30;
break;
case 4:
spin_index = 15; // Back to centre
break;
}
for(; spin_index > 0; spin_index--)
{
if ( read_sharp_cm(LEFT) < 25 )
{
set_motor(BOTH, FORWARD, 0);
return TRUE;
}
delay_ms(50); //TODO: Is Timing Appropriate??
}
set_motor(BOTH, FORWARD, 0); Check_Sharp(); } break; } return FALSE; }
//****************************************************************************
// Angry_Dance
//
// This function is just to give the robot a bit of personality by having it
// perform some kind of action to demonstrate that it is angry/flustered.
//
//****************************************************************************
void Angry_Dance( void )
{
int index;
Meter_Scan = OFF;
Meter_Set(0xFF, 0); Light_Display(0xE3);
set_motor(LEFT, FORWARD, 75); delay_ms(15); set_motor(RIGHT, FORWARD, 75);
Sound(4000);delay_ms(80); Sound(4000);delay_ms(80); Sound(4000);delay_ms(80); set_motor(BOTH, FORWARD, 0); delay_ms(30);
set_motor(LEFT, BACKWARD, 75); set_motor(RIGHT, BACKWARD, 75); delay_ms(200);
set_motor(BOTH, FORWARD, 0);
Meter_Scan = ON; }
//---------------------------------------------------------------
// Wakeup_Routine
//
// This runs through a routine to demo/test the hardware on
// startup, and to give a bit of attitude to the Silver Sumo.
//
// This is to give some kind of social interaction to any
// viewers, other than just blinky lights.
//---------------------------------------------------------------
void Wakeup_Routine( void )
{
#ifdef NC_DEBUG
putrsUSART( (rom char *)"\n\n\n\n\n\n\n\n\r ********************\n\r *Silver Sumo Online*\n\r ********************\n\n\r -> GUARDIAN MODE SELECTED \n\r" ); // It's my debug, and I'll cry/write what I want to =)
#endif // NC_BEUG
set_status_led( ON ); Meter_Mode = AUTO_UPDATE; delay_ms(250);delay_ms(250); // Delay needed, otherwise LEDs will act funny.
Meter_Set(0x01, 0); // Wake up call! Sound_Ramp(3000, 5000, 1000); delay_ms(100); Meter_Set(0x03, 0); Sound_Ramp(3000, 7000, 750); delay_ms(100); Meter_Set(0x07, 0); Sound_Ramp(3000, 9000, 500); delay_ms(100); Meter_Set(0x0F, 0); delay_ms(200); Meter_Set(0x1F, 0);
delay_ms(250);delay_ms(250); Meter_Set(0x0F, 0); delay_ms(50); Meter_Set(0x07, 0); delay_ms(50); Meter_Set(0x03, 0); delay_ms(50); Meter_Set(0x01, 0); delay_ms(50);
Check_Battery();
delay_ms(250);delay_ms(250); // Move a little Light_Display(0x80); delay_ms(150);
Display_Bitmap = 0x01;
for(display_index = 0; display_index < 8; display_index++)
{
Light_Display(Display_Bitmap);
Display_Bitmap = Display_Bitmap << 1;
delay_ms(150);
}
Display_Bitmap = 0x80;
for(display_index = 0; display_index < 8; display_index++)
{
Light_Display(Display_Bitmap);
Display_Bitmap = Display_Bitmap >> 1;
delay_ms(150);
}
Sound(500); // Either a yawn, or a groan Light_Display(0x00); delay_ms(200); Sound(500); Light_Display(0xAA);
}
#ifdef NC_DEBUG
// ---------------------------------------------------------------------------
// int_to_hex()
//
// converts an integer bit value to a hex string. The integer is passed as a
// uint16, but its real size is given by iSize. iSize is used only to fix the
// size of the string returned (i.e. number of positions for the number).
//
// iSize = 4 for 4 digit output (16 bit number)
// 2 for 2 digit output ( 8 bit number)
//
//
// NOTICE: "Happy Days Code" ... no error checking
//
//
// <!> NOTE <!>: There is a bug associated with address 0x0100 the fix was to
// add 0 at the 17th element of the hex string (?? ! ??)
//
// ---------------------------------------------------------------------------
void int_to_hex( uint16 iVal, uint8 iSize, char *buf )
{
char szHex[] = "0123456789ABCDEF0";
char szTmp[8];
uint16 iTmp;
uint8 idx;
idx = 0; szTmp[idx] = 0;
switch( iSize )
{
case 4:
iTmp = iVal >> 12;
szTmp[idx++] = szHex[iTmp];
szTmp[idx] = 0;
iVal -= ( iTmp << 12 );
iTmp = iVal >> 8;
szTmp[idx++] = szHex[iTmp];
szTmp[idx] = 0;
iVal -= ( iTmp << 8 );
case 2:
iTmp = iVal >> 4;
szTmp[idx++] = szHex[iTmp];
szTmp[idx] = 0;
iVal -= ( iTmp << 4 );
szTmp[idx++] = szHex[iVal];
szTmp[idx] = 0;
}
strcpy( buf, szTmp ); } #endif