///////////////////////////////////////////////////////////////////////////
//
//     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);
}