#include "trappic.h"

////////////////////////////////////////////////////////////////////////////////
// trappic.c                                                                  //
// laser tweezers trap-feedback-PIC(trapPIC) firmware version 0.9.1alpha      //
//                                                                            //
// description:   this code is used on both trapPIC processors, trapA & trapB //
//                the trap that is directly connected to the PIC is simply    //
//                called 'trap', the trap connected to the other PIC is       //
//                called 'siblingTrap'                                        //
//                receives commands and sends data to host, routed by comPIC  //
//                reads light sensors from AD                                 //
//                shares data with sibling trapPIC                            //
//                calculates feedback and moves trap by writing to DA         //
//                                                                            //
// Authors:       Steve Smith, Shane Saxon, Zach Radding                      //
////////////////////////////////////////////////////////////////////////////////


enum{                         //defines the operation mode, used by a[mode]
   kManual = 0,               //no feedback in manual mode, allows host control
   kConstantForce,               //feedback modes may overide host vector commands
   kConstantPosition,
   kSiblingForce,             //uses sibling trap measurements for target
   kSiblingPosition          //possibly no use for this mode
};

enum{
   kHostPacketID = 42,        //host packet header identifier
   kPicPacketID = 213         //inter-PIC communication packet header ID
};



//we use the following enumerators to index all of our variables, this allows us
//to set any variable from the host and read any variable using the variableID,
//a[] array stores unsigned bytes, the b[] array stores unsigned 16bit words,
//each has its own index starting from 1, index 0 is for cycling through array
//this method is simpler than using pointers and makes it easy to add commands

//----------------index for 8bit(byte) variables in the a[array]----------------
enum{
   byteZero = 0,              //reserved for cycling through array mode

   mode,                      //operation mode of the trap
   feedbackAxisEnabled,       //axis or axes where feedback operates

   daActiveChannels,          //output DA channels to use, bitwise
   adActiveChannels,          //input AD channels to scan, bitwise
                              //note, AD can only scan sequential channels
                              //from ch0-N or ch4-N, N can be 0-7, see datasheet

   trapInvertAxes,            //Inverts axes of the DA output channels, bitwise
   trapSwapXY,                //swaps X and Y of the DA output

   adInvertAxes,              //Inverts axes of the AD input channels, bitwise
   psdSwapXY,                 //swaps X and Y of the PSD force
   leverSwapXY,               //swaps X and Y of the Lever position

   constantForceGain,            //used by feedback from local PSD
   constantPositionGain,
   constantForceRateDivider,     //feedback takes n+2 cycles
   constantPositionRateDivider,   //for reducing the frequency of the feedback rate

   siblingForceGain,          //used by feedback from sibling PSD
   siblingPositionGain,
   siblingForceRateDivider,   //feedback takes n+2 cycles
   siblingPositionRateDivider, //for reducing the frequency of the feedback rate

   optionVariableIDA,         //the a[] optional byte to transmit to the host
   optionVariableIDB,         //the b[] optional word to transmit to the host
                              //setting the optionVaribleID to 0 causes cycle
                              //through entire array
   cycleRate,                 //determines rate at which to cycle through data
                              //ie, a value of 4 would increment every 4 cycles

   restartPIC,                //if true the PIC will restart the program

   cycleCount,                //counts 0-255 and is transmitted to comPIC

   ARRAYSIZEA                 //size of the byte array a[ARRAYSIZEA]
};


//----------------------index of variables in the b[array]----------------------
//----------------all values are 2 byte words using AD/DA units-----------------
enum{
   piezoVectorX = 1,          //for recieving vector moves from the host and
   piezoVectorY,              //used by the feedback algorithms
   dummyVectorZ,
   laserVector,

   piezoX,                    //values used by the DA to set piezo voltage
   piezoY,
   dummyZ,                    //not currently in use, available for future use
   laserPercent,              //sets the percentage of laser current supplied,
                              //the laser power supply determines how many amps
                              //represent 100%

   piezoLimitLoX,             //limits the range of the DA outputs
   piezoLimitHiX,
   piezoLimitLoY,
   piezoLimitHiY,
   dummyLimitLoZ,
   dummyLimitHiZ,
   laserLimitLo,
   laserLimitHi,

   adPsdX,                    //the X force, raw PSD value read from AD
   adPsdY,                    //the Y force, raw PSD value read from AD
   adIris,                    //the Z detector, raw PSD value read from AD     debug

   adPsdOffsetX,              //callibration offsets added to the raw value
   adPsdOffsetY,
   adIrisOffset,

   psdX,                      //AD value with callibration offset added
   psdY,                      //AD value with callibration offset added
   psdSum,                    //the sum of the X and Y force, no offset used
   iris,                      //AD value with callibration offset added

   adLeverX,                  //the X position, raw PSD value read from AD
   adLeverY,                  //the Y position, raw PSD value read from AD
   adLeverOffsetX,            //callibration offsets added to the raw value
   adLeverOffsetY,

   leverX,                    //AD value with callibration offset added
   leverY,                    //AD value with callibration offset added
   leverSum,                  //the sum of the X and Y position, no offset used
   temperature,               //the temperature, for environmental control

   psdTargetX,                //feedback target values
   psdTargetY,
   leverTargetX,
   leverTargetY,

   siblingPsdX,               //for matching sibling force
   siblingPsdY,
   siblingPsdSum,
   siblingIris,

