/****** amigotest_rx2.c ************************************************** * Amigo test program * TX to AmigoBot Enabled * SPI to Master PIC enabled * Receive from AMIGOBOT works also * * ECE4006 * Lim Seang * Walid Mnif * Tamer Shadid ************************************************************************* */ #include /******************************* * PIC Configuration Settings ******************************* */ __CONFIG(1,HS); // HS oscillator __CONFIG(2,PWRTEN & BOREN & BORV42 & WDTDIS); // Resets & disable watchdog __CONFIG(3,CCP2RC1); // CCP2 to RC1 (rather than to RB3) __CONFIG(4,LVPDIS); // RB5 enabled for I/O /******************************* * Definitions and equates ******************************* */ /******************************* * Global variables ******************************* */ char OLDRIGHT; //may be used for rotations char OLDLEFT; //may be used for rotations char OLDSPEED; //used to store old speeds char SPICNT; //counter used blink LED indicating that the spi flag was set char COMMAND; char TESTCNT; char TXCOUNT; char STARTCNT; char ALIVECNT; // Counter for blinking "Alive" LED char TMR1X; // Eight-bit extension to TMR1 char CCPR1X; // Eight-bit extension to CCPR1 char DTIMEL; // Half-period value char DTIMEH; char DTIMEX; long i; //bits used ininterrupt long loopsize; bit TXQUEUE; //bit to see if the queue is empty or not bit STARTED; //bit used to see if the amigobot has been started or not bit TESTENABLE; //bit is set in the LOPRISR bit iSTOP; //bit used in interrupt to distinguish stop bit commandOK; //nocommand was sent bit UPDATESONAR; //bit is set in Hiprisr to tell mainline to update SSPBUF int RXINDEX; //used to count incoming receives in HIPRISR char RXDATA; //data that will be written to sspbuff to //update the other pic with most recent //sonar data char SSPUPDATE; //byte that will be written to SSPBUFF int iSONARDATA; //sonar data array index int RA1LED = 10; //counters for led's used for test int RB5LED = 10; int RB4LED = 10; int RB3LED = 10; long RXMainLineCNT = 100;//just initial value of 1 second between //every time the uart is enabled. /******************************* * Variable arrays ******************************* */ unsigned char TXcommand[] = {0xFA,0xFB,0x06,0x00,0x00,0x00,0x00,0x00,0x00,0x00};//remember last byte isnt used. unsigned char SONARDATA[] = {0x00,0x00,0x00,0x00,0x00,0x00}; unsigned char AmigoSIP[50]; long iAmigoSIP=0;//the counter for the SIP's /******************************* * Constant strings * * For stability reasons, create an EVEN number of elements in any given array ******************************* */ unsigned char sync0[] = {0xFA,0xFB,0x03,0x00,0x00,0x00}; unsigned char sync1[] = {0xFA,0xFB,0x03,0x01,0x00,0x01}; unsigned char sync2[] = {0xFA,0xFB,0x03,0x02,0x00,0x02}; unsigned char open[] = {0xFA,0xFB,0x03,0x01,0x00,0x01}; unsigned char pulse[] = {0xFA,0xFB,0x03,0x00,0x00,0x00}; unsigned char close[] = {0xFA,0xFB,0x03,0x02,0x00,0x02}; unsigned char enable[] = {0xFA,0xFB,0x06,0x04,0x3B,0x01,0x00,0x05,0x3B}; unsigned char vel2[] = {0xFA,0xFB,0x06,0x20,0x3B,0x05,0x05,0x25,0x40}; unsigned char stop[] = {0xFA,0xFB,0x03,0x1D,0x00,0x1D}; unsigned char amigosonar[] = {0xFA,0xFB,0x06,0x1C,0x3B,0x01,0x00,0x1D,0x3B,0x00}; /******************************* * Function prototypes ******************************* */ void traverseSIP(void); void getAmigoData(void); void Initial(void); void AmigoInit(void); void BlinkAlive(void); void LoopTime(void); void send_command(unsigned char * command_data); void TXtest(void); void PULSE(void); void CommandHandler(void); void convertsonardata(void); int exponent(int,int); char sonardatahelper(float); void sspbyteassem(char,char); /******************************* * main() ******************************* */ void main() { Initial(); // Initialize everything while(1) { if(!(--RXMainLineCNT)){ getAmigoData();//it's been 1 sec, get amigobot data } if(!(--RB3LED)){//counters for the testing led's RB3 = 0; } if(!(--RB4LED)){ RB4 = 0; } if(!(--RB5LED)){ RB5 = 0; } if(!(--RA1LED)){ RE2 = 0;//test RE2 once //RA3 = 0; } if(!(--SPICNT))//will blink RA2 LED if spi flag was set { RA2 = 0;//turn off led after .5 seconds if(TESTENABLE) { send_command(vel2); TESTENABLE = 0;//motors enabled and we moving }//end test enable }//end if TESTCNT if(!STARTED)//Check if Amigobot has been initialized { AmigoInit(); } else{ PULSE();//this ensures that a pulse is sent to the amigobot //once every second } if(UPDATESONAR){ //RA1 = 0; convertsonardata(); }//end if updatesonar BlinkAlive(); LoopTime(); // Delay for 10 ms } //end while(1) } /******************************* * Initial() * * This subroutine performs all initializations of variables and registers. ******************************* */ void Initial() { ADCON1 = 0b10001110; // Enable PORTA & PORTE as digital I/O pins TRISA = 0b11100001; // Set I/O for PORTA TRISB = 0b11000100; // Set I/O for PORTB TRISC = 0b10011010; // Set I/O for PORTC TRISD = 0b00001111; // Set I/O for PORTD TRISE = 0b00000000; // Set I/O for PORTE SSPCON1 = 0b00100100; SSPSTAT = 0b01000000; SSPIF = 0; SSPIE = 1; PR2 = 222; // Set B=223 for Timer2 scalar T2CON = 0b00110111; // Set A=16, C=7 for Timer 2 scalar EECON1 = 0; // Set up EECON1 for EEPROM access EEIF = 1; // Clear write-operation flag IPEN = 1; // Enable priority levels for interrupts TMR2IP=0; SSPIP = 0; //********************* iSTOP = 0; SPICNT = 0; TESTENABLE = 0; TXQUEUE = 0; // initially no commands exist in the queue STARTED=0; // if 1, then the amigobot has been initialized SPBRG = 64; // assigning 19200 baud TXSTA = 0b00100100; RCSTA = 0b10010000; TXIE = 0; //disables interupts for uart RCIE = 0; //initially set to zero until after initialization //of amigobot UPDATESONAR = 0; commandOK = 1; iSONARDATA = 0; RXINDEX = 0; //initialize the index of receives from amigobot RCIP = 1; // set receive to high priority ALIVECNT = 250; // Setup counter for blinking "Alive" LED TXCOUNT = 10; TESTCNT = 50; STARTCNT = 0; DEBUG = 0; RE2 = 1;//debugging thing } /******************************* * BlinkAlive() * * This subroutine briefly blinks the LED next to the PIC every two-and-a-half * seconds. ******************************* */ void BlinkAlive() { //RA3 = 1; RA4 = 1; // Turn off LED if (!(--ALIVECNT)) // Decrement loop counter, return if not 0 { ALIVECNT = 250; // Reinitialize ALIVECNT RA4 = 0; // Turn on LED for ten ms every 2.5 sec } } /******************************** * send_command() * * This subroutine is mainly used by AmigoInit to send * the initialization packets to the Amigobot, the loop is similar * to the nios board's loop, but the difference is in how * the pic sends on it's Tx buffer ******************************** */ void send_command(unsigned char * command_data) { long loop = (command_data[2]) + 3; //loopsize is byte count +3 long k = 0; for(k = 0; k < loop;k++) //send the bytes to the bot { //start for loop TXREG = command_data[k]; //actually sending the individual bytes while (!TXIF){} }//end for loop }//end send_command() /******************************** * AmigoInit() * * This subroutine actually makes sure that the initialization * packets are sent in the correct order. ******************************** */ void AmigoInit() { if(!(--TXCOUNT)) { TXCOUNT= 10; //reinitialize TXCount for .1 sec if (STARTCNT == 0) { send_command(sync0); } else if (STARTCNT == 1) { send_command(sync1); } else if (STARTCNT == 2) { send_command(sync2); } else if (STARTCNT == 3) { send_command(open); } else if (STARTCNT == 4) { send_command(amigosonar); } else if (STARTCNT == 5) { send_command(pulse); GIEL = 1; // Enable low-priority interrupts GIEH = 1; // Enable all interrupts STARTED = 1; //amigobot has been initialzed //RCIE = 1; //enable receive interrupts } STARTCNT = STARTCNT+1; }//endif TXCOUNT }//end AmigoInit(); /******************************** * PULSE() * * This subroutine actually uses sendcommand() to * send a pulse command to amigobot about every second ******************************** */ void PULSE() { if(!(--TXCOUNT)) { TXCOUNT = 105;//reinitialize TXCOUNT for 1.05 sec send_command(pulse); TXQUEUE = 1; // delete this line later on }//end if TXCOUNT }//end pulse void TXtest() { if(TXQUEUE) //is there something in the queue to send. { send_command(enable); //do the comparing TXQUEUE = 0; TESTENABLE = 1; } }//end TXTEST /******************************** * convertsonardata() * * This subroutine is run in mainline and is initiated * to run each time the Amigobot finishes sending * a Server Information Packet and two Amigobot Sonar * data has been received ******************************** */ void convertsonardata(){ char finalbits; //float itotal = 0; //float itotal2 = 0; int k = 0; //char condeci; float fSonardata; //WORKING WITH THE FIRST SONAR READING @ sonardata[0] fSonardata = (float)((SONARDATA[2]<<8)+SONARDATA[1]); finalbits = sonardatahelper(fSonardata); sspbyteassem(SONARDATA[0],finalbits); while(!RA5){}//wait till RA5 is HIGH SSPBUF = SSPUPDATE;//before updating SSPBUF k = 0; fSonardata = (float)((SONARDATA[5]<<8)+SONARDATA[4]); finalbits = sonardatahelper(fSonardata); sspbyteassem(SONARDATA[3],finalbits); while(!RA5){}//wait till RA5 is HIGH SSPBUF = SSPUPDATE;//before updating SSPBUF old line UPDATESONAR = 0; }//end convertsonardata() /******************************** * sonardatahelper() * * This subroutine just takes the actual values of sonar * converts it to millimeters, and then returns a 2 bit * representation of the distance ******************************** */ char sonardatahelper(float ftotal){ char charbits; ftotal = ftotal * .555;//convert to millimeters if(ftotal >= 1500){ charbits = 0x00; } else if((ftotal < 1500) && (ftotal >= 900)){ charbits = 0x01; } else if((ftotal <900) && (ftotal >= 300)){ charbits = 0x02; } else if(ftotal < 300){ charbits = 0x03; }//endelse return charbits; }//end sonardatahelper /******************************** * sspbyteassem() * * This subroutine is called from convertsonardata() and it * takes in the sonar number and the two bit value that it * corresponds to (the 2-bit resolution of distance * we are displaying on the ipaq) It updates the SSPUPDATE * byte that will later be copied to SSPBUF to be sent to other pic ******************************** */ void sspbyteassem(char whichsonar,char twobit){ switch(whichsonar){ case 0: SSPUPDATE = (SSPUPDATE & 0b11110011)+ ((twobit<<2)& 0b00001100);//bit3,2 break; case 2: SSPUPDATE = (SSPUPDATE & 0b00111111)+ ((twobit<<6)& 0b11000000);//bit7,6 break; case 5: SSPUPDATE = (SSPUPDATE & 0b11001111)+ ((twobit<<4)& 0b00110000);//bit5,4 break; default: break; }//endswitch }//end sspbyteassem /******************************** * exponent() * * This exponent takes in int value of base and an int value of power * and computers base^power and returns the result. * * we figured out a faster way to implementing this so this function * is not used but is here for future reference. ******************************** */ int exponent(int base, int power){ long result=1; if (power==0){ return base; } while(power > 0){ result=result*2; power--; }//end while return base*result; }//end exponent /******************************** * getAmigoData() * * This function actually enables the interrupts and waits 150secs * while the amigobot is sending packets and then disables the interrupt * and resets the 1 second counter before it is initialized * again. ******************************** */ void getAmigoData(){ int k = 0; //RA1LED=20; if(OERR){ OERR=0; RA1=1; CREN=0; CREN=1; } if(FERR){ RXDATA = RCREG; } RCIF = 0; RCREG = 0;//make sure theree's nothing in RCREG RCREG = 0;//make sure there's nothing in RCREG RCIE = 1; while (k<=15){ while (!TMR2IF) { } // Wait until TMR2IF flag is set TMR2IF = 0; // Clear flag k++; } RCIE = 0; RXMainLineCNT = 100; iAmigoSIP = 0; RA1 = 0; RA3 = 0; traverseSIP(); }//end getAmigoData() /******************************** * traverseSIP() * * This function reads through the 50byte array and * sucks out the data needed and puts it into SONARDATA[] ******************************** * void traverseSIP(){ int k = 0; int j = 23; int l = 0; while (k<=50){ if (AmigoSIP[k]==0xFA){ RE2 = 1; RA1LED = 10; if (AmigoSIP[k+1] == 0xFB){ if(AmigoSIP[k+22]== 0x02){ RB3 = 1; RB3LED = 10; }//end if checking for 22nd bit RB5 = 1; RB5LED = 10; if ((k + 28)<= 50){ if(AmigoSIP[k+22]== 0x02){ RB3 = 1; RB3LED = 10; } RB4 = 1; RB4LED = 10; while(j<=28){ SONARDATA[l] = AmigoSIP[k+j]; //AmigoSIP[k+j]=SONARDATA[l]; j++; l++; }//end while UPDATESONAR = 1; }//endif k+28 }//end if k+1 ==0XFB }//end if 0xFA k++; }//end while k<=50 }//end traverseSIP() /******************************** * LoopTime() * * This subroutine waits for the TMR2IF flag to indicate that 10ms have passed * since the last time this occurred. ******************************** */ void LoopTime() { while (!TMR2IF) { } // Wait until TMR2IF flag is set TMR2IF = 0; // Clear flag }//end LoopTime() /****** HiPriISR interrupt service routine * * Basically this subroutine is set up to handle * a RECIEVE interrupt. **************************************** */ void interrupt HiPriISR() { while(RCIF){ RA3 = 1; AmigoSIP[iAmigoSIP]=RCREG; //RA3=1; iAmigoSIP++; if(iAmigoSIP > 50){ RCIE = 0; } } }//end HiPriISR() /****** LoPriISR interrupt service routine *********************/ void interrupt low_priority LoPriISR() // Low-priority ISR { RA2 = 1;//setting LED to light up showing transmitting if(SSPIF) { COMMAND = SSPBUF; if(COMMAND != 0b00000011){ CommandHandler(); } else{ SSPBUF = 0x03; }//end else } SSPIF = 0; SPICNT = 10;//for LED } /******************************************* * CommandHandler() * * Called from the low priority interrupt routine * whenever there is a new command from the pda. * ************************************** */ void CommandHandler() { char LNIBBLE; char HNIBBLE; char CSUMH; char CSUML; HNIBBLE = (COMMAND >> 4) & 0x0f; //shifted to right 4 bits then mask LNIBBLE = (COMMAND) & 0x0f; //mask off top 4 bits switch (HNIBBLE){ case 1://go command OLDSPEED = vel2[5]; TXcommand[3] = 0x04; TXcommand[4] = 0x3b; TXcommand[5] = 0x01; TXcommand[6] = 0x00; TESTENABLE = 1; break; case 2://stop command iSTOP = 1; loopsize = (stop[2]) + 3; //loopsize is equal to the byte count +3 i = 0; //integer for "for" loop for(i = 0; i < loopsize;i++) //send the bytes of data from command_data to the bot { //start for loop TXREG = stop[i]; //actually sending the individual bytes while (!TXIF){} } break; //end case2 case 3://right 30 degrees TXcommand[3] = 0x0D; TXcommand[4] = 0x1b; TXcommand[5] = 0x1e; TXcommand[6] = 0x00; break; case 4://left 30degress TXcommand[3] = 0x0D; TXcommand[4] = 0x3b; TXcommand[5] = 0x1e; TXcommand[6] = 0x00; break; case 5://case of velocity change TXcommand[3] = 0x20; TXcommand[4] = 0x3b; if (LNIBBLE==1) { TXcommand[5] = 0x05; TXcommand[6] = 0x05; } else if(LNIBBLE ==2) { TXcommand[5] = 0x08; TXcommand[6] = 0x08; } else if(LNIBBLE ==3) { TXcommand[5] = 0x12; TXcommand[6] = 0x12; } OLDSPEED = TXcommand[5];//keep track of previous velocity break; case 6://turn while still moving right TXcommand[3] = 0x20; TXcommand[4] = 0x3b; TXcommand[6] = OLDSPEED; TXcommand[5] = OLDSPEED - 3; break; //end case 6 case 7://turn while still moving left TXcommand[3] = 0x20; TXcommand[4] = 0x3b; TXcommand[5] = OLDSPEED; TXcommand[6] = OLDSPEED - 3; break; //end case 7 case 8://stop diff turning and go straight TXcommand[3] = 0x20; TXcommand[4] = 0x3b; TXcommand[5] = OLDSPEED; TXcommand[6] = OLDSPEED; break; //end of case 8 default: commandOK = 0; //do something break; }//end switch /* Now that the TXcommand data is formed, we now create checksum and send */ if (commandOK){ if (iSTOP==0) { CSUML = TXcommand[4] + TXcommand[6]; if(TXcommand[4] > CSUML)//check if carry bit { CSUMH = 1 + TXcommand[3] + TXcommand[5]; }//endif else { CSUMH = TXcommand[3] + TXcommand[5]; } TXcommand[7] = CSUMH; TXcommand[8] = CSUML; //sending data to amigobot loopsize = (TXcommand[2]) + 3; //loopsize is byte count +3 i = 0; for(i = 0; i < loopsize;i++) //sending the bytes of data to the bot { TXREG = TXcommand[i]; //actually sending the individual bytes while (!TXIF){} } }//end if not stop }//end if commandOK iSTOP = 0;//re-zero stop commandOK = 1;//re-OK commandok }//end command handler