#include "compic.h"

////////////////////////////////////////////////////////////////////////////////
// compic.c                                                                   //
// Mini Laser Tweezers comPIC firmware version 0.2.2                          //
//                                                                            //
// Description: Firmware for the comPIC, communicates to host over USB        //
//              routes data between host, motorPIC and trapPIC's              //
//              timing source for feedback rate                               //
//                                                                            //
// Authors: Steve Smith, Shane Saxon, Zach Radding                            //
//                                                                            //
////////////////////////////////////////////////////////////////////////////////


enum{
   kHostPacketID = 42,           //communication with host packet header ID
   kPicPacketID = 213,           //inter-PIC communication packet header ID
   kCommandPacketSize = 31,      //size of the incoming host command packet
   kTrapReadBufferSize = 21,
   kMotorBufferSize = 5,
   kTrapWriteBufferSize = 16
};



int16 *ramPointer;   //used by ltClearRam() to initialize all memory
int8 firstAddress;   //variables above this line not cleared
                     //CAUTION DIRECT MEMORY ACCESS USED FOR USB WRITE BUFFER

int8 checkSum;                      //for communications

int8 commandBuffer[kCommandPacketSize];        //commands and settings from host
int8 trapWriteBuffer[kTrapWriteBufferSize];     //commands sent to both traps
int8 motorWriteBuffer[kMotorBufferSize];

int8 trapAReadBuffer[kTrapReadBufferSize+kTrapReadBufferSize+kMotorBufferSize];   //data from trapA

int8 commandBufferIndex;
int8 commandCheckSum;
int8 commandBufferGood;
int8 previousCommandCount;

//do not change any variables above this line, machine code sensitive!

int8 errorNumber, errorTemp;
int8 readByteGood;      //indicates no errors in readSettings
int8 trapAReadBufferGood, trapBReadBufferGood;

      //variables for host communication and data testing
int8 readHostByte, writeData;
int8 i, timeOut, motorReadByte, motorWriteByte;

int8 cycleCountPointer, cycleCountHiPointer;

int8 readIndex, writeIndex;

int16 cycleTime;
int16 cycleCount;
int16 cycleCountHiByte;              //increments every time cycleCount rolls over
int16 timeOutCount;


////////////////////////////////////////////////////////
//clears all GPR's
void ltClearRam(void)
{
for (ramPointer = *firstAddress; ramPointer < 0x7FE; ramPointer+=2)
	{
//		if(ramPointer != *cycleTime)  //example of how to skip variables
      *ramPointer = 0;
	}
}


////////////////////////////////////////////////////////
void ltInitVariables(void)
{
ltClearRam();

cycleTime = 63161 + 11;         //65061 is 20kHz, 5kHz 63636, 4kHz 63161, 2kHz 60786, 1kHz 56036
                                //11 is overhead for calculating timer interrupt
cycleCount = 0;
cycleCountHiByte = 0;
timeOut = 0;
errorNumber = 0;

writeData = 1;

commandBufferIndex = 0;
commandCheckSum = 0;
commandBufferGood = 0;

cycleCountPointer = &cycleCount;
cycleCountHiPointer = &cycleCountHiByte;
readIndex = &trapAReadBuffer[0];
writeIndex = &trapAReadBuffer[0];
}


////////////////////////////////////////////////////////
void ltSetupPic(void)
{
setup_adc_ports(NO_ANALOGS);
setup_adc(ADC_OFF);
setup_psp(PSP_DISABLED);
setup_wdt(WDT_ON);                           //turn watch dog timer on
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);     //timer used by interrupt handler
setup_timer_1(T1_DISABLED);
setup_timer_2(T2_DISABLED,0,1);
setup_timer_3(T3_DISABLED);
setup_timer_4(T4_DISABLED,0,1);
setup_comparator(NC_NC_NC_NC);
setup_vref(FALSE);
}