   siblingLeverX,             //for matching sibling position
   siblingLeverY,
   siblingLeverSum,
   siblingTemperature,

   ARRAYSIZEB
};


int8 *ramPointer;             //first block of RAM is initialized, except for
int8 firstAddress;            //piezo DA position, allows non-destructive reset

int8 errorNumber, errorTemp;        //communication error handling
int8 commandBufferGood;
int8 siblingDataGood;
int8 receiveBuffer[16];             //holds communication data until processed
int8 transmitBuffer[5];             //used for outgoing data selected by the
                                    //optionVariableID's

int8 a[ARRAYSIZEA];                 //unsigned 8bit variables, indexed
int16 b[ARRAYSIZEB];                //unsigned 16bit variables, indexed

int8 optionByteCycle, optionWordCycle;

int8 tempByte;
int16 temp;

   //add variables below this line
int8 skipCount;


////////////////////////////////////////////////////////
//clears all GPR's
void ltClearRam(void)
{
for (ramPointer = &firstAddress; ramPointer<0xFF; ramPointer++)
	{    //we skip the piezo position variables to not move the trap during reset
      if(*b[piezoX] > ramPointer || *b[laserPercent] < ramPointer)
   		*ramPointer = 0;
	}
}

////////////////////////////////////////////////////////
void ltInitVariables(void)
{
ltClearRam();

a[mode] = kManual;
a[feedbackAxisEnabled] = 3;            // both x and y axis enabled
a[optionVariableIDA] = 0;              //0 for auto-cycling through array
a[optionVariableIDB] = 0;
optionByteCycle = 0;
optionWordCycle = 0;

a[restartPIC] = 0;                     //0 is for don't reset the PIC until told

skipCount = 0;

a[constantForceRateDivider] = 0;          // 0 = fastest
a[constantPositionRateDivider] = 3;
a[constantForceGain] = 32;                //adjust for critical damping
a[constantPositionGain] = 32;

a[siblingForceRateDivider] = 3;          //feedback completes every n+1 cycles
a[siblingPositionRateDivider] = 3;
a[siblingForceGain] = 16;                //gains used by feedback
a[siblingPositionGain] = 16;

b[psdTargetX]= 32768;                  //center force feedback targets
b[psdTargetY] = 32768;
b[leverTargetX]= 32768;                //center position feedback targets
b[leverTargetY] = 32768;

b[adPsdOffsetX] = 32768;
b[adPsdOffsetY] = 32768;
b[adIrisOffset] = 32768;
b[adLeverOffsetX] = 32768;
b[adLeverOffsetY] = 32768;

b[piezoLimitHiX] = 65535;
b[piezoLimitLoX] = 0;
b[piezoLimitHiY] = 65535;
b[piezoLimitLoY] = 0;

a[adActiveChannels] = 0b11111111;      //scan all 8 AD channels
a[daActiveChannels] = 0b00000011;      //write only to the X and Y piezo

b[piezoVectorX] = 32768;                //32768 is center of vector range
b[piezoVectorY] = 32768;                //represents zero change
b[dummyVectorZ] = 32768;
b[laserVector] = 32768;

b[piezoX] = 32768;
b[piezoY] = 32768;
b[dummyZ] = 32768;
b[laserPercent] = 32768;

if (input(PIN_G2))                  //true for trapA else this is the trapB PIC
   {
      a[trapSwapXY] = 1;               //if true swap X and Y of the piezo axes
      a[trapInvertAxes] = 0b00000000;    //flips axes of DA outputs
      a[adInvertAxes] = 0b00000010;   //add power detect for non-destructive reset, debug
   }
else                                   //trapB specific variables
   {
      a[trapSwapXY] = 0;
      a[trapInvertAxes] = 0b00000001;
      a[adInvertAxes] = 0b00010011;
   }
a[psdSwapXY] = 0;
}


////////////////////////////////////////////////////////
void ltSetupPic(void)
{
setup_adc_ports(NO_ANALOGS);
setup_adc(ADC_OFF);
setup_psp(PSP_DISABLED);
setup_wdt(WDT_ON);                          //watchdog used in case of lock-up
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
setup_timer_1(T1_DISABLED);
setup_timer_2(T2_DISABLED,1,1);
setup_timer_3(T3_DISABLED);
setup_timer_4(T4_DISABLED,0,1);
setup_comparator(NC_NC_NC_NC);
setup_vref(FALSE);
}

////////////////////////////////////////////////////////
//SPI speed of the DAC8534 is up to 10Mhz we use 9.5Mhz, PIC clock divided by 4
void ltHighSpeedSPI(void)
{
#asm
   MOVLW  0x20             //sets SPI speed to PIC clock divided by 4 , 9.5Mhz
   MOVWF  SSPCON1          //SSPCON1 register

   MOVLW  0xC0             //samples at end of cycle, should we change this to 0x00, debug
   MOVWF  SSPSTAT          //SSPSTAT register
#endasm
}

////////////////////////////////////////////////////////
//MAX1168 SPI is limited to 4.8Mhz we use 2.4Mhz, PIC doesn't do 4.8Mhz
void ltLowSpeedSPI(void)
{
#asm
   MOVLW  0x21             //sets SPI speed to 2.4Mhz
   MOVWF  SSPCON1          //SSPCON1 register

   MOVLW  0xC0             //samples at end of cycle, was changed from 0xC0, seems to work, debug
   MOVWF  SSPSTAT          //SSPSTAT register
#endasm
}

