#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 } }