////////////////////////////////////////////////////////
//the code for setup_spi does not work perfectly, so we are using our own,
//setup using assembly instructions so that it matches PIC18F6520 datasheet
//ports and register addressess may need to be changed if different PIC
void ltSpiCustomSetup(void)
{
#asm
                  //first, configure port_C tri-state, (the SPI port)
BCF    0xF94.5             //SDO, PIN C5 is data out
BSF    0xF94.4             //SDI, PIN C4 is data in
BCF    0xF94.3             //SCK, PIN C3 sets clock to output for master mode

                  //second, setup the SSPCON1 and SSPSTAT registers
MOVLW  0x00                //SPI MODE_01 uses 0x00
MOVWF  0xFC7               //SSPSTAT register

MOVLW  0x00                //SPI MODE_01 uses 0x00, 9.5Mbps with 38Mhz clock
MOVWF  0xFC6               //SSPCON1 register

                  //third, enable the SPI port using SSPEN bit
BSF    0xFC6,5             //SSPEN bit of SSPCON1
#endasm
}


/////////////////////////////////////////////////////////////////////////////
// sets up the ports and reads all the data from the FTDI chip to clear buffer
// resets the FTDI-245BM (USB chip) if busy for too long
void ltClearUSB(void)
{
set_tris_d(0xFF);          //set port D to input, this is the FTDI USB data port

while (input(PIN_A3) == 0)             //0 when there is data in the buffer
	{
      timeOutCount = 0;                //reset the timeout counter
      while (input(PIN_A2))            //1 when the chip is busy
      {
         if (timeOutCount++ > 500)     //exception handling for USB lock-up
            {
               if (!errorNumber)
                  errorNumber = 44;    //error 33, USB timeout on chip busy

               output_low(PIN_A4);     //reset USB if locked up
               delay_us(100);
               output_high(PIN_A4);
            }
      }
   	output_high(PIN_A0);       //here we just clock the data out, disgarded
   	output_low(PIN_A0);
      output_high(PIN_A0);
	}
}


////////////////////////////////////////////////////////
void ltSetupPorts(void)
{
set_tris_a(0xEC);          //FTDI USB reset, control, and status lines
set_tris_d(0xFF);          //set port D to input, this is the FTDI USB data port
output_high(PIN_A0);       //read pin(RD), high to low triggers data transfer
output_high(PIN_A1);       //clock pin(WR), high to low triggers data transfer
output_high(PIN_A4);       //reset pin for the FTDI USB, low resets the USB
ltClearUSB();              //resets and empties the USB read buffer

set_tris_b(0x00);          //ports used for control of SPI devices, not SPI port
output_high(PIN_B0);       //motorPIC SS, active on low, currently unused,    debug
output_high(PIN_B3);       //motorPIC chip select(CS pin), active on low

set_tris_c(0xFE);
ltSpiCustomSetup();        //setup_spi doesn't work right, so we use our own

set_tris_e(0xF0);          //PIC to PIC bus control lines
output_high(PIN_E0);
output_high(PIN_E1);
output_high(PIN_E2);       //not ready to communicate with other PICs on 8-bit bus
output_low(PIN_E3);

set_tris_f(0xFF);          //8 bit bus between the PICs
set_tris_g(0xFF);          //not used, could put status LED's on this port,    debug
}


////////////////////////////////////////////////////////
void ltResetComPIC(void)
{
ltInitVariables();

ltSetupPic();

ltSetupPorts();

ltClearUSB();              //resets and empties the USB read buffer
}