////////////////////////////////////////////////////////
//the code for setup_spi does not work perfectly, so we are using our own,
//setup using asm instructions so that it matches PIC18F6520 datasheet
void ltSpiCustomSetup(void)
{
set_tris_c(0xD7);             //sets the SPI port tri-state
                              //also sets CS and EOC pins used for the AD and DA

ltLowSpeedSPI();              //half speed is 2.4Mhz
}


////////////////////////////////////////////////////////
void ltSetupPorts(void)
{
set_tris_a(0xFF);          //unused port, available for future use
set_tris_b(0xF2);          //CS, EOS and LDAC lines for DA and AD

output_high(PIN_B0);       //disable CS for AD, note pin D5 is conversion done
output_high(PIN_B2);       //disable CS for DA
output_low(PIN_B3);        //enable LDAC for the DA

ltSpiCustomSetup();        //setup port C for SPI communication with AD & DA

set_tris_d(0xFF);          //unused port, available for future use
set_tris_f(0xFF);                //PIC to PIC bi-directional 8bit data bus
set_tris_g(0xFF);                //pin G2 is for trapA vs trapB identification

if (input(PIN_G2))
   {
      set_tris_e(0xCF);          //PIC to PIC bus control lines
      output_high(PIN_E4);
      output_high(PIN_E5);        //comPIC will wait to send mode byte
   }
else
   {
      set_tris_e(0x3F);          //PIC bus control lines, signaling, not data
      output_high(PIN_E6);
      output_high(PIN_E7);        //comPIC will wait to send mode byte
   }
}


////////////////////////////////////////////////////////
//send the start conversion signal to the AD (MAX1168)
//leave the cable select (CS) pin low(active), it is set high by ltAdRead()
void ltAdConvert(void)
{
#use fast_io(B)

output_low(PIN_B0);                       //enables CS on the AD

spi_write(0xEF);                       //start conversion for all 8 channels
}


////////////////////////////////////////////////////////
//process the data from receiveSettings if variableIDs are in valid range
void ltProcessCommands(void)
{
if (commandBufferGood == 0)
   {
      if(errorNumber == 0)
         errorNumber = 95;                   //error 95, bad command buffer
      return;                                //exit if command buffer isn't good
   }

commandBufferGood = 0;                       //reset for next receive commands

if (input(PIN_G2))
   {
      if ( receiveBuffer[0] < ARRAYSIZEA )         //the byte command
         a[receiveBuffer[0]] = receiveBuffer[1];
      else
         if (errorNumber == 0)
            errorNumber = 9;


      if ( receiveBuffer[2] < ARRAYSIZEB )         //the word commands
         {
            *(&b[receiveBuffer[2]]+1) = receiveBuffer[3];
            *(&b[receiveBuffer[2]]) = receiveBuffer[4];
         }
      else
         if (errorNumber == 0)
            errorNumber = 9;

      if ( receiveBuffer[5] < ARRAYSIZEB )
         {
            *(&b[receiveBuffer[5]]+1) = receiveBuffer[6];
            *(&b[receiveBuffer[5]]) = receiveBuffer[7];
         }
      else
         if (errorNumber == 0)
            errorNumber = 9;
   }
else
   {
      if ( receiveBuffer[8] < ARRAYSIZEA )         //the byte command
         a[receiveBuffer[8]] = receiveBuffer[9];
      else
         if (errorNumber == 0)
            errorNumber = 9;


      if ( receiveBuffer[10] < ARRAYSIZEB )         //the word commands
         {
            *(&b[receiveBuffer[10]]+1) = receiveBuffer[11];
            *(&b[receiveBuffer[10]]) = receiveBuffer[12];
         }
      else
         if (errorNumber == 0)
            errorNumber = 9;

      if ( receiveBuffer[13] < ARRAYSIZEB )
         {
            *(&b[receiveBuffer[13]]+1) = receiveBuffer[14];
            *(&b[receiveBuffer[13]]) = receiveBuffer[15];
         }
      else
         if (errorNumber == 0)
            errorNumber = 9;
   }


if (a[restartPIC])         //restart the program when commanded by the host,
   while(1);               //this is done by making the watch-dog-timer timeout
                           //a bit crude, but effective!


   //copy option ID's with values to transmitBuffer
if (a[optionVariableIDA])
   {
      transmitBuffer[0] = a[optionVariableIDA];       //the optionByte ID
      transmitBuffer[1] = a[a[optionVariableIDA]];    //the optionByte value
   }
else
   {
      if (++optionByteCycle == ARRAYSIZEA) //roll over to beginning if at end of array
         optionByteCycle = 0;

      transmitBuffer[0] = optionByteCycle;            //the optionByte ID
      transmitBuffer[1] = a[optionByteCycle];         //the optionByte value
   }

if (a[optionVariableIDB])
   {
      transmitBuffer[2] = a[optionVariableIDB];
      transmitBuffer[3] = *(&b[a[optionVariableIDB]]);
      transmitBuffer[4] = *(&b[a[optionVariableIDB]]+1);
   }
else
   {
      if (++optionWordCycle == ARRAYSIZEB) //roll over to beginning if at end of array
         optionWordCycle = 0;

      transmitBuffer[2] = optionWordCycle;
      transmitBuffer[3] = *(&b[optionWordCycle]);
      transmitBuffer[4] = *(&b[optionWordCycle]+1);
   }
}


