main.c

/************************ HEADERS **********************************/
// Set the  myidcode  value to the number of the board !!!!
#define myidcode 9


#include "P2P.h"
#include "MRF24J40.h"
#include "SymbolTime.h"
#include "I2C.h"
#include "hardwareprofile.h"
    
/************************ VARIABLES ********************************/

/*******************************************************************/
// The variable myChannel defines the channel that the P2P connection
// is operate on. This variable will be only effective if energy scan
// (ENABLE_ED_SCAN) is not turned on. Once the energy scan is turned
// on, the operating channel will be one of the channels available with
// least amount of energy (or noise).
/*******************************************************************/
#define chanA (CHANNEL_11 + (myidcode-1)*16)

BYTE myChannel = chanA;
int timetoconnect;
TICK CTick;    
int uctry;
int myconectionid;
char running;

#include "typeDef.h"

int lasttag;
TICK txtime;

TICK waketime;
TICK lasttransactiontime;

BYTE BroadcastPacketAR[256];

int BroadcastPacketPoint,instate;
char Broadcast;
unsigned int REQCOUNT=0;

void CheckP2P(void);
void processCMD(BYTE cmdid,BYTE dbytes);

int oldV = 0;
int v;
int w = 0;
int ureceiver =0;
int R0LATDIST;
int R0MIDDIST;
int R1LATDIST;
int R1MIDDIST;

volatile int Ustate =0;
volatile int Ucycles =0;
volatile int Uthreshold;
volatile int UltraSonicChannel=0;
volatile int UltraSonicState;
volatile int UltraSonicCycles;
int Previous=0;
int lastad;
int ECHO;

// JAMES ADDING THIS SHIT IN TO SEND STUFF EVERY NTH PACKET
int packetRecdCount = 0;


void setupP2P(void)
{
	InitSymbolTimer();
	P2PInit();   
	#if defined(PICDEMZ)
		INTCONbits.GIEH = 1;
	#elif defined(EXPLORER16)
	#else
	#endif
	running=0;
	SetChannel(myChannel);
	
	/*******************************************************************/
	// Function EnableNewConnection will enable the device to accept
	// request to establish P2P connection from other devices. If a
	// P2P connection has been established, user can choose to call
	// function DisableNewConnection() to reject further request to 
	// establish P2P connection, if it is desired for the application
	/*******************************************************************/
	EnableNewConnection();
}

void CheckP2P(void)
{
	BYTE i;
	
	/*******************************************************************/
	// Function ReceivedPacket will return a boolean to indicate if a 
	// packet has been received by the transceiver. If a packet has been
	// received, all information will be stored in the rxFrame, structure  
	// of RECEIVED_FRAME.
	/*******************************************************************/
	if( ReceivedPacket() )
	{
		lasttransactiontime=TickGet();
		/*******************************************************************/
		// If a packet has been received, following code prints out some of
		// the information available in rxFrame.
		/*******************************************************************/
		if (rxFrame.SourceLongAddress[4]==myidcode)
		{
			if (rxFrame.PayLoadSize>0)
			{ 
				processCMD(rxFrame.PayLoad[0],rxFrame.PayLoadSize-1);
			}
		}
		/*******************************************************************/
		// Function DiscardPacket is used to release the current received packet.
		// After calling this function, the stack can start to process the
		// next received frame 
		/*******************************************************************/
		DiscardPacket();
	}
	else
	{
		/*******************************************************************/
		// Macro isDataRequesting returns the boolean to indicate if
		// the Data Request command has received any feedback from its 
		// associated device. If there won't be any message from the associate
		// device to the device with radio off during idle, the RFD device
		// will not required to send out Data Request command, thus the return
		// value of macro isDataRequesting is always FALSE
		/*******************************************************************/
		if( isDataRequesting() == FALSE )
		{
			if (Broadcast)
			{
				FlushTx();
				SlaveID=myidcode;
				for (i=0;i<Broadcast;i++)
				{
					WriteData(BroadcastPacketAR[i]);
				}
				BroadcastPacket(myPANID, FALSE, FALSE);
				Broadcast=0;
				txtime=TickGet();
			}
			
			#ifdef ENABLE_FREQUENCY_AGILITY
			/***********************************************************************
			* AckFailureTimes is the global variable to track the transmission 
			* failure because no acknowledgement frame is received. Typically,
			* this is the indication of either very strong noise, or the PAN
			* has hopped to another channel. Here we call function ResyncConnection
			* to resynchronize the connection.
			*   The first parameter is the destination long address of the peer node
			*   that we would like to resynchronize to.
			*   The second parameter is the bit map of channels to be scanned
			*************************************************************************/
			if( AckFailureTimes > ACK_FAILURE_TIMES )
			{
				ResyncConnection(P2PConnections[0].PeerLongAddress, 0x07FFF800);    
			}
			
			#endif
		
		}   // end of data requesting
	}
}

