| Revised IgORElib | Silver Sumo Hardware Routines | Guardian Program |
I'll be the first to say that most of the code on this page is fluff. Code written to do blinky lights and just to test certain routines. This was written before I had coded the separate hardware routines. The main code for the sumo routine is in YELLOW.
You can download the code from here
// =========================================================================== // Basic Sumo Program - Igore Ottawa Robotics Enthusiasts // =========================================================================== // // Requires Version 1.02 of IgoreLib // // Demonstration of a basic sumo robot algorithm to stay in the ring, // and to find an opponent and charge at them. // // ===========================================================================
#define NC_TIME_GENERAL 75
//#define NC_DEBUG
#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 <stdlib.h> #include <string.h> #include <ctype.h>
// *************************** Main Global Variables ************************* uint16 Meter_Bitmap = 0; uint8 Meter_Mode = AUTO_UPDATE;
// *************************** Function Declarations ************************* void main( void ); void int_to_hex( uint16 , uint8 , char * ); void Display_Demo( void );
// ---------------------------------------------------------------------------
// main()
// ---------------------------------------------------------------------------
void main(void)
{
int16 ir_scan=0;
uint8 scan_index = 0;
uint8 demo_bitmap = 0;
#ifdef NC_DEBUG char szBuffer[16]; #endif
igore_init();
Light_Display(0xAA); Meter_Display(0xAAAA);
set_status_led( ON ); delay_ms(250);delay_ms(250);delay_ms(250);delay_ms(250);
Light_Display(0xFF); // Meter_Display(0xFFFF); Meter_Set(0x01, 0); delay_ms(2*NC_TIME_GENERAL); Meter_Set(0x03, 0); delay_ms(2*NC_TIME_GENERAL); Meter_Set(0x07, 0); delay_ms(2*NC_TIME_GENERAL); Meter_Set(0x0F, 0); delay_ms(2*NC_TIME_GENERAL); Meter_Set(0x1F, 0); delay_ms(2*NC_TIME_GENERAL);
set_status_led( OFF );
set_motor(BOTH, FORWARD, 0);
Meter_Mode = OFF;
Display_Demo();
clear_bumper();
Meter_Mode = AUTO_UPDATE; Meter_Set(0, 0);
while(!was_bumper_hit(TRUE))
{
demo_bitmap = 0x01;
Meter_Set(read_sharp_cm(RIGHT), 1);
for(scan_index=0; scan_index < 8; scan_index++)
{
Light_Display(demo_bitmap);
demo_bitmap = demo_bitmap << 1;
delay_ms(NC_TIME_GENERAL);
}
}
clear_bumper();
while(!was_bumper_hit(TRUE))//for(;;)
{
if (is_line(OUTSIDE_RIGHT)) // Line on front right
{
Light_Display(0xE0);
set_motor(BOTH, BACKWARD, 100); // Reverse
delay_ms(250);delay_ms(250);delay_ms(250);
set_motor(LEFT, BACKWARD, 100); // Spin CW around
delay_ms(10);
set_motor(RIGHT, FORWARD, 100);
delay_ms(250);delay_ms(250);delay_ms(250);delay_ms(250);delay_ms(250);
}
else if (is_line(OUTSIDE_LEFT)) // Line on front left
{
Light_Display(0x83);
set_motor(BOTH, BACKWARD, 100); // Reverse
delay_ms(250);delay_ms(250);delay_ms(250);
set_motor(RIGHT, BACKWARD, 100); // Spin CCW around
delay_ms(10);
set_motor(LEFT, FORWARD, 100);
delay_ms(250);delay_ms(250);delay_ms(250);delay_ms(250);delay_ms(250);
}
else // No lines found, acquire Target
{
Light_Display(0x80);
ir_scan = read_sharp_ir(RIGHT);
#ifdef NC_DEBUG int_to_hex( ir_scan, 4, szBuffer ); putrsUSART( (rom char *)" First 0x" ); putsUSART( szBuffer ); putrsUSART( (rom char *)"\n\r" ); #endif // NC_BEUG
if (ir_scan < 0x75) // No target in line of sight, start sweeping
{
#ifdef NC_DEBUG
int_to_hex( ir_scan, 4, szBuffer );
putrsUSART( (rom char *)" Third 0x" );
putsUSART( szBuffer );
putrsUSART( (rom char *)"\n\r" );
#endif // NC_BEUG
set_motor(LEFT, FORWARD, 80); // Scan CCW delay_ms(10); set_motor(RIGHT, BACKWARD, 80);
for(scan_index = 0; scan_index > 5; scan_index++)
{
ir_scan = read_sharp_ir(RIGHT);
#ifdef NC_DEBUG
int_to_hex( ir_scan, 4, szBuffer );
putrsUSART( (rom char *)" Second 0x" );
putsUSART( szBuffer );
putrsUSART( (rom char *)"\n\r" );
#endif // NC_BEUG
if (ir_scan < 0x75)
delay_ms(50);
else
{
//set_motor(BOTH, FORWARD, 100); // Target Sighted, RAMING SPEED!
Light_Display(0x00); // Was 0xE3
set_motor(LEFT, FORWARD, 100);
delay_ms(10);
set_motor(RIGHT, FORWARD, 100);
break;
}
}
}
else
{
Light_Display(0x00); // Was 0xE3
set_motor(LEFT, FORWARD, 100);
delay_ms(10);
set_motor(RIGHT, FORWARD, 100);
}
}
} }
#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
//****************************************************************************
// Light_Display
//
// Additional hardware to the igore produced by Nicholas Cogghe has required
// certain additional calls to acces this hardware. Display lights for the
// top of Igore have been added for debugging and/or entertainment purposes.
//
// This hardware is a simple shift register attached to 8 LEDs.
//
//TODO: no control over the Master Reset from the shift register is
//available, this would be handy in the future.
//
//****************************************************************************
void Light_Display( uint8 pattern )
{
int index;
for(index = 0; index < 8; index++)
{
PORTDbits.RD3 = !(0x01 & pattern); // Put Data to Pin D3
//delay_us(5); // Pause for Effect/Applause
PORTDbits.RD2 = 1;
//delay_us(5); //TODO: This could be shorter?
PORTDbits.RD2 = 0;
pattern = pattern >> 1; // Lather, Rinse, Repeat.
}
}
//****************************************************************************
// Display_Demo
//
// This function will run through various display demos with the top lights
//****************************************************************************
void Display_Demo( void )
{
uint8 scan_index = 0;
uint16 meter_index = 1;
clear_bumper();
while(!was_bumper_hit(TRUE))
{
Meter_Display(meter_index);
meter_index = meter_index << 1;
switch(scan_index++)
{
case 0:
Light_Display(0x22);
delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL);
break;
case 1: Light_Display(0x44); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
case 2: Light_Display(0x88); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
case 3: Light_Display(0x11); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
case 4: Light_Display(0x22); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
case 5: Light_Display(0x11); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
case 6: Light_Display(0x88); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
case 7: Light_Display(0x44); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
case 8: Light_Display(0x22); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
case 9: Light_Display(0x02); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
case 10: Light_Display(0x05); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
case 11: Light_Display(0x88); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
case 12: Light_Display(0x50); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
case 13: Light_Display(0x20); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
case 14: Light_Display(0x50); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
case 15: Light_Display(0x88); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
case 16: Light_Display(0x05); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
case 17: Light_Display(0x02); delay_ms(NC_TIME_GENERAL);delay_ms(NC_TIME_GENERAL); break;
default: meter_index = 1; scan_index = 0; } }
Light_Display(0x00); set_status_led( ON );
Light_Display(0x80); delay_ms(250);delay_ms(250);delay_ms(250);delay_ms(250); Light_Display(0xC1); delay_ms(250);delay_ms(250);delay_ms(250);delay_ms(250); Light_Display(0xE3); delay_ms(250);delay_ms(250);delay_ms(250);delay_ms(250); Light_Display(0xF7); delay_ms(250);delay_ms(250);delay_ms(250);delay_ms(250);
for(scan_index = 0; scan_index < 10; scan_index++)
{
Light_Display(0xFF);
delay_ms(50);
Light_Display(0x00);
delay_ms(50);
Meter_Display(scan_index);
}
set_status_led( FLASH ); }
//****************************************************************************
// Meter_Display
//
// Additional hardware to the igore produced by Nicholas Cogghe has required
// certain additional calls to acces this hardware. Display lights for the
// top of Igore have been added for debugging and/or entertainment purposes.
//
// This hardware is a simple shift register attached to 8 LEDs. (NOTE: Last
// bit is not connected to any LED in hardware)
//
//TODO: no control over the Master Reset from the shift register is
//available, this would be handy in the future.
//
//****************************************************************************
void Meter_Display( uint16 pattern )
{
int index;
for(index = 0; index < 16; index++)
{
PORTDbits.RD4 = !(0x01 & pattern); // Put Data to Pin D3
// Pause for Effect/Applause
PORTAbits.RA4 = 1; // Clock goes up,
PORTAbits.RA4 = 0; // Clock goes down
pattern = pattern >> 1; // Lather, Rinse, Repeat.
}
}
void Meter_Set(uint8 pattern, uint8 meter)
{
uint16 new_pattern = 0x001F & pattern; // Each meter only contains 5 LEDs
switch(meter)
{
case 0: // Set all meters to same value
Meter_Bitmap = (Meter_Bitmap & 0xFFE0) | new_pattern;
new_pattern = new_pattern << 5;
Meter_Bitmap = (Meter_Bitmap & 0xFC1F) | new_pattern;
new_pattern = new_pattern << 5;
Meter_Bitmap = (Meter_Bitmap & 0x83FF) | new_pattern;
break;
case 1: // Meter 1, top row, LED 1 (far left/outside)
Meter_Bitmap = (Meter_Bitmap & 0xFFE0) | new_pattern;
break;
case 2: // Meter 2, Left Verticle Row (Outside), LED 1 bottom new_pattern = new_pattern << 5; Meter_Bitmap = (Meter_Bitmap & 0xFC1F) | new_pattern; break;
case 3: // Meter 3, Right Verticle Row (Inside), LED 1 bottom new_pattern = new_pattern << 10; Meter_Bitmap = (Meter_Bitmap & 0x83FF) | new_pattern; break;
default: // Wrong Meter Selection, Do Nothing. break; } }
void Meter_Update( void )
{
int index;
uint16 pattern = Meter_Bitmap;
for(index = 0; index < 16; index++)
{
PORTDbits.RD4 = !(0x01 & pattern); // Put Data to Pin D3
// Pause for Effect/Applause
PORTAbits.RA4 = 1; // Clock goes up,
PORTAbits.RA4 = 0; // Clock goes down
pattern = pattern >> 1; // Lather, Rinse, Repeat.
}
}