////////////////////////////////////////////////////////
//sync with comPIC and receive host commands
void ltReceiveCommands(void)
{
#use fast_io(E)
#use fast_io(F)

errorTemp = 0;          //used for temp error, note also switches memory block!!


if (input(PIN_G2))         //tells comPIC that trapPIC is ready to receive
   output_low(PIN_E5);     //trap A
else
   output_low(PIN_E7);     //trap B

while (input(PIN_E0)){}    //wait for synchronization from comPIC


#asm
movlw kPicPacketID         //put 1st byte in w register, this is the PacketID

check1:
btfss 0xf84,3              //if C2 lo, trapPIC unavailable, wait until its ready
goto check1                //C2 high, ready, will exit loop within 3 cycles

btfss 0xf84,3              //uses this to catch up if a cycle behind
goto check2
nop
nop

check2:
btfss 0xf84,3              //catch up one more cycle if still behind
goto check3
nop
nop

check3:
cpfseq 0xF85               //if 1st byte is not the right ID we exit
goto check5                //copy 1st byte from w to receiveByte
nop

movff 0xF85,receiveBuffer[0]    //transfer next byte to receiveBuffer array
addwf receiveBuffer[0],0        //calculates checksum
nop

movff 0xF85,receiveBuffer[1]
addwf receiveBuffer[1],0
nop

movff 0xF85,receiveBuffer[2]
addwf receiveBuffer[2],0
nop

movff 0xF85,receiveBuffer[3]
addwf receiveBuffer[3],0
nop

movff 0xF85,receiveBuffer[4]
addwf receiveBuffer[4],0
nop

movff 0xF85,receiveBuffer[5]
addwf receiveBuffer[5],0
nop

movff 0xF85,receiveBuffer[6]
addwf receiveBuffer[6],0
nop

movff 0xF85,receiveBuffer[7]
addwf receiveBuffer[7],0
nop

movff 0xF85,receiveBuffer[8]
addwf receiveBuffer[8],0
nop

movff 0xF85,receiveBuffer[9]
addwf receiveBuffer[9],0
nop

movff 0xF85,receiveBuffer[10]
addwf receiveBuffer[10],0
nop

movff 0xF85,receiveBuffer[11]
addwf receiveBuffer[11],0
nop

movff 0xF85,receiveBuffer[12]
addwf receiveBuffer[12],0
nop

movff 0xF85,receiveBuffer[13]
addwf receiveBuffer[13],0
nop

movff 0xF85,receiveBuffer[14]
addwf receiveBuffer[14],0
nop

movff 0xF85,receiveBuffer[15]
addwf receiveBuffer[15],0
nop

check4:
cpfseq 0xF85
goto check6                      //if w not same as checksum generate error
goto check7                      //were good, exit

check5:
movlw 3                          //error3, not a receive mode packet
movwf errorTemp
goto check7

check6:
movlw 4                          //error4, bad checksum from receive mode packet
movwf errorTemp
goto check7

check7:
#endasm


if (input(PIN_G2))
   output_high(PIN_E5);           //trapB sync pin, low during idle period
else
   output_high(PIN_E7);           //trapA sync pin


if (errorTemp)
   {
      if (errorNumber == 0)
         errorNumber = errorTemp;
   }
else
   commandBufferGood = 1;
}

////////////////////////////////////////////////////////
void ltReceiveSiblingTrapData(void)
{
#use fast_io(E)
#use fast_io(F)

errorTemp = 0;          //used for temp error, note also switches memory block!!
siblingDataGood = 0;       //clear until packet verified by checksum

#asm
movlw kPicPacketID         //put 1st byte in w register, this is the PacketID

check1:
btfss 0xf84,3              //if C2 lo, trapPIC unavailable, wait until its ready
goto check1                //C2 high, ready, will exit loop within 3 cycles

btfss 0xf84,3              //uses this to catch up if a cycle behind
goto check2
nop
nop

check2:
btfss 0xf84,3              //catch up one more cycle if still behind
goto check3
nop
nop

check3:
cpfseq 0xF85               //if 1st byte is not the right ID we exit
goto check5                //copy 1st byte from w to receiveByte
nop                                                      //not needed for TrapA, debug

   movff 0xF85,&b[siblingPsdX]                                  //PSD data
	addwf &b[siblingPsdX],0
   nop

	movff 0xF85,&b[siblingPsdX]+1
	addwf &b[siblingPsdX]+1,0
   nop

	movff 0xF85,&b[siblingPsdY]
	addwf &b[siblingPsdY],0
   nop

   movff 0xF85,&b[siblingPsdY]+1
	addwf &b[siblingPsdY]+1,0
   nop

   movff 0xF85,&b[siblingPsdSum]
	addwf &b[siblingPsdSum],0
   nop

   movff 0xF85,&b[siblingPsdSum]+1
	addwf &b[siblingPsdSum]+1,0
   nop

   movff 0xF85,&b[siblingIris]
	addwf &b[siblingIris],0
   nop

   movff 0xF85,&b[siblingIris]+1
	addwf &b[siblingIris]+1,0
   nop



	movff 0xF85,&b[siblingLeverX]                             //lever data
	addwf &b[siblingLeverX],0
   nop

	movff 0xF85,&b[siblingLeverX]+1
	addwf &b[siblingLeverX]+1,0
   nop

   movff 0xF85,&b[siblingLeverY]
	addwf &b[siblingLeverY],0
   nop

	movff 0xF85,&b[siblingLeverY]+1
	addwf &b[siblingLeverY]+1,0
   nop

   movff 0xF85,&b[siblingLeverSum]
	addwf &b[siblingLeverSum],0
   nop

   movff 0xF85,&b[siblingLeverSum]+1
	addwf &b[siblingLeverSum]+1,0
   nop



   movff 0xF85,tempByte         //pad out the transmitBuffer, err and cycleCount
	addwf tempByte,0
   nop

   movff 0xF85,tempByte
	addwf tempByte,0
   nop

   movff 0xF85,tempByte
	addwf tempByte,0
   nop

   movff 0xF85,tempByte
	addwf tempByte,0
   nop

   movff 0xF85,tempByte
	addwf tempByte,0
   nop


   movff 0xF85,tempByte
	addwf tempByte,0
   nop

   movff 0xF85,tempByte
	addwf tempByte,0
   nop


check4:
cpfseq 0xF85
goto check6                      //if w not same as checksum generate error
goto check7                      //were good, exit

check5:
movlw 5                          //error5, not a receive mode packet
movwf errorTemp
goto check7

check6:
movlw 6                          //error6, bad checksum from sibling packet
movwf errorTemp
goto check7

check7:
#endasm

if (errorTemp)
   {
      if (errorNumber == 0)
         errorNumber = errorTemp;
   }
else
   siblingDataGood = 1;

}