void processCMD(BYTE cmdid,BYTE dbytes)
{
	int speed,tmp;
	int i;
	unsigned int workword;
	
	switch (cmdid)
	{
		case 0:
			BroadcastPacketAR[0]=0;
			Broadcast=1;  // number in packet
			break;
			
		case 1:
			BroadcastPacketAR[0]=1;
			workword=PORTB;workword=~(workword>>12) &3;
			workword |=((LATB>>6) & 0x30);
			BroadcastPacketAR[1]=workword & 0x33;
			Broadcast=2;  // number in packet
			break;
			
		case 2:
			if (dbytes==1)
			{
				_LATB10=!(rxFrame.PayLoad[1] & 1);
				if (rxFrame.PayLoad[1] & 2) _LATB11=0; else _LATB11=1;
			}
			break;
			
		case 3:
			BroadcastPacketAR[0]=3;  // get internal counter
			BroadcastPacketAR[1]=REQCOUNT & 0xff;
			BroadcastPacketAR[2]=(REQCOUNT>>8) & 0xff;REQCOUNT++;
			Broadcast=3;  // number in packet
			break;
			
		case 4:
			if (dbytes==1)	// set motor speed
			{
				speed=(char)rxFrame.PayLoad[1];
				if (speed>100) speed=100;
				if (speed<-100) speed=-100;
				I2S();I2send(0xb0);I2send(1);I2send(speed);I2P();
			}
			break;
			
		case 5: // request motor status here via I2C
			// on receive data send result via MiWi
			I2S();I2send(0xb0);I2send(1);I2SR();I2send(0xb1);
			BroadcastPacketAR[0]=5;
			BroadcastPacketAR[1]=I2GET(1);tmp=I2GET(1);tmp=I2GET(1);
			for (i=0;i<12;i++)
			{
				BroadcastPacketAR[i+2]=I2GET(i!=11);
			}
			I2P();Broadcast=14;  // number in packet
			break;
			
		case 6:
			if (dbytes==1)	// set power
			{
				I2S();I2send(0xC0);I2send(0);I2send(rxFrame.PayLoad[1]);I2P();
			}
			break;
			
		case 7:
			if (dbytes==1)	// set servo pwm
			{
				I2S();I2send(0xb0);I2send(7);I2send(rxFrame.PayLoad[1]);I2P();
			}
			break;
			
		case 8:  // request motor current here via I2C
			// on receive data send result via MiWi
			I2S();I2send(0xb0);I2send(3);I2SR();I2send(0xb1);
			BroadcastPacketAR[0]=8;
			BroadcastPacketAR[1]=I2GET(0);
			I2P();Broadcast=2;  // number in packet
			break;
			
		case 9:  // request servo setup via I2C
			// on receive data send result via MiWi
			I2S();I2send(0xb0);I2send(16);I2SR();I2send(0xb1);
			BroadcastPacketAR[0]=9;
			for (i=0;i<8;i++)
			{
				BroadcastPacketAR[i+1]=I2GET(i!=7);
			}
			I2P();Broadcast=9;  // number in packet
			break;
			
		case 10:  
			if (dbytes==8)	// set servo setup via I2C
			{
				// on receive data send result via MiWi
				I2S();I2send(0xb0);I2send(16);
				for (i=0;i<8;i++)
				{
					I2send(rxFrame.PayLoad[i+1]);
				}
				I2P();
			} 
			break;
			
		case 128: // generic control packet
			if( packetRecdCount++ > 20 )
			{
				_LATB10=0; _LATB11=0;
				packetRecdCount = 0;
			}
			else { _LATB10=1;_LATB11=1; }
			
			speed=(char)rxFrame.PayLoad[2];
			if( speed > 100 ) speed = 100;
			if ( speed < -100 ) speed =- 100;
			
			/*(speed > 5) {_LATB11=0; _LATB10 = 1; }
			else if (speed < -5){ _LATB11=1; _LATB10 = 0;}
			else {_LATB10=1;_LATB11=1;}*/
			I2S();I2send(0xb0);I2send(1);I2send(speed);I2P();
			I2S();I2send(0xb0);I2send(7);I2send(rxFrame.PayLoad[1]);I2P();
			if( rxFrame.PayLoad[5] == 0x01 )
			{
				I2S();I2send(0xb0);I2send(4);I2SR();I2send(0xb1);
				BroadcastPacketAR[0]=129;
				BroadcastPacketAR[1]=I2GET(0);
				I2P();Broadcast=2;
			}
			
			// Sending V
			if( packetRecdCount % 1 == 0 )
			{
				BroadcastPacketAR[0]=254;
				BroadcastPacketAR[1]=v;
				BroadcastPacketAR[2]=123;
				I2P();Broadcast=3;
			}
			
			// Motor Speed
//			if( rxFrame.PayLoad[3] == 0x01 )
			if( packetRecdCount % 20 == 0 ) // Every 4 control packets.
			{
				I2S();I2send(0xb0);I2send(1);I2SR();I2send(0xb1);
				BroadcastPacketAR[0]=5;
				BroadcastPacketAR[1]=100;
				BroadcastPacketAR[1]+=I2GET(1);tmp=I2GET(1);tmp=I2GET(1);
				for (i=0;i<12;i++)
				{
					I2GET(i!=11);
				}
				I2P();Broadcast=2;  // number in packet
			}
			break;
			
	}
}