////////////////////////////////////////////////////////
//writes a USB packet with data assigned by writeID[] array
void ltWriteHostPacket(void)
{
set_tris_d(0x00);

#asm
MOVLW kHostPacketID
MOVWF 0xF8C                         //put packet ID on portD for the USB chip
                                    //the ID is sent twice in a row
CHECK0:
BTFSC 0xF80.2                       //wait for FTDI USB chip to not be busy
GOTO  CHECK0
BSF   0xF80.1                       //toggle the write pin on the USB chip
BCF   0xF80.1

CHECK1:
BTFSC 0xF80.2
GOTO  CHECK1
BSF   0xF80.1
BCF   0xF80.1

ADDLW kHostPacketID              //put the first checksum value in w

CLRF  0xFEA                         //clear the high byte of FSR0, we use bank0

MOVFF cycleCountPointer,0xFE9           //point low byte of FSR0L to low data byte
MOVFF 0xFEF,0xF8C                   //put first data byte for the USB, port D
ADDWF 0xFEF,0                       //calculates checksum by adding up data
CHECK2:
BTFSC 0xF80.2
GOTO  CHECK2
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1                       //increment FSR0L for the high data byte
MOVFF 0xFEF,0xF8C                   //put first data byte for the USB, port D
ADDWF 0xFEF,0
CHECK3:
BTFSC 0xF80.2
GOTO  CHECK3
BSF   0xF80.1
BCF   0xF80.1


MOVFF readIndex,0xFE9
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK4:
BTFSC 0xF80.2
GOTO  CHECK4
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK5:
BTFSC 0xF80.2
GOTO  CHECK5
BSF   0xF80.1
BCF   0xF80.1


INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK6:
BTFSC 0xF80.2
GOTO  CHECK6
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK7:
BTFSC 0xF80.2
GOTO  CHECK7
BSF   0xF80.1
BCF   0xF80.1


INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK8:
BTFSC 0xF80.2
GOTO  CHECK8
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK9:
BTFSC 0xF80.2
GOTO  CHECK9
BSF   0xF80.1
BCF   0xF80.1


INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK10:
BTFSC 0xF80.2
GOTO  CHECK10
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK11:
BTFSC 0xF80.2
GOTO  CHECK11
BSF   0xF80.1
BCF   0xF80.1


INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK12:
BTFSC 0xF80.2
GOTO  CHECK12
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK13:
BTFSC 0xF80.2
GOTO  CHECK13
BSF   0xF80.1
BCF   0xF80.1


INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK14:
BTFSC 0xF80.2
GOTO  CHECK14
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK15:
BTFSC 0xF80.2
GOTO  CHECK15
BSF   0xF80.1
BCF   0xF80.1


INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK16:
BTFSC 0xF80.2
GOTO  CHECK16
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK17:
BTFSC 0xF80.2
GOTO  CHECK17
BSF   0xF80.1
BCF   0xF80.1


INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK18:
BTFSC 0xF80.2
GOTO  CHECK18
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK19:
BTFSC 0xF80.2
GOTO  CHECK19
BSF   0xF80.1
BCF   0xF80.1


INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK20:
BTFSC 0xF80.2
GOTO  CHECK20
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK21:
BTFSC 0xF80.2
GOTO  CHECK21
BSF   0xF80.1
BCF   0xF80.1


INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK22:
BTFSC 0xF80.2
GOTO  CHECK22
BSF   0xF80.1
BCF   0xF80.1


INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK23:
BTFSC 0xF80.2
GOTO  CHECK23
BSF   0xF80.1
BCF   0xF80.1


INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK24:
BTFSC 0xF80.2
GOTO  CHECK24
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK25:
BTFSC 0xF80.2
GOTO  CHECK25
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK26:
BTFSC 0xF80.2
GOTO  CHECK26
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK:
BTFSC 0xF80.2
GOTO  CHECK
BSF   0xF80.1
BCF   0xF80.1


INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK27:
BTFSC 0xF80.2
GOTO  CHECK27
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK28:
BTFSC 0xF80.2
GOTO  CHECK28
BSF   0xF80.1
BCF   0xF80.1


INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK29:
BTFSC 0xF80.2
GOTO  CHECK29
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK30:
BTFSC 0xF80.2
GOTO  CHECK30
BSF   0xF80.1
BCF   0xF80.1


INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK31:
BTFSC 0xF80.2
GOTO  CHECK31
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK32:
BTFSC 0xF80.2
GOTO  CHECK32
BSF   0xF80.1
BCF   0xF80.1


INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK33:
BTFSC 0xF80.2
GOTO  CHECK33
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK34:
BTFSC 0xF80.2
GOTO  CHECK34
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK35:
BTFSC 0xF80.2
GOTO  CHECK35
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK36:
BTFSC 0xF80.2
GOTO  CHECK36
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK37:
BTFSC 0xF80.2
GOTO  CHECK37
BSF   0xF80.1
BCF   0xF80.1



INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK38:
BTFSC 0xF80.2
GOTO  CHECK38
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK39:
BTFSC 0xF80.2
GOTO  CHECK39
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK40:
BTFSC 0xF80.2
GOTO  CHECK40
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK41:
BTFSC 0xF80.2
GOTO  CHECK41
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK42:
BTFSC 0xF80.2
GOTO  CHECK42
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK43:
BTFSC 0xF80.2
GOTO  CHECK43
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK44:
BTFSC 0xF80.2
GOTO  CHECK44
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK45:
BTFSC 0xF80.2
GOTO  CHECK45
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK46:
BTFSC 0xF80.2
GOTO  CHECK46
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK47:
BTFSC 0xF80.2
GOTO  CHECK47
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK48:
BTFSC 0xF80.2
GOTO  CHECK48
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK49:
BTFSC 0xF80.2
GOTO  CHECK49
BSF   0xF80.1
BCF   0xF80.1

//end of motor data
INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK50:
BTFSC 0xF80.2
GOTO  CHECK50
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK51:
BTFSC 0xF80.2
GOTO  CHECK51
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK52:
BTFSC 0xF80.2
GOTO  CHECK52
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK53:
BTFSC 0xF80.2
GOTO  CHECK53
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK54:
BTFSC 0xF80.2
GOTO  CHECK54
BSF   0xF80.1
BCF   0xF80.1



MOVFF errorNumber,0xF8C
ADDWF errorNumber,0
CHECK55:
BTFSC 0xF80.2
GOTO  CHECK55
BSF   0xF80.1
BCF   0xF80.1


MOVFF cycleCountPointer,0xFE9         //point low byte of FSR0L to low data byte
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK56:
BTFSC 0xF80.2
GOTO  CHECK56
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK57:
BTFSC 0xF80.2
GOTO  CHECK57
BSF   0xF80.1
BCF   0xF80.1


MOVFF cycleCountHiPointer,0xFE9
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK58:
BTFSC 0xF80.2
GOTO  CHECK58
BSF   0xF80.1
BCF   0xF80.1

INCF  0xFE9,1
MOVFF 0xFEF,0xF8C
ADDWF 0xFEF,0
CHECK59:
BTFSC 0xF80.2
GOTO  CHECK59
BSF   0xF80.1
BCF   0xF80.1



MOVWF 0xF8C                         //put w on port D, sends the checksum
CHECK60:
BTFSC 0xF80.2
GOTO  CHECK60
BSF   0xF80.1
BCF   0xF80.1

#endasm
}