////////////////////////////////////////////////////////
void ltTransmitTrapData(void)
{
#use fast_io(E)
#use fast_io(F)

errorTemp = 0;
errorNumber = a[cycleCount]; //debug
set_tris_f(0x00);       //set 8bit parrallel port to write to comPIC

#asm
movlw kPicPacketID      //put 1st byte in w register, this is the PacketID
movwf 0xF85

check1:
btfss 0xf84,3           //if lo, comPIC unavailable, wait until its ready
goto check1             //high, ready, will exit loop within 3 cycles

btfss 0xf84,3
goto check2
nop
nop

check2:
btfss 0xf84,3
goto check3
nop
nop

check3:
   movff &b[psdX],0xF85                                  //PSD data
	addwf &b[psdX],0
   nop

	movff &b[psdX]+1,0xF85
	addwf &b[psdX]+1,0
   nop

	movff &b[psdY],0xF85
	addwf &b[psdY],0
   nop

   movff &b[psdY]+1,0xF85
	addwf &b[psdY]+1,0
   nop

   movff &b[psdSum],0xF85
	addwf &b[psdSum],0
   nop

   movff &b[psdSum]+1,0xF85
	addwf &b[psdSum]+1,0
   nop

   movff &b[Iris],0xF85
	addwf &b[Iris],0
   nop

   movff &b[Iris]+1,0xF85
	addwf &b[Iris]+1,0
   nop



	movff &b[leverX],0xF85                             //lever data
	addwf &b[leverX],0
   nop

	movff &b[leverX]+1,0xF85
	addwf &b[leverX]+1,0
   nop

   movff &b[leverY],0xF85
	addwf &b[leverY],0
   nop

	movff &b[leverY]+1,0xF85
	addwf &b[leverY]+1,0
   nop

   movff &b[leverSum],0xF85
	addwf &b[leverSum],0
   nop

   movff &b[leverSum]+1,0xF85
	addwf &b[leverSum]+1,0
   nop



   movff transmitBuffer[0],0xF85
	addwf transmitBuffer[0],0
   nop

   movff transmitBuffer[1],0xF85
	addwf transmitBuffer[1],0
   nop

   movff transmitBuffer[2],0xF85
	addwf transmitBuffer[2],0
   nop

   movff transmitBuffer[3],0xF85
	addwf transmitBuffer[3],0
   nop

   movff transmitBuffer[4],0xF85
	addwf transmitBuffer[4],0
   nop


   movff errorNumber,0xF85
	addwf errorNumber,0
   nop

   movff a[cycleCount],0xF85
	addwf a[cycleCount],0
   nop

check4:
	movwf 0xF85             //send checksum
   nop
   nop

   goto check6

check5:
   movlw 8                    //error8 comPIC unavailable while receiveSettings
   movwf errorTemp

check6:
#endasm

set_tris_f(0xFF);

if (errorTemp)
   {
      if (errorNumber == 0)
         errorNumber = 8;     //error8, comPIC unavailable while receiveSettings
   }
else
   errorNumber = 0;           //error has been sent to comPIC reset to zero
}

////////////////////////////////////////////////////////
//transmit data to comPIC and sibling trapPIC, also receive data from sibling,
//one PIC transmits while the other two listen
void ltExchangeTrapData(void)
{
#use fast_io(E)
#use fast_io(G)

temp = 255;

while (input(PIN_E1))                        //wait for turn to transmit
   {
      if (temp -- == 0)
         {
            if (!errorNumber)
               errorNumber = 90;             //error 90, trapA out of sync on TX
            return;                          //exit if timeout
         }
   }

if (input(PIN_G2))                           //trapA transmits first
   {
      output_low(PIN_E4);                    //TX bus control line, ready
      ltTransmitTrapData();       //transmit data to comPIC and sibling trapPIC
      output_high(PIN_E4);                   //done transmitting

      while (input(PIN_E2))                  //wait for sibling's turn to TX
         {
            if (temp-- == 0)
               return;                       //exit if timeout
         }

      output_low(PIN_E5);                    //RX bus control line, ready
      ltReceiveSiblingTrapData();            //receive data from sibling trapPIC
      output_high(PIN_E5);                   //done recieving
   }
else                                         //trapB is reverse order of trapA
   {
      output_low(PIN_E7);
      ltReceiveSiblingTrapData();
      output_high(PIN_E7);

      while (input(PIN_E2))                  //wait for turn to transmit
         {
            if (temp -- == 0)
               return;                       //exit if timeout
         }

      output_low(PIN_E6);
      ltTransmitTrapData();
      output_high(PIN_E6);
   }
}