/**
 * This function is run once every program cycle. Every time it is run it checks Ustate value
 * to determine what part of the ultrasonic procedure to perform. First the chip is sent, then
 * the system has a "cooldown" period, and then any results from the receivers are sampled by the ADC
 **/
int ultrasonics(void)
{
	switch (Ustate){ 			
		case 0: 
			Ustate++;            
			if(!UltraSonicChannel){		//Send chirp on appropriate channel                              
					OC2CON = 0xe;
					OC3CON = 0x0;
					OC4CON = 0x0;
			}
			else{
					OC3CON = 0xe;
					OC2CON = 0x0;
					OC4CON = 0xe;
			}  				
			break;
		case 1:
			if ((Ucycles++)==8){
				Ustate++;
				OC2CON = 0x0; //Stops OC PWM
				OC3CON = 0x0;
				OC4CON = 0x0;
			}
			break;
		case 2:
			if ((Ucycles++)==21){ //cooldown for 20 cycles, then:
				Ustate++;				//Prepares ADC for sampling
				AD1CON2=0;
				AD1CON1=0x04e0;
				AD1CON3=0x0202;
				AD1CON1=0x84e0;
				if (!ureceiver){
					AD1CHS0 = 0x16;	//selects appropriate pin for sampling
				}			 
				else {
					AD1CHS0 = 0x17;                                                        
				}
				AD1CON1bits.SAMP=1; Uthreshold = 0x7FFF; //samples input pin

			}
			break;

		case 3:
			v=ADC1BUF0;AD1CON1bits.SAMP=1; //reads from sampling buffer, samples again
			if (v>Uthreshold) {  //If we have received the chirp, 
				Ustate++;			//move to next state                     
				if (!UltraSonicChannel){		//Save distances as cycles taken to receive chirp                             
					if (!ureceiver)R0MIDDIST = Ucycles-21;
					else R1MIDDIST = Ucycles-21;
				}
				else{
					if (!ureceiver)R0LATDIST = Ucycles-21;
					else R1LATDIST = Ucycles-21;
				}
			}			
			else {
				Uthreshold = v+32;			//If we've received nothing, set the threshold to a low value
				Ucycles++;					//do not move on until something is received/max cycles reached.
			}
			if ((Ucycles++) > 820){
				Ustate++;
				switch (UltraSonicChannel){
				if (!UltraSonicChannel){		//Save distances as cycles taken to receive chirp                             
						if (!ureceiver)R0MIDDIST = 0x0FFF;
						else R1MIDDIST = 0x0FFF;
				}
				else{
					if (!ureceiver)R0LATDIST = 0x0FFF;
					else R1LATDIST = 0x0FFF;
				}						
			}				
			break;
			case 4:
				Ucycles = 0;
				Ustate = 0;
				UltraSonicChannel=!UltraSonicChannel;
				if (!UltraSonicChannel) ureceiver=!ureceiver;
				break;
		}
	}
}

void setUpRegs()
{
	TRISB=0x3030;
	
	AD1CON2=0x0000;
	AD1CON1=0x04E0;
	AD1CON3=0x0202;
	AD1CON1=0x84E0;
	AD1CHS0=0x05;
	
	AD1PCFGL=0xFFC7; // 11111111 11000111 
	AD1PCFGH=0xFFFF;
}

int getSample()
{
	int adcpin5 = ADC1BUF0;
	AD1CON1bits.SAMP=1;
	return adcpin5;
}
int main(void)
{   
	long int idel;
	/*******************************************************************/
	// Initialize the system
	/*******************************************************************/
	for (idel=0;idel<0x7fff;idel++);
	BoardInit();
	setupP2P();
	running=0;
	waketime=lasttransactiontime=txtime=TickGet();uctry=0;
	
	setUpRegs();
	
	
	timetoconnect=0;
	lasttag=0;
	BroadcastPacketPoint=0;instate=0;
	SlaveID=0;Broadcast=0;
	BroadcastPacketAR[0]=0;Broadcast=1;
	_LATB10=0;_LATB11=1;
	while(1)
	{
		v=getSample();
		CTick = TickGet();
		CheckP2P();
		//ultrasonics();
//		if( w > 254 ) w = 0;
//		w += v;
	}
}