myserial.icb cmuHBtest.ic

/****************/ /* cmuHBtest.ic */ /****************/ /* These are the specifics for this particular robot. */ /* Motor port 0 controls the left motor, polarity is normal, green LED indicates Forward */ /* Motor port 1 controls the right motor, polarity is normal, green LED indicates Forward */ /* Analog port 5 has been modified for the Sharp GP2D12 IR Ranging sensor. Conversions are: */ /* | Distance | White Object | Black Object | */ /*_|__________|______________|______________|_*/ /* | 3" | 137 | 140 | */ /* | 3.5" | 127 | 131 | */ /* | 4" | 117 | 122 | */ /* | 4.5" | 106 | 112 | */ /* | 5" | 97 | 104 | */ /* | 5.5" | 90 | 96 | */ /* | 6" | 84 | 89 | */ /* | 6.5" | 80 | 85 | */ /* | 7" | 75 | 82 | */ /* | 7.5" | 70 | 78 | */ /* | 8" | 66 | 74 | */ /* | 8.5" | 64 | 70 | */ /* | 9" | 61 | 67 | */ /* | 9.5" | 58 | 65 | */ /* | 10" | 56 | 61 | */ /* | 15" | 39 | ?? | */ /* | 20" | 31 | 34 | */ /* | 30+" | 24 | 2 | */ /* CMUcam */ int l=0; /* Define left motor port */ int r=1; /* Define right motor port */ /* These may need to be adjusted to get your bot to go in a straight line **/ int lm=25; /* Left motor forward speed */ int rm=25; /* Right motor forward speed */ int lbump=13; /* Define bumper ports */ int rbump=11; /* The bump sensors are plugged into the HB's digital in's 13 & 11 */ char buff[11]; /*******************************************************************/ /* Main Command code */ /*******************************************************************/ void main() { printf("Press Start to Go!\\n"); while(!start_button()){} tone(800.0,0.5); serial_init(0); sr_init_CMUcam(); while(1){ track(); printf("Pos=%d, Nod=%d Sz=%d, Cnf=%d\\n",buff[1],buff[3],buff[8],buff[9]); /* buff[0]=77 always, buff[1]=Servo Pos, buff[2]=Mx, buff[3]=My */ /* buff[4] to buff[6]=X/Y box co-ords, */ /* buff[8]=size/distance of object, buff[9]=Confidence. */ /* (45 is center of scan and is servo center.) */ if(buff[9] < 49){ ao(); } if(buff[1] > 66 < 94 && buff[8] > 121 < 253 && buff[9] > 50){ ao(); } if(buff[8] < 120 && buff[9] > 50){ Bwd(); } if(buff[8] > 253 && buff[9] >50){ Fwd(); } if(buff[1] > 95 && buff[9] > 50){ Right(); } if(buff[1] < 65 && buff[9] > 50){ Left(); } } } /*************************/ /* Initialize CMU Camera */ /*************************/ void sr_init_CMUcam() { serial_string("RS\r"); serial_wait(0x3a); printf("Camera Reset\\n"); sleep(0.1); serial_string("CR\r"); serial_wait(0x3a); serial_string("PM 0\r"); serial_wait(0x3a); /* serial_string("S1 60\r"); serial_wait(0x3a); sr_blink(); sleep(0.1); */ serial_string("MM 14\r"); serial_wait(0x3a); printf("Middle Mass On\\n"); sleep(0.5); serial_string("CR 18 44 19 32\r"); serial_wait(0x3a); sleep(3.0); printf("Auto White and Gain are Set\\n"); sleep(0.5); printf("Grab Color...\\n"); serial_string("TW\r"); /* Acquire... */ sleep(1.0); serial_string("\r"); serial_wait(0x3a); printf("Color Locked In!\\n"); serial_string("PM 1\r"); serial_wait(0x3a); serial_string("L1 2\r"); serial_wait(0x3a); sleep(2.0); serial_string("RM 3\r"); serial_wait(0x3a); sleep(2.0); } /****************/ /***********************************************************/ /* Receive characters until desired character is received. */ /***********************************************************/ void serial_wait(int c){ int d=0; while(d != c){ d = serial_getchar(0); } } /*******************************************************/ /* Send string of ASCII characters to serial port. */ /* String MUST be terminated with carriage return (/r) */ /*******************************************************/ void serial_string(char c[]) { int x,y=0; while(y != 0x0d){ y = c[x++]; serial_putchar(y); } } /***************************************************/ /* Track Color Command */ /***************************************************/ void track() { int x; serial_string("TC\r"); serial_wait(0xff); for(x=0;x<11;x++){ buff[x]=serial_getchar(0); } } /***************************************************/ /* Motor Drive commands */ /***************************************************/ void Left(){ motor(l,-15); motor(r,15); } void Right(){ motor(l,15); motor(r,-15); } void Bwd(){ motor(l,25); motor(r,25); } void Fwd(){ motor(l,-25); motor(r,-25); } /**************************************/ /* Start Routine */ /**************************************/ void init(){ printf("Press Start to Go!\\n"); /* wait for start button to get */ while(!start_button()){} /* then run program. */ tone(800.,1.); }

org MAIN_START #include "6811regs.asm" * Masks for serial port PORTD_WOM EQU $20 BAUD9600 EQU $B0 TRENA EQU $0C ; Transmit, Receive ENAble RDRF EQU $20 ; Receive Data Register Full TDRE EQU $80 ; Transmit Data Register Empty * initializes the serial port for transmission * and reception of 9600 baud serial data: subroutine_serial_init: ldx #$1000 bclr SPCR,x #PORTD_WOM * turn off wired-or mode ldaa #BAUD9600 * mask for 9600 baud speed staa BAUD,x * stored into BAUD register ldaa #TRENA * mask for Transmit, Rec. enable staa SCCR2,x * store in control register ldy #$1000 * send first character so next ldaa #$40 * one won't get lost staa SCDR,y blah brclr SCSR,y TDRE blah rts * transmits a character and waits for it to finish transmission: subroutine_serial_putchar: ldy #$1000 tba * transfer character into A staa SCDR,y * store A reg in SCDR WCLoop brclr SCSR,y TDRE WCLoop * wait until data sent rts * The following code receives a character from the serial port: subroutine_serial_getchar: ldx #$1000 RCLoop brclr SCSR,x RDRF RCLoop * wait until char ready ldaa SCDR,x * input character to A tab * transfer A to B clra * zero A to make D an int again rts