////////////////////////////////////////////////////////
//callibrates AD using offsets, similar algorithm to ltProcessTrapVectors
void ltAdOffsets(void)
{
temp = b[adPsdX];

if (bit_test(b[adPsdOffsetX],15))    //the high bit is true for positive vectors
   {                                 //ie, true if value is 32768 or above
      b[psdX] = b[adPsdX] + (b[adPsdOffsetX] - 32768);
      if (b[psdX] < temp )
         b[psdX] = 65535;          //if exceeds limit set to limit
   }
else
   {
      b[psdX] = b[adPsdX] - ( 32768 - b[adPsdOffsetX]);
      if (b[psdX] > temp)
         b[psdX] = 0;
   }

temp = b[adPsdY];

if (bit_test(b[adPsdOffsetY],15))
   {
      b[psdY] = b[adPsdY] + (b[adPsdOffsetY] - 32768);
      if (b[psdY] < temp )
         b[psdY] = 65535;
   }
else
   {
      b[psdY] = b[adPsdY] - ( 32768 - b[adPsdOffsetY]);
      if (b[psdY] > temp)
         b[psdY] = 0;
   }

temp = b[adIris];

if (bit_test(b[adIrisOffset],15))
   {
      b[iris] = b[adIris] + (b[adIrisOffset] - 32768);
      if (b[iris] < temp )
         b[iris] = 65535;
   }
else
   {
      b[iris] = b[adIris] - ( 32768 - b[adIrisOffset]);
      if (b[iris] > temp)
         b[iris] = 0;
   }

temp = b[adLeverX];

if (bit_test(b[adLeverOffsetX],15))
   {
      b[leverX] = b[adLeverX] + (b[adLeverOffsetX] - 32768);
      if (b[leverX] < temp)
         b[leverX] = 65535;
   }
else
   {
      b[leverX] = b[adLeverX] - ( 32768 - b[adLeverOffsetX]);
      if (b[leverX] > temp)
         b[leverX] = 0;
   }

temp = b[adLeverY];

if (bit_test(b[adLeverOffsetY],15))
   {
      b[leverY] = b[adLeverY] + (b[adLeverOffsetY] - 32768);
      if (b[leverY] < temp)
         b[leverY] = 65535;
   }
else
   {
      b[leverY] = b[adLeverY] - ( 32768 - b[adLeverOffsetY]);
      if (b[leverY] > temp)
         b[leverY] = 0;
   }
}


////////////////////////////////////////////////////////
//process piezo vectors with limits
void ltProcessTrapVectors(void)
{
temp = b[piezoX];

if (bit_test(b[piezoVectorX],15))    //the high bit is true for positive vectors
   {                                 //and false for negative vectors
      b[piezoX] += b[piezoVectorX] - 32768;
      if (b[piezoX] < temp || b[piezoX] > b[piezoLimitHiX])
         b[piezoX] = b[piezoLimitHiX];           //if exceeds limit set to limit
   }
else
   {
      b[piezoX] -= 32768 - b[piezoVectorX];
      if (b[piezoX] > temp || b[piezoX] < b[piezoLimitLoX])
         b[piezoX] = b[piezoLimitLoX];
   }
b[piezoVectorX] = 32768;


temp = b[piezoY];

if (bit_test(b[piezoVectorY],15))    //the high bit is true for positive vectors
   {                                 //and false for negative vectors
      b[piezoY] += b[piezoVectorY] - 32768;
      if (b[piezoY] < temp || b[piezoY] > b[piezoLimitHiY])
         b[piezoY] = b[piezoLimitHiY];           //if exceeds limit set to limit
   }
else
   {
      b[piezoY] -= 32768 - b[piezoVectorY];
      if (b[piezoY] > temp || b[piezoY] < b[piezoLimitLoY])
         b[piezoY] = b[piezoLimitLoY];
   }
b[piezoVectorY] = 32768;


temp = b[dummyZ];

if (bit_test(b[dummyVectorZ],15))    //the high bit is true for positive vectors
   {                                 //and false for negative vectors
      b[dummyZ] += b[dummyVectorZ] - 32768;
      if (b[dummyZ] < temp || b[dummyZ] > b[dummyLimitHiZ])
         b[dummyZ] = b[dummyLimitHiZ];           //if exceeds limit set to limit
   }
else
   {
      b[dummyZ] -= 32768 - b[dummyVectorZ];
      if (b[dummyZ] > temp || b[dummyZ] < b[dummyLimitLoZ])
         b[dummyZ] = b[dummyLimitLoZ];
   }
b[dummyVectorZ] = 32768;


temp = b[laserPercent];

if (bit_test(b[piezoVectorX],15))    //the high bit is true for positive vectors
   {                                 //and false for negative vectors
      b[laserPercent] += b[laserVector] - 32768;
      if (b[laserPercent] < temp || b[laserPercent] > b[laserLimitHi])
         b[laserPercent] = b[laserLimitHi];           //if exceeds limit set to limit
   }
else
   {
      b[laserPercent] -= 32768 - b[laserVector];
      if (b[laserPercent] > temp || b[laserPercent] < b[laserLimitLo])
         b[laserPercent] = b[laserLimitLo];
   }
b[laserVector] = 32768;
}