////////////////////////////////////////////////////////
void ltProcessCommands(void)
{
if (commandBufferGood)           //copies data to trapWriteBuffer
{
   for (i = 0; i < kTrapWriteBufferSize; i++)
      trapWriteBuffer[i] = commandBuffer[3 + i];


   for (i = 0; i < kMotorBufferSize; i++)
         motorWriteBuffer[i] = commandBuffer[(3 + kTrapWriteBufferSize) + i];
/*
   if ( commandBuffer[3 + kTrapWriteBufferSize + kMotorBufferSize] == 22)
      writeData = commandBuffer[3 + kTrapWriteBufferSize + kMotorBufferSize + 1];
*/
   if ( commandBuffer[3 + kTrapWriteBufferSize + kMotorBufferSize + 2] == 99)
     cycleTime = commandBuffer[3 + kTrapWriteBufferSize+kMotorBufferSize + 3]  //could be improved, debug
      * 256 + commandBuffer[3 + kTrapWriteBufferSize + kMotorBufferSize + 4]
      + 11; //11 is for overhead of calculating timer in interrupt routine

   commandBufferGood = 0;
}
else
   for (i = 0; i < 16; i++)   //clear buffer, prevents sending commands twice
      trapWriteBuffer[i] = 0;
}


////////////////////////////////////////////////////////
void ltReadHostByte(void)
{
#use fast_io(D)
#use fast_io(A)

while (input(PIN_A2)){}                //wait if USB chip busy
output_high(PIN_A0);							//tell the USB chip to output it's data
readHostByte = input_d();
output_low(PIN_A0);
}


