/* FLASHOne.c Demonstration of FLASH C coding in ICC12 for trilat MiniDragon. (c) 2009 Paul Maxim, William Spears, Jerry Hamann */ #include "putchar_dp256.c" // Also includes stdio.h and hcs12dp256.h //****************************************************************************** // Prototype declarations for functions within this source file //****************************************************************************** void Cycle_7Seg(void); unsigned char Read_robotID(void); void Toggle_PORTAb6(void); void Wait(unsigned int); void PLL_init(void); void SCI_init(void); void TMR_init(void); void TOF_isr(void); #define SYNC_BYTE 0x33 #define FILL_BYTE 0x01 unsigned char Man_decode(unsigned short int); unsigned short int Man_encode(unsigned char); unsigned char SCI1_receive(void); unsigned char SCI1_receive_byte(void); void SCI1_transmit(unsigned char); void SCI1_transmit_byte(unsigned char); void Display_7SegBearing(int, int); void Sonar_activate(void); void Sonar_trilaterate(float* , float*); enum { SENSOR_MODE_RX, SENSOR_MODE_TX }; void Sonar_change_mode(unsigned char ); void Sonar_change_mode_wait(unsigned int ); //****************************************************************************** unsigned int rollCount; // Global variable for timer overflow accounting. // #pragma interrupt_handler TOF_isr /* For FLASH, need to directly imbed the ISR vector at top of memory */ /* The value of 0xEFDE is for Boot Loader secondary vector address, use 0xFFDE for an absolute load */ #pragma abs_address: 0xEFDE void(*TOI_interrupt_vector[])(void)={TOF_isr}; #pragma end_abs_address /* For MiniDragon FLASH install, retaining Boot Loader, here are the vectors for the essential system interrupts. If you opt for removing the Boot Loader as well, change the following .org to 0xFFF8 .*/ asm( ".area vector(abs)\n" ".org 0xEFF8\n" // Reset, Clock monitor fail reset, COP // failure reset, and unimplemented // instruction. ".word 0xC000, 0xC000, 0xC000, 0xC000\n" ".text" ); // start at beginning of code whenever any // of the above exceptions occur //****************************************************************************** //****************************************************************************** void main() { //****************************************************************************** unsigned char own_ID; unsigned char tmp_ID; unsigned char sender_ID; float x, y; unsigned char ucharRead; int i; INTR_OFF(); /* For RAM and EEPROM, must embed ISR vector in RAM vector table */ //_LP(0x3E5E) = (unsigned short)TOF_isr; PLL_init(); SCI_init(); TMR_init(); INTR_ON(); printf("FLASHOne Version 20090205\n\r"); own_ID = Read_robotID(); printf("Robot: %d\n\r", own_ID); for(;;) { tmp_ID = Read_robotID(); if( tmp_ID != own_ID ) { own_ID = tmp_ID; printf("*Robot: %d\n\r", own_ID); } if( own_ID == 0 ) { // We'll have Robot 0 always transmit Sonar_change_mode(SENSOR_MODE_TX); for(i=0;i<327;i++) { SCI1_transmit_byte(FILL_BYTE); } /* BEWARE: The time interval between sending SYNC_BYTE and activating sonar is very critical to system timing calibration. For the data sequence shown here... SYNC_BYTE(byte), ID(word), FILL(word), FILL(word) the acoustic pulse is triggered 1.74 ms AFTER Trigger occurs on the received end (YES, ON THE RECEIVED END). */ SCI1_transmit_byte(SYNC_BYTE); SCI1_transmit(own_ID); SCI1_transmit(FILL_BYTE); SCI1_transmit(FILL_BYTE); Sonar_activate(); Wait(10); Sonar_change_mode(SENSOR_MODE_RX); Cycle_7Seg(); // Visual progress meter } else { Sonar_change_mode(SENSOR_MODE_RX); do { ucharRead=SCI1_receive_byte(); Toggle_PORTAb6(); // Subtle progress meter } while(ucharRead != SYNC_BYTE); sender_ID=SCI1_receive(); Sonar_activate(); Sonar_trilaterate(&x, &y); // Display in tenths of inches. printf("%d, %d\n\r", (int)(10*x),(int)(10*y)); Display_7SegBearing((int)x,(int)y); // Show bearing to source. } } } //****************************************************************************** // Rotate a single segment of the MiniDragon 7 segment display on each call //****************************************************************************** void Cycle_7Seg(void) { static unsigned char LED=0x01; DDRH = 0xff; PTH = LED = (LED > 0x10 ? 0x01 : LED*2); } //****************************************************************************** // Provide a rough "Bearing" to received Trilat sonar on the // MiniDragon 7 segment display (assumes axis of the display is on the 45deg // ray splitting quadrant I of the sensor array, with the "top" of the display // segments located closest to the origin of the sensors. //****************************************************************************** void Display_7SegBearing(int x, int y) { unsigned char LED=0; float y2x = 1.0*y/x; // Assign each segment a 60 degree swath with +/-15 degree overlap LED |= ((x<=0) && (y<=0)) ? 0x01 : 0x00; // segA LED |= ((x<=0) && ( (y2x>=-1.73) && (y2x<= 0.58) )) ? 0x02 : 0x00; // segB LED |= ((y>=0) && ( (y2x<=-0.58) || (y2x>= 1.73) )) ? 0x04 : 0x00; // segC LED |= ((x>=0) && (y>=0)) ? 0x08 : 0x00; // segD LED |= ((x>=0) && ( (y2x>=-1.73) && (y2x<= 0.58) )) ? 0x10 : 0x00; // segE LED |= ((y<=0) && ( (y2x<=-0.58) || (y2x>= 1.73) )) ? 0x20 : 0x00; // segF DDRH = 0xff; PTH = LED; } //****************************************************************************** // This function decodes a 16 bit Manchester encoded variable into the actual // 8 bit decoded variable and it was originally written by Aaron Ramsey. // This function also implements a simple method of error detection // based on the balanced nature of the Manchester encoding algorithm. //****************************************************************************** unsigned char Man_decode(unsigned short int to_decode){ //****************************************************************************** unsigned char odd_byte, even_byte; odd_byte = (unsigned char)(to_decode >> 8); if((odd_byte & 0xAA) ^ (((~odd_byte) & 0x55) << 1)){ PTH = 0xF9; // display E - error on 7 segment display return 0; } odd_byte &= 0xAA; even_byte = (unsigned char)to_decode; if((even_byte & 0x55) ^ (((~even_byte) & 0xAA) >> 1)){ PTH = 0xF9; // display E - error on 7 segment display return 0; } even_byte &= 0x55; return (odd_byte | even_byte); } //****************************************************************************** // This function encodes a 8 bit variable into a 16 bit Manchester // encoded variable and it was originally written by Aaron Ramsey. // The odd bits [7,5,3,1] are encoded into the MSB 8 bits of data to // return and the even bits [6,4,2,0] are encoded into the // LSB 8 bits of data to return. Encoding this way saves a lot // of bit testing and flipping. // For example, 0x0E (0b0000_1110) encodes into 0x5AA6 (0b0101_1010_1010_0110) // instead of the more natural 0x55A9 (0b0101_0101_1010_1001). //****************************************************************************** unsigned short int Man_encode(unsigned char to_encode) { //****************************************************************************** unsigned char odd_byte, even_byte; odd_byte = (to_encode & 0xAA) | (((~to_encode) & 0xAA) >> 1); even_byte = (to_encode & 0x55) | (((~to_encode) & 0x55) << 1); return ((unsigned short int)odd_byte << 8) | even_byte; } //****************************************************************************** // The MiniDragon development board has a 16 MHz crystal, however with the // use of the on-chip Phase-Lock-Loop of the HCS12, a bus speed of 24 MHz // can be achieved. For RAM development under D-Bug12, the PLL_init is // completed by D-Bug12 itself. For EEPROM or FLASH installation, we must // complete PLL_init() ourselves. All future timing (serial I/O, timebase for // counter/timer, etc.) is based upon this. #define PLLSEL 0b10000000 #define PLLON 0b01000000 #define PLL_LOCK 0b00001000 //****************************************************************************** void PLL_init(void) { //****************************************************************************** // PLL code for 24MHz bus speed from a 16MHz crystal CLKSEL &= ~PLLSEL; // clear bit 7, clock derived from OSCCLK PLLCTL |= PLLON; // turn PLL on, bit 6 = 1 PLL on SYNR = 0x05; // ECLK = 24MHz * (SYNR+1)/(REFDV+1) REFDV = 0x03; while((CRGFLG & PLL_LOCK) == 0);// wait until PLL is locked CLKSEL |= PLLSEL; // switch over to PLL clock // switch over to PLL clock } //****************************************************************************** // Read the jumpers set on PORTB bits 3 thru 0 to obtain robot ID. // These four jumpers are provided on the UW DRL DOTR (Daughter) Board for // the MiniDragon controller in trilateration module. //****************************************************************************** unsigned char Read_robotID(void){ //****************************************************************************** // make PORTB[3:0] input pins DDRB = DDRB & 0b11110000; // get the robot ID from PORTB[3:0] return (PORTB & 0b00001111); } //****************************************************************************** // Initialize the serial interface //****************************************************************************** void SCI_init(void){ //****************************************************************************** unsigned char temp = 0; SCI0BDL = SCI1BDL = 0x9C; // set BAUD rate to 9600 = (24M)/(16*156.25) SCI0BDH = SCI1BDH = 0x00; // and 156 == 0x9C SCI0CR1 = SCI1CR1 = 0x00; // 8 bit data, no parity SCI0CR2 = SCI1CR2 = 0x0C; // no interrupts, enable receiver and transmitter temp = SCI1SR1; // two stage RDRF flag clearing: temp = SCI1DRL; // first read the SCI Status Register 1 // second read the SCI Data Register Low temp = SCI0SR1; temp = SCI0DRL; } //****************************************************************************** // Read a 16-bit encoded value from SCI1 and returns the "hopefully" correctly // decoded 8-bit value that was originally submitted for transmission. //****************************************************************************** unsigned char SCI1_receive(void){ //****************************************************************************** unsigned short int temp1, temp2; while(!(0x20 & SCI1SR1)); // wait for the RDRF flag temp1 = SCI1DRL; while(!(0x20 & SCI1SR1)); // wait for the RDRF flag temp2 = SCI1DRL; return Man_decode((temp1 << 8) | temp2); } //****************************************************************************** // Read an 8-bit value, from SCI1, that is not encoded. // This is often used to synchronize the receiver with the transmitter. //****************************************************************************** unsigned char SCI1_receive_byte(void){ //****************************************************************************** while(!(0x20 & SCI1SR1)); // wait for the RDRF flag return SCI1DRL; } //****************************************************************************** // Transmit a single byte on SCI1 with Manchester encoding. //****************************************************************************** void SCI1_transmit(unsigned char to_send){ //****************************************************************************** // encode the variable unsigned short int send = Man_encode(to_send); // store the encoding of bits [7, 5, 3, 1] unsigned char send1 = (unsigned char)(send >> 8); // store the encoding of bits [6, 4, 2, 0] unsigned char send2 = (unsigned char)send; // send the first 8 bits while(!(0x80 & SCI1SR1)); // wait for the TDRE flag SCI1DRL = send1; // transmit the character // send the last 8 bits while(!(0x80 & SCI1SR1)); SCI1DRL = send2; } //****************************************************************************** // Send an 8-bit value, on SCI1, that is not encoded. // This is often used to syncronize the receiver with the transmitter. //****************************************************************************** void SCI1_transmit_byte(unsigned char to_sync){ //****************************************************************************** while(!(0x80 & SCI1SR1)); // wait for the TDRE flag SCI1DRL = to_sync; // transmit the character } //****************************************************************************** void Sonar_activate(void){ //****************************************************************************** unsigned char i = 0; DDRA = 0xff; PORTA = PORTA | 0x80; // PORTA bit 7 is the Trigger line to the XSRFv2.0 for (i=0; i<15; i++) { // A very quick loop, 0.2 msec total runtime asm("NOP"); // to produce a Trigger pulse time. asm("NOP"); } PORTA = PORTA & 0x7F; // PIC on XSRF initiates Sonar on falling edge. } //****************************************************************************** // PORTA[3,2] (pins 59 and 60 on MiniDragon) are used to control // the send/receive switch of the XSRFv2.0 board via lines CTL2 and CTL1 . // The "mode" input to this function has the following connotation: // 0 - Rx mode // 1 - Tx mode //****************************************************************************** void Sonar_change_mode(unsigned char mode){ //****************************************************************************** DDRA = 0xff; if (mode == 0){ // Rx mode // CTL1 (PORTA[2]) = 0 V, CTL2 (PORTA[3]) = 5 V PORTA = PORTA & 0b11111011; PORTA = PORTA | 0b00001000; } else if (mode == 1){ // Tx mode // CTL1 (PORTA[2]) = 5 V, CTL2 (PORTA[3]) = 0 V PORTA = PORTA | 0b00000100; PORTA = PORTA & 0b11110111; Sonar_change_mode_wait(30); } } //****************************************************************************** // at 9600BAUD each byte takes aprox. 1ms //****************************************************************************** void Sonar_change_mode_wait(unsigned int i){ //****************************************************************************** unsigned int j; for(j = 0; j < i; j++){ SCI1_transmit(FILL_BYTE); } } //****************************************************************************** void Sonar_trilaterate(float *rx, float *ry){ //****************************************************************************** unsigned int tcA, tcB, tcC; long result; float x , y , a , b, c, ticks; unsigned int start; unsigned int roll1, roll2, roll3; TCTL3 = 0x51; // input capture (on rising edge to capture TCNT at start) TFLG1 = TFLG1; // a smart way to clear the flag associated with IC // wait for ECHOPL to go high (after ~230us) // we only need to monitor TFLG1[4, 6, 7] so we need to clear the other bits // that may interfere, e.g. pulse accumulators 0 through 3 while (!(TFLG1 & 0xD0)); // obtain the correct time when ECHOPL goes high start = TCNT; rollCount = 0; // need to detect free running counter rollovers from here TCTL3 = 0xA2; // now, adjust to input capture on falling edge TFLG1 = TFLG1; // next we'll poll three times, but before trying that, make sure that // we don't hang here with no response at all (no sensors triggered). while (!(TFLG1 & 0xD0)){ if (rollCount > 1){ // the free running counter (TCNT) will roll over every 65,535 * 1.33 us // PMM Aug 12, 2006 - 0.133us // which means we should never have more than one rollover during normal // operation; as soon as we detect more than one rollover, // we break out of the loop break; } } // find which sensor triggered and accompany this with the correct number // of rollovers (0 or 1 expected). // remember the number of overflows that occured // since the Pulse Accumulators may raise a flag in TFLG1, filter the bits switch (TFLG1 & 0xD0){ case 0x80: roll1 = rollCount; break; // sensor A triggered case 0x40: roll2 = rollCount; break; // sensor B triggered case 0x10: roll3 = rollCount; // sensor C triggered } TFLG1 = TFLG1; while (!(TFLG1 & 0xD0)){ if (rollCount > 1){ break; } } switch (TFLG1 & 0xD0){ case 0x80: roll1 = rollCount; break; // sensor A triggered case 0x40: roll2 = rollCount; break; // sensor B triggered case 0x10: roll3 = rollCount; // sensor C triggered } TFLG1 = TFLG1; while (!(TFLG1 & 0xD0)){ if (rollCount > 1){ break; } } switch (TFLG1 & 0xD0){ case 0x80: roll1 = rollCount; break; // sensor A triggered case 0x40: roll2 = rollCount; break; // sensor B triggered case 0x10: roll3 = rollCount; // sensor C triggered } TFLG1 = TFLG1; /* Obtain the time difference when ECHOPL goes high. The speed of sound is approximately 343.6 meters/second or 34.36 centimeters/millisec at 68F (20C) (atmospheric pressure dependent). We utilize this characteristic to compute an estimated between center of transmitting ultrsonic transducer to the center of the receiving transducer. distance = (counts * time/count + system delay) * speed of sound - offset We subtract 2.54 cm (1 inch) to account for the distance between the tip of the cone and transducer, two of these distances per sensor path. The clock we use for the timer is 24 MHz / 32 = 750,000 Hz so the counter will increment 750,000 times per second, and since we use milliseconds in our calculations, we only need to divide the number of count ticks by 750 OR multiply by 0.00133 The delay between the Transmitter triggering and the Receiver triggering is a negative 1.74 ms for SYNC(b), ID(w), FILL(w), FILL(w) . This fact can be confirmed with an oscilloscope and accounts for the time of transmission of the serial bit stream. */ /* "Y-axis" sensor */ tcA = TC4; result = (long)tcA - (long)start; ticks = roll1 * 65536 + result; a = (((ticks / 750.0) - 1.74) * 34.36 - 5.08); // cm === (------- ms ---------) * cm/ms - cm // "Origin" sensor tcB = TC6; result = (long)tcB - (long)start; ticks = roll2 * 65536 + result; b = (((ticks / 750.0) - 1.74) * 34.36 - 5.08); // "X-axis" sensor tcC = TC7; result = (long)tcC - (long)start; ticks = roll3 * 65536 + result; c = (((ticks / 750.0) - 1.74) * 34.36 - 5.08); //compute x, y // x = (b^2-c^2+d^2)/(2d) where d = 6 in = 15.24 cm // y = (b^2-a^2+d^2)/(2d) where d = 6 in = 15.24 cm x = ((((b + c) * (b - c)) / 15.24 + 15.24) / 2.0)/2.54; y = ((((b + a) * (b - a)) / 15.24 + 15.24) / 2.0)/2.54; // Example of various diagnostic level displays // printf("[%u,%u,%u]+", tcA, tcB, tcC); // Raw counts per sensor channel // printf("(%d,%d,%d) : ", (int)roll1, (int)roll2, (int)roll3); // Rolls per // printf("(%d,%d,%d) = ", (int)a, (int)b, (int)c); // Net cm per sensor // printf("(%d,%d)\r\n", (int)x, (int)y); *rx = x; *ry = y; } //****************************************************************************** void TMR_init(void){ //****************************************************************************** // Configure all channels for input capture. TIOS = 0x00; TCTL2 = 0x00; PACTL = 0x00; PBCTL = 0x00; ICPAR = 0x00; // all four 8-bit Pulse Accumulators disabled TCTL3 = 0x00; TCTL4 = 0x00; // capture disabled on first four IC channels TSCR1 = 0x80; // set TEN - Timer Enable TIE = 0x00; // no interrupts for IC or OC TSCR2 = 0x85; // set TOI (Timer Overflow Int.), div clock by 32 // with preset of PR[2:0]=b101 TFLG1 = 0xD0; // clear the TFLG1 register channels 7, 6 and 4 } //****************************************************************************** // Keep track of the number of rollovers of the free running counter. // Note, must complete the following additional tasks to enable this // interrupt service routine. // (1) Describe TOF_isr as an interrupt handler to ICC12 (ensures appropriate // handling of call and return). // #pragma interrupt_handler TOF_isr // (2) Register vector to TOF_isr. // (a) For D-Bug12 RAM and EEPROM implementation, write a RAM vector via... // _LP(0x3E5E) = (unsigned short)TOF_isr; // which should be completed very early in initialization. // (b) For FLASH implementation, must assemble to the target // location in the binary memory image (and, there are two possible // configurations for this). // (i) On-chip FLASH with Boot Loader still in place: requires use // of the secondary vector table in FLASH 0xEF80 thru 0xEFFF. // (ii) On-chip FLASH with Boot Loader removed: you must define the // primary vector table in FLASH 0xFF80 thru 0xFFFF. // Example for option (i) above... // #pragma abs_address: 0xEFDE // void(*TOI_interrupt_vector[])(void)={TOF_isr}; // #pragma end_abs_address // (3) Configure the timer for correct mode and interrupt (TOI, see TMR_init() // in this source code). // (4) Enable interrupts. //****************************************************************************** void TOF_isr(void){ //****************************************************************************** TFLG2 = 0x80; // reset the flag that caused the interrupt rollCount++; // using a global variable } //****************************************************************************** // For diagnostic purposes, this function toggles the digital output at big 6 // on Port A. Developers can use this to aid in measuring timings of routines // as well as provide a subtle "progress of algorithm" indicator. You can find // bit 6 of Port A at pin 63 on the MiniDragon processor header (right side). //****************************************************************************** void Toggle_PORTAb6(void){ //****************************************************************************** static unsigned char theBit=1; unsigned char portVal; DDRA |= 0x40; // Make bit 6 an output portVal = PORTA; if (theBit == 1) { PORTA = portVal | 0x40; theBit = 0; } else { PORTA = portVal & 0xbf; theBit = 1; } } //****************************************************************************** // Basic delay function; a value of 32 will yield approximately 100 ms // Measurements: i = 1 ==> 3.14 ms // i = 2 ==> 6.24 ms // i = 10 ==> 31.2 ms // i = 100 ==> 312 ms // Based on a 24 MHz MiniDragon bus speed //****************************************************************************** void Wait(unsigned int i) { //****************************************************************************** unsigned int j; while(i){ for(j = 0; j < 5000; j++); i--; } }