/////////////////////////////////////////////////////////////////////////// // // SRF07 Sonar, CMPS01 Compass & MD03 Motor Drive Test Routines // // // Commercial use of this software is prohibited // Private,Hobby and Educational use ony is permitted // // (C) Copyright 2001, Devantech Ltd. // Written June/July 2001 by Gerald Coe, using HITECH PICC compiler // // ////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////// // // This software is used on a test jig that we built to test that the // SRF07 Sonar, CMPS01 Compass and MD03 H-Bridge Motor Driver, all work // together on the I2C Bus. // // The Jig is very simple, being a PIC16F877 running with a 4MHz resonator // An LCD display (Powertip 2004 - 20 character, 4 lines) is connected to // PortB (DB0 to RB0 thro' DB7 to RB7) and the RS, RW and E strobes to // PortD bits 2,3 and 4. // // PortC bits 3,4 are the I2C bus pins with 1k pullup resistors and // connect to the modules. // // The normal I2C clock rate for the modules is 100khz. However with a // small precaution this can be taken up to 1Mhz. This test software uses // a 1Mhz clock. The 100uS delays at strategic points in the read/write // routines are all that is neccesary to do this. // // I2C Bus Addresses // // Serial EEPROMs -> A0-AE max. 8 eproms // MD03 -> B0-BE max. 8 motor boards // CMPS01 -> C0 max. 1 compass // SRF07 -> E0-FE max. 16 sonar's // // This software basically just; // 1. Starts the SRF07 ranging // 2. Reads and displays the compass reading // 3. Reads and displays motor data, changing direction if neccesary // 4. waits for the sonar to complete and reads and displays range info. // 5. loop around forever // ////////////////////////////////////////////////////////////////////////// #include "pic.h" #include "stdio.h" // prototypes void setup(void); void print(char *s); void line1(void); void line2(void); void line3(void); void line4(void); void delay(void); void read_compass(unsigned char *buffer); void start_sonar_ping(unsigned char addr, unsigned char cmd); void read_sonar(unsigned char addr, unsigned char *buffer); void read_motor(unsigned char addr, unsigned char *buffer); void write_md03(unsigned char addr, unsigned char reg, unsigned char data); // LCD strobe signals static bit E @ (unsigned)&PORTD*8+4; static bit RW @ (unsigned)&PORTD*8+3; static bit RS @ (unsigned)&PORTD*8+2; bit direction; bank1 char s[20]; void main(void) { unsigned char i, x[36], ver; unsigned int a,r; setup(); for(i=0; i<20; i++) s[i]=0; for(;;) { start_sonar_ping(0xea, 0x54); // i2c address, ANN cm ping cmd read_compass(x); // read compass while sonar is ranging a = ((x[2]<<8) + x[3])/10; r = ((x[2]<<8) + x[3])%10; ver = x[0]; sprintf(s,"Compass %02d %3d.%1d ", ver,a,r); line1(); print(s); read_motor(0xb0, x); // process motor while sonar ranging sprintf(s,"MD03 %02d %3d %3d ", x[7],x[4],x[5]); line4(); print(s); // print motor version, temp & current s[3] = 0; if(x[1]&0x01) s[0]='A'; else s[0]=' '; // indicate Accel flag if(x[1]&0x02) s[1]='T'; else s[1]=' '; // indicate Temperature flag if(x[1]&0x04) s[2]='I'; else s[2]=' '; // indicate Current limit flag print(s); if(!(x[1]&0x01)) { // if done accelerating if(direction) { write_md03(0xb0, 2, 245); // set speed to fast write_md03(0xb0, 3, 255); // set acceleration (gentle) write_md03(0xb0, 0, 1); // move forwards direction = 0; } else { write_md03(0xb0, 2, 25); // set speed to slow write_md03(0xb0, 3, 255); // set acceleration (gentle) write_md03(0xb0, 0, 2); // move backwards direction = 1; } } do read_sonar(0xea, x); while(x[0]==0xff); // wait for sonar to respond a = ((x[2]<<8) + x[3])/100; // display result r = ((x[2]<<8) + x[3])%100; // in metres ver = x[0]; sprintf(s,"Sonar %2X %2X %3d.%2d", ver,x[1],a,r); line2(); print(s); for(i=4; i<24; i++) { if(x[i]) s[i-4]='X'; else s[i-4]='-'; } s[i-4]=0; line3(); print(s); } } // wait for busy flag void wait4BF(void) { TRISB = 0xff; // PORTB to input mode do { RW = 1; RS = 0; E = 0; E = 1; }while(PORTB&0x80); // wait for bit 7 to go low E = 0; TRISB = 0x00; // PORTB back to output mode } // send command to LCD void lcd_cmd(unsigned char cmd) { wait4BF(); PORTB = cmd; RW = 0; E = 0; RS = 0; E = 1; E = 0; } // send data to LCD void lcd_data(unsigned char data) { wait4BF(); PORTB = data; RW = 0; E = 0; RS = 1; E = 1; E = 0; } // set cursor to start of LCD line void line1(void) // cursor to start of line 1 { lcd_cmd(0x80); } void line2(void) // cursor to start of line 2 { lcd_cmd(0xc0); } void line3(void) // cursor to start of line 3 { lcd_cmd(0x94); } void line4(void) // cursor to start of line 4 { lcd_cmd(0xd4); } // only used to init LCD void delay(void) // about 30mS ish { int i; for(i=0; i<2000; i++); } // used in I2C read/write routines to allow 1Mhz clock speeds void delay100(void) // about 100uS ish { // with a 4MHz Xtal int i; for(i=0; i<6; i++); } // print buffer to LCD void print(char *s) { while(*s) lcd_data(*s++); } // read 8 bytes from compass void read_compass(unsigned char *buffer) { char x; SEN = 1; // send start bit while(SEN); // and wait for it to clear ACKDT = 0; // acknowledge bit SSPIF = 0; SSPBUF = 0xC0; // 11000000 - write command while(!SSPIF); // wait for interrupt SSPIF = 0; // then clear it. delay100(); // 100uS ish SSPBUF = 0; // read from address 0 while(!SSPIF); // SSPIF = 0; // delay100(); // 100uS ish RSEN = 1; // send repeated start bit while(RSEN); // and wait for it to clear SSPIF = 0; SSPBUF = 0xC1; // 11000001 - read command while(!SSPIF); // wait for interrupt SSPIF = 0; // then clear it. for(x=0; x<7; x++) { RCEN = 1; // start receiving while(!STAT_BF); // wait for data buffer[x] = SSPBUF; // and get it ACKEN = 1; // start acknowledge sequence while(ACKEN); // wait for ack. sequence to end } RCEN = 1; // start receiving while(!STAT_BF); // wait for data buffer[7] = SSPBUF; // and get it ACKDT = 1; // not acknowledge for last byte ACKEN = 1; // start acknowledge sequence while(ACKEN); // wait for ack. sequence to end PEN = 1; // send stop bit while(PEN); // } // read 36 bytes from sonar void read_sonar(unsigned char addr, unsigned char *buffer) { char x; SEN = 1; // send start bit while(SEN); // and wait for it to clear ACKDT = 0; // acknowledge bit SSPIF = 0; SSPBUF = addr; // 0xe0-0xfe for sonar while(!SSPIF); // wait for interrupt SSPIF = 0; // then clear it. delay100(); // 100uS ish SSPBUF = 0; // read from address 0 while(!SSPIF); // SSPIF = 0; // delay100(); // 100uS ish RSEN = 1; // send repeated start bit while(RSEN); // and wait for it to clear SSPIF = 0; SSPBUF = addr+1; // 0xe0-0xfe for sonar, set read bit while(!SSPIF); // wait for interrupt SSPIF = 0; // then clear it. for(x=0; x<35; x++) { RCEN = 1; // start receiving while(!STAT_BF); // wait for data buffer[x] = SSPBUF; // and get it ACKEN = 1; // start acknowledge sequence while(ACKEN); // wait for ack. sequence to end } RCEN = 1; // start receiving while(!STAT_BF); // wait for data buffer[35] = SSPBUF; // and get it ACKDT = 1; // not acknowledge for last byte ACKEN = 1; // start acknowledge sequence while(ACKEN); // wait for ack. sequence to end PEN = 1; // send stop bit while(PEN); // } // read 8 bytes from motor board void read_motor(unsigned char addr, unsigned char *buffer) { char x; SEN = 1; // send start bit while(SEN); // and wait for it to clear ACKDT = 0; // acknowledge bit SSPIF = 0; SSPBUF = addr; // 0xb0-0xbe for motor board while(!SSPIF); // wait for interrupt SSPIF = 0; // then clear it. delay100(); // 100uS ish SSPBUF = 0; // read from address 0 while(!SSPIF); // SSPIF = 0; // delay100(); // 100uS ish RSEN = 1; // send repeated start bit while(RSEN); // and wait for it to clear SSPIF = 0; SSPBUF = addr+1; // 0xb0-0xbe for motor board, set read bit while(!SSPIF); // wait for interrupt SSPIF = 0; // then clear it. for(x=0; x<7; x++) { RCEN = 1; // start receiving while(!STAT_BF); // wait for data buffer[x] = SSPBUF; // and get it ACKEN = 1; // start acknowledge sequence while(ACKEN); // wait for ack. sequence to end } RCEN = 1; // start receiving while(!STAT_BF); // wait for data buffer[7] = SSPBUF; // and get it ACKDT = 1; // not acknowledge for last byte ACKEN = 1; // start acknowledge sequence while(ACKEN); // wait for ack. sequence to end PEN = 1; // send stop bit while(PEN); // } // Starts the sonar performing a ranging - it does not wait for it void start_sonar_ping(unsigned char addr, unsigned char cmd) { SEN = 1; // send start bit while(SEN); // and wait for it to clear SSPIF = 0; SSPBUF = addr; // must be 0xe0-0xfe for sonar while(!SSPIF); // wait for interrupt SSPIF = 0; // then clear it. delay100(); // 100uS ish SSPBUF = 0; // address of sonar command register while(!SSPIF); // SSPIF = 0; // delay100(); // 100uS ish SSPBUF = cmd; // sonar command while(!SSPIF); // SSPIF = 0; // PEN = 1; // send stop bit while(PEN); // } // writes data to a motor board register void write_md03(unsigned char addr, unsigned char reg, unsigned char data) { SEN = 1; // send start bit while(SEN); // and wait for it to clear SSPIF = 0; SSPBUF = addr; // must be 0xb0-0xbe for motor while(!SSPIF); // wait for interrupt SSPIF = 0; // then clear it. delay100(); // 100uS ish SSPBUF = reg; // address of register to write to while(!SSPIF); // SSPIF = 0; // delay100(); // 100uS ish SSPBUF = data; // write the data while(!SSPIF); // SSPIF = 0; // PEN = 1; // send stop bit while(PEN); // } //----------------------------------------------------------------------- // set up the PIC ports and initialise the LCD display void setup(void) { char x; __CONFIG(0x3f72); TRISA = 0xff; // All inputs TRISB = 0x00; // actually, its controlled by display driver TRISC = 0x18; // RC3-4 are inputs TRISD = 0x00; // RD4,3,2 are E,R/W,RS OPTION = 0x88; // portb pullups off - prescaler to wdt ADCON1 = 0x00; // all portA is analog - data left justified SSPSTAT = 0x80; // slew rate disabled SSPCON = 0x38; // enable port in 7 bit slave mode SSPCON2 = 0x00; // not a lot SSPADD = 0; // baud rate in address reg x = SSPBUF; // dummy read clears BF delay(); PORTB = 0x38; RW = 0; E = 0; RS = 0; E = 1; E = 0; delay(); E = 1; E = 0; delay(); E = 1; E = 0; delay(); lcd_cmd(0x08); lcd_cmd(0x01); lcd_cmd(0x0c); }