////////////////////////////////////////////////////////
//reads and processess command packets from host, breaks if nothing in the
//buffer, can process packets split between multiple calls to this function
void ltReadHostPacket(void)
{
#use fast_io(A)

set_tris_d(0xFF);

if (input(PIN_A3))      //timeOut is to stop USB if no host
   {
      if (timeOut != 0)
         timeOut--;
   }
else
   timeOut = 255;

while (commandBufferIndex < 2)         //search for packet header
   {
      if (input(PIN_A3))               //exit if nothing in the buffer
         return;

      ltReadHostByte();

      if (readHostByte == kHostPacketID)
         commandBuffer[commandBufferIndex++] = readHostByte;
      else
            commandBufferIndex = 0;
   }

while (commandBufferIndex < kCommandPacketSize)    //get rest of packet
   {
      if (input(PIN_A3))
         return;

      ltReadHostByte();
      commandBuffer[commandBufferIndex++] = readHostByte;
   }

commandCheckSum = 0;
for (i = 0; i < kCommandPacketSize - 1; i++)
   commandCheckSum += commandBuffer[i];


      //ok to process commands if checksum good and commandCount header == tail
if (  commandBuffer[kCommandPacketSize - 1]  == commandCheckSum &
      commandBuffer[2] == commandBuffer[kCommandPacketSize - 2] )
   {

      if (previousCommandCount == commandBuffer[2])
         commandBufferGood = 0;
      else
         {
            commandBufferGood = 1;
            previousCommandCount = commandBuffer[2];
         }
   }


commandBufferIndex = 0;       //reset to process next packet
commandCheckSum = 0;
}

////////////////////////////////////////////////////////
//sync from piezoPIC and send one option byte with ID
void ltWriteTrapCommands(void)
{
#use fast_io(E)
#use fast_io(F)

errorTemp = 0;          //forces us into Block0 of RAM since we use direct memory addressing


if (input(PIN_E5)      || input(PIN_E7) ||
       input(PIN_E4) == 0 || input(PIN_E6) == 0) //check trapPIC's write line
   {

      if (errorNumber == 0)
            errorNumber = 88;       //error 88, trapPIC writeting out of sync
   }


output_low(PIN_E0);

delay_us(2);

set_tris_f(0x00);       //set 8bit parrallel port to read from comPIC

#asm
movlw kPicPacketID      //load packetID into register w
movwf 0xF85             //put byte value in w register onto the data bus port

bsf 0xf84,3             //C2 high, ready, comPIC will exit loop within 3 cycles
nop
nop
bcf 0xf84,3             //comPIC uses this to catch up one cycle, if behind
nop
nop
bsf 0xf84,3             //for comPIC to catch up one more cycle, if needed
bcf 0xf84,3
nop
nop
nop
nop                     //piezoPIC and comPIC are now synced together


movff trapWriteBuffer[0],0xF85   //transfer next byte to readbyte array
addwf trapWriteBuffer[0],0       //calculates checksum
nop

movff trapWriteBuffer[1],0xF85     //transfer next byte to readbyte array
addwf trapWriteBuffer[1],0         //calculates checksum
nop

movff trapWriteBuffer[2],0xF85   //transfer next byte to readbyte array
addwf trapWriteBuffer[2],0       //calculates checksum
nop

movff trapWriteBuffer[3],0xF85
addwf trapWriteBuffer[3],0
nop

movff trapWriteBuffer[4],0xF85
addwf trapWriteBuffer[4],0
nop

movff trapWriteBuffer[5],0xF85
addwf trapWriteBuffer[5],0
nop

movff trapWriteBuffer[6],0xF85
addwf trapWriteBuffer[6],0
nop

movff trapWriteBuffer[7],0xF85
addwf trapWriteBuffer[7],0
nop

movff trapWriteBuffer[8],0xF85
addwf trapWriteBuffer[8],0
nop

movff trapWriteBuffer[9],0xF85
addwf trapWriteBuffer[9],0
nop

movff trapWriteBuffer[10],0xF85
addwf trapWriteBuffer[10],0
nop

movff trapWriteBuffer[11],0xF85
addwf trapWriteBuffer[11],0
nop

movff trapWriteBuffer[12],0xF85
addwf trapWriteBuffer[12],0
nop

movff trapWriteBuffer[13],0xF85
addwf trapWriteBuffer[13],0
nop

movff trapWriteBuffer[14],0xF85
addwf trapWriteBuffer[14],0
nop

movff trapWriteBuffer[15],0xF85
addwf trapWriteBuffer[15],0
nop

movwf 0xF85             //send checksum
nop
nop
nop
#endasm

set_tris_f(0xFF);

output_high(PIN_E0);
output_low(PIN_E1);                      //allows for other delays,             debug
}