////////////////////////////////////////////////////////
//called before and after a piezo write to the DA
void ltDaChangeAxes(void)
{
if (bit_test(a[trapInvertAxes],0))
   b[piezoX] = 65535 - b[piezoX];

if (bit_test(a[trapInvertAxes],1))
   b[piezoY] = 65535 - b[piezoY];

if (bit_test(a[trapInvertAxes],2))
   b[dummyZ] = 65535 - b[dummyZ];

if (bit_test(a[trapInvertAxes],3))
   b[laserPercent] = 65535 - b[laserPercent];


if (a[trapSwapXY])
   {
      temp = b[piezoX];
      b[piezoX] = b[piezoY];
      b[piezoY] = temp;
   }
}

////////////////////////////////////////////////////////
//writes to the DA as set by the a[daActiveChannels]
//switches to high speed SPI(9.5Mhz with 38Mhz PIC clock), flips the axes by
//calling ltDaFlipAxes then writes to the piezo DA, then flips the axes back and
//switches to half speed SPI(4.75Mhz)
void ltDaWrite(void)
{
#use fast_io(B)

if (a[daActiveChannels] == 0)             //exit if not writing to DA's
   return;

ltProcessTrapVectors();      //feedback takes priority over user, reverse this, debug

ltDaChangeAxes();                   //flip piezo axes back for feedback routines

ltHighSpeedSPI();                         //SPI clock rate of 9.5Mhz

if (bit_test(a[daActiveChannels],0))
{
   output_low(PIN_B2);                    //sync signal
   spi_write(0x10);                       //a[command] byte, selects channel
   spi_write(make8(b[piezoX],1));         //load piezo value to the DA
   spi_write(make8(b[piezoX],0));
   output_high(PIN_B2);
}

if (bit_test(a[daActiveChannels],1))
{
   output_low(PIN_B2);
   spi_write(0x12);
   spi_write(make8(b[piezoY],1));
   spi_write(make8(b[piezoY],0));
   output_high(PIN_B2);
}

if (bit_test(a[daActiveChannels],2))
{
   output_low(PIN_B2);
   spi_write(0x14);
   spi_write(make8(b[dummyZ],1));
   spi_write(make8(b[dummyZ],0));
   output_high(PIN_B2);
}

if (bit_test(a[daActiveChannels],3))
{
   output_low(PIN_B2);
   spi_write(0x16);
   spi_write(make8(b[laserPercent],1));
   spi_write(make8(b[laserPercent],0));
   output_high(PIN_B2);
}

ltLowSpeedSPI();                         //SPI clock rate of 2.4Mhz

ltDaChangeAxes();                   //flip piezo axes back for feedback routines
}


////////////////////////////////////////////////////////
//called after reading from the AD
void ltAdChangeAxes(void)
{
if (bit_test(a[adInvertAxes],0))
   b[adPsdX] = 65535 - b[adPsdX];

if (bit_test(a[adInvertAxes],1))
   b[adPsdY] = 65535 - b[adPsdY];

if (bit_test(a[adInvertAxes],2))
   b[psdSum] = 65535 - b[psdSum];

if (bit_test(a[adInvertAxes],3))
   b[adIris] = 65535 - b[adIris];

if (bit_test(a[adInvertAxes],4))
   b[adLeverX] = 65535 - b[adLeverX];

if (bit_test(a[adInvertAxes],5))
   b[adLeverY] = 65535 - b[adLeverY];

if (bit_test(a[adInvertAxes],6))
   b[leverSum] = 65535 - b[leverSum];

if (bit_test(a[adInvertAxes],7))
   b[temperature] = 65535 - b[temperature];

if (bit_test(a[psdSwapXY],0))
   {
      temp = b[adPsdX];
      b[adPsdX] = b[adPsdY];
      b[adPsdY] = temp;
   }

if (bit_test(a[leverSwapXY],2))
   {
      temp = b[leverX];
      b[leverX] = b[leverY];
      b[leverY] = temp;
   }
}

////////////////////////////////////////////////////////
//read the AD (MAX1168), flip the axes and add offsets
void ltAdRead(void)
{
#use fast_io(B)

int16 timeOutCount = 0;

while (input(PIN_B1))         //wait for the end of conversion, break on timeout
{
   timeOutCount++;
   if (timeOutCount > 500)    //exit if takes too long
   {
      if (!errorNumber)
         errorNumber = 33;    //error 33, AD timeout on waiting for EOC
      return;
   }
}

*(&b[adPsdX]+1) = spi_read(0);            //read channels 0-3
*(&b[adPsdX])   = spi_read(0);
*(&b[adPsdY]+1) = spi_read(0);
*(&b[adPsdY])   = spi_read(0);
*(&b[psdSum]+1) = spi_read(0);
*(&b[psdSum])   = spi_read(0);
*(&b[adIris]+1) = spi_read(0);
*(&b[adIris])   = spi_read(0);

*(&b[adLeverX]+1) = spi_read(0);          //read channels 4-7
*(&b[adLeverX])   = spi_read(0);
*(&b[adLeverY]+1) = spi_read(0);
*(&b[adLeverY])   = spi_read(0);
*(&b[leverSum]+1) = spi_read(0);
*(&b[leverSum])   = spi_read(0);
*(&b[temperature]+1) = spi_read(0);
*(&b[temperature])  = spi_read(0);


output_high(PIN_B0);                      //disable AD by making CS high

if (input(PIN_G2))
   output_low(PIN_E5);
else
   output_low(PIN_E7);


ltAdChangeAxes();                         //swaps and inverts the axes as needed

ltAdOffsets();                            //calculate the offsets
}