////////////////////////////////////////////////////////
void ltReadTrapAData(void)
{
#use fast_io(E)
#use fast_io(F)

errorTemp = 0;

#asm
CLRF     0xFEA
MOVFF    writeIndex,0xFE9        //point the FSR to the current right position
#endasm


if (input(PIN_E4))                     //add errors for A, B, or both A&B
   errorTemp = 77;                     //error 78, trap A unavailable


set_tris_f(0xFF);       //set 8bit parrallel port to read from comPIC

#asm
BSF 0xf84,3             //C2 high, ready, comPIC will exit loop within 3 cycles
NOP
NOP
BCF 0xf84,3             //comPIC uses this to catch up one cycle, if behind
NOP
NOP
BSF 0xf84,3             //for comPIC to catch up one more cycle, if needed
BCF 0xf84,3
NOP                     //add or subtract nop's here to callibrate timing
NOP                     //use oscilloscope on pins 0, 5 & 7 at 100ns

MOVLW kPicPacketID      //this is the packet ID, we put it in register w
CPFSEQ 0xF85            //if 1st byte is not the right ID we exit
goto  check0             //copy 1st byte from w to trapReadBuffer
NOP
nop

MOVFF 0xF85,0xFEF       //copy byte read to location specified by FSR
ADDWF 0xFEF,0           //calculates checksum

INCF  0xFE9             //increment the FSR pointer for the next byte
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

NOP
CPFSEQ 0xF85
goto  check1                   //if w not same as checksum generate error
goto  check2                   //were good, exit

check0:
MOVLW 32                       //error3, not a read mode packet
MOVWF errorTemp
goto  check2

check1:
MOVLW 33                       //error4, bad checksum from read mode packet
MOVWF errorTemp
goto  check2

check2:
#endasm

output_high(PIN_E1);
output_low(PIN_E2);           //works faster with low here, debug

if (errorTemp)
   {
      trapAReadBufferGood = 0;  //bad packet don't process

      if (errorNumber == 0)
         errorNumber = errorTemp;
   }
else
   trapAReadBufferGood = 1;       //no errors, the data is ok to process
}

////////////////////////////////////////////////////////
void ltReadTrapBData(void)
{
#use fast_io(E)
#use fast_io(F)

errorTemp = 0;
#asm
CLRF     0xFEA
MOVFF    writeIndex,0xFE9        //point the FSR to the current right position
MOVLW    kTrapReadBufferSize
ADDWF    0xFE9
#endasm

if (input(PIN_E6))                     //wait for trapPIC
      errorTemp = 80;                  //error 80, trap B unavailable

set_tris_f(0xFF);       //set 8bit parrallel port to read from comPIC

#asm
BSF 0xf84,3             //C2 high, ready, comPIC will exit loop within 3 cycles
NOP
NOP
BCF 0xf84,3             //comPIC uses this to catch up one cycle, if behind
NOP
NOP
BSF 0xf84,3             //for comPIC to catch up one more cycle, if needed
BCF 0xf84,3
NOP                     //add or subtract nop's here to callibrate timing
NOP                     //use oscilloscope on pins 0, 5 & 7 at 100ns

MOVLW kPicPacketID      //this is the packet ID, we put it in register w
CPFSEQ 0xF85            //if 1st byte is not the right ID we exit
goto  check0             //copy 1st byte from w to trapReadBuffer
NOP

MOVFF 0xF85,0xFEF       //copy byte read to location specified by FSR
ADDWF 0xFEF,0           //calculates checksum

INCF  0xFE9             //increment the FSR pointer for the next byte
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

INCF  0xFE9
MOVFF 0xF85,0xFEF
ADDWF 0xFEF,0

NOP
CPFSEQ 0xF85
goto  check1                   //if w not same as checksum generate error
goto  check2                   //were good, exit

check0:
MOVLW 16                       //error16, bad packetID, trapPIC B
MOVWF errorTemp
goto  check2

check1:
MOVLW 17                       //error17, bad checksum from trapPIC B
MOVWF errorTemp
goto  check2

check2:
#endasm


output_high(PIN_E2);

if (errorTemp)
   {
      trapBReadBufferGood = 0;  //bad packet don't process

      if (errorNumber == 0)
         errorNumber = errorTemp;
   }
else
   trapBReadBufferGood = 1;       //no errors, the data is ok to process
}

////////////////////////////////////////////////////////
//sends and reads a byte over SPI with the motor controller
//bytes are synchronously sent both directions
void ltMotorSPI(void)
{
#asm
   nop                           //delay allows slave (motorPIC) to be ready
   nop
   nop
   nop

   movff motorWriteByte,0xFC9     //SSPBUF, starts SPI transfer

check0:
   btfss 0xFC7,0        //SSPSTAT, BF bit, loops until the byte is transferred
   goto  check0

   INCF 0xFE9                  //increment the FSR pointer
   MOVFF 0xFC9,0xFEF           //copy byte read to location specified by FSR
#endasm
}

////////////////////////////////////////////////////////
//sends and reads motor packet
void ltMotorCommunication(void)
{
#asm
CLRF     0xFEA
MOVFF    writeIndex,0xFE9        //point the FSR to the current right position
MOVLW    kTrapReadBufferSize
ADDWF    0xFE9
ADDWF    0xFE9
DECF     0xFE9
#endasm

output_low(PIN_B3);                    //enable the motorPIC SPI communication

motorWriteByte = motorWriteBuffer[0];
ltMotorSPI();

motorWriteByte = motorWriteBuffer[1];
ltMotorSPI();

motorWriteByte = motorWriteBuffer[2];
ltMotorSPI();

motorWriteByte = motorWriteBuffer[3];
ltMotorSPI();

motorWriteByte = motorWriteBuffer[4];
ltMotorSPI();

output_high(PIN_B3);                   //disable the motorPIC  SPI
}


/////////////////////////////////////////////////////////////////
//Interupt handler using timer0 (rtcc is the same as timer0)
/////////////////////////////////////////////////////////////////
#int_rtcc
timer0_handler()
{
set_timer0(cycleTime + get_timer0());     //interrupt timer sets the frequency

restart_wdt();                            //restarts code if program hangs

ltWriteTrapCommands();           //sends configuration byte and syncs trapPICs
delay_us(5);
ltReadTrapAData();
delay_us(5);
ltReadTrapBData();

ltProcessCommands();

ltMotorCommunication();

cycleCount++;
if (cycleCount == 0)
   cycleCountHiByte++;
}


////////////////////////////////////////////////////////
void main()
{
ltInitVariables();

ltSetupPic();

ltSetupPorts();

output_low(PIN_C0);       //trapPICs reset pin
delay_ms(500);          //give other PIC's time to boot, why is this necessary, debug
output_high(PIN_C0);       //trapPICs reset pin
delay_ms(200);

enable_interrupts(int_rtcc);        //interrupt driven program
enable_interrupts(GLOBAL);

while(1)
   {
      while (commandBufferGood){}   //wait for command write to other PICs
      ltReadHostPacket();

      if (timeOut != 0 && writeData)
      {
         ltWriteHostPacket();
         errorNumber = 0;                 //clear error
      }
   }
}