////////////////////////////////////////////////////////
void ltConstantForce(void)   // makes constant force feedback
{
skipCount++;
if (skipCount > a[constantForceRateDivider])
   {
   if (((skipCount-a[constantForceRateDivider]) == 1) && (a[feedbackAxisEnabled] & 1))
      {
      if (b[psdTargetX] > b[psdX])
         b[piezoVectorX] = 32768 + ((b[psdTargetX] - b[psdX]) * a[constantForceGain])/64;
      else
         b[piezoVectorX] = 32768 - ((b[psdX] - b[psdTargetX]) * a[constantForceGain])/64;
      }
   if (((skipCount-a[constantForceRateDivider]) == 2) && (a[feedbackAxisEnabled] & 2))
      {
      if (b[psdTargetY] > b[psdY])
         b[piezoVectorY] = 32768 + ((b[psdTargetY] - b[psdY]) * a[constantForceGain])/64;
      else
         b[piezoVectorY] = 32768 - ((b[psdY] - b[psdTargetY]) * a[constantForceGain])/64;
      }
   if ((skipCount-a[constantForceRateDivider]) > 1)   skipCount = 0;
   }
}

////////////////////////////////////////////////////////
void ltConstantPosition(void)  // makes constant light-lever position
{
skipCount++;
if (skipCount > a[constantPositionRateDivider])
   {
   if ((skipCount-a[constantPositionRateDivider]) == 1)
      {
      if (b[leverTargetX] > b[leverX])
         b[piezoVectorX] = 32768 - ((b[leverTargetX] - b[leverX])* a[constantPositionGain])/64;
      else
         b[piezoVectorX] = 32768 + ((b[leverX] - b[leverTargetX])* a[constantPositionGain])/64;
      }
   else
      {
      if (b[leverTargetY] > b[leverY])
         b[piezoVectorY] = 32768 - ((b[leverTargetY] - b[leverY])* a[constantPositionGain])/64;
      else
         b[piezoVectorY] = 32768 + ((b[leverY] - b[leverTargetY])* a[constantPositionGain])/64;
      skipCount = 0;
      }
   }
}

////////////////////////////////////////////////////////
void ltSiblingForce(void) // also called "autoAlign" feedback
{
skipCount++;
if (skipCount > a[siblingForceRateDivider])
   {
   if ((skipCount-a[siblingForceRateDivider]) == 1)
      {
      if (b[siblingPsdX] > b[psdX])
         b[piezoVectorX] = 32768 + ((b[siblingPsdX] - b[psdX]) * a[siblingForceGain])/256;
      else
         b[piezoVectorX] = 32768 - ((b[psdX] - b[siblingPsdX]) * a[siblingForceGain])/256;
      }
   else
      {
      if (b[siblingPsdY] > b[psdY])
         b[piezoVectorY] = 32768 + ((b[siblingPsdY] - b[psdY]) * a[siblingForceGain])/256;
      else
         b[piezoVectorY] = 32768 - ((b[psdY] - b[siblingPsdY]) * a[siblingForceGain])/256;
      skipCount = 0;
      }
   }
}

////////////////////////////////////////////////////////
void ltSiblingPosition(void)
{}
// Probably cannot work at any reasonable speed.
// Want to move local lever position to match changes in sibling position?
// First remove offsets due to different lever-mirror positions.
// Then normalize changes by different leverSum values.
////////////////////////////////////////////////////////


void main()
{

ltInitVariables();                  //sets up variables specific to each trapPIC

ltSetupPic();                       //sets up the PIC, including watchdog timer

ltSetupPorts();                     //configures the IO ports

ltAdConvert();                      //send start conversion command to the AD

while(1)                            //start program loop
   {
      restart_wdt();                //PIC reboots if this is not called before
                                    //the watchdog timer runs out, see trappic.h

      ltReceiveCommands();          //gets commands and synchronizes with comPIC

      ltExchangeTrapData();         //send data to comPIC, get sibling trap data

      ltProcessCommands();          //process commands received from comPIC

      ltAdRead();                   //reads the photo sensors as measured by AD


      switch (a[mode])                    //mode determines type of feedback
      {
         case kConstantForce:
            ltConstantForce();         //feedback target is fixed PSD force
            break;
         case kConstantPosition:
            ltConstantPosition();      //target is fixed lever position
            break;
         case kSiblingForce:
            ltSiblingForce();          //target is sibling PSD force
            break;
         case kSiblingPosition:
            ltSiblingPosition();       // target is sibling lever position
            break;                     // but what is signal? Local position?
      }

      ltDaWrite();                           //moves trap by writing to DA

      ltAdConvert();                         //restart sample conversion for AD

      a[cycleCount]++;                       //for detecting lost cycles
   }
}
