Alvaro Cassinelli
/
skinGames_II
save loops
Diff: hardwareIO/hardwareIO.cpp
- Revision:
- 0:df6fdd9b99f0
diff -r 000000000000 -r df6fdd9b99f0 hardwareIO/hardwareIO.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hardwareIO/hardwareIO.cpp Tue Dec 02 04:39:15 2014 +0000 @@ -0,0 +1,447 @@ +#include "hardwareIO.h" + +HardwareIO IO; // preintantiation of cross-file global object IO + +// -------------------------------------- (0) SETUP ALL IO (call this in the setup() function in main program) + +Serial pc(USBTX, USBRX); // tx, rx +LocalFileSystem local("local"); // Create the local filesystem under the name "local" + + SPI spiDAC(MOSI_PIN, MISO_PIN, SCK_PIN); // mosi, miso, sclk + DigitalOut csDAC(CS_DAC_MIRRORS); + + DigitalOut Laser_Red(LASER_RED_PIN); // NOTE: this is NOT the lock in sensing laser (actually, not used yet) + DigitalOut Laser_Green(LASER_GREEN_PIN); + DigitalOut Laser_Blue(LASER_BLUE_PIN); + + // Some manual controls over the hardware function: + InterruptIn switchOne(SWITCH_ONE); + InterruptIn switchTwo(SWITCH_TWO); + AnalogIn ainPot(POT_ANALOG_INPUT); + //DigitalIn RotaryEncoderPinA(ROTARY_ENCODER_PINA); // not needed: this is done in the CRotaryEncoder library (as input or interrupt) + //DigitalIn RotaryEncoderPinB(ROTARY_ENCODER_PINB); + + DigitalIn twoStateSwitch(TWO_STATE_SWITCH); + +void HardwareIO::init(void) { + + // Set laser powers down: + setRedPower(0);// TTL red laser (not used - actually not present) + setGreenPower(0); + setBluePower(0); + + //Serial Communication setup: + pc.baud(115200);// + //pc.printf("Serial Connection established \r\n"); + // pc.baud(921600);//115200);// + + // Setup for lock-in amplifier and pwm references: + setLaserLockinPower(1);// actually this is the Red laser in the hardware + lockin.init(); + + // Setup for DAC control to move the mirrors: + // Set spi for 8 bit data, high steady state clock, + // second edge capture, with a 10MHz clock rate: + csDAC = 1; + spiDAC.format(16,0); + spiDAC.frequency(16000000); + + // default initial mirror position: + writeOutX(CENTER_AD_MIRROR_X); + writeOutY(CENTER_AD_MIRROR_Y); + + // Rotary encoder 1 (to set the FIXED THRESHOLD VALUE): + rotaryEncoder1.SetMinMax(0,255); + rotaryEncoder1.Set(0); // initial value + + // Rotary encoder 2 (to set the additional correction angle): + rotaryEncoder2.SetMinMax(-10,10); + rotaryEncoder2.Set(0); // initial value + + // Load LUT table: + setLUT(); + + // Set interrupts on RAISING edge for button-switch functions: + // Note: The pin input will be logic '0' for any voltage on the pin below 0.8v, and '1' for any voltage above 2.0v. + // By default, the InterruptIn is setup with an internal pull-down resistor, but for security and clarity I will do it explicitly here: + switchOne.mode(PullUp); // pull down seems not very good + switchTwo.mode(PullUp); + switchOne.fall(this, &HardwareIO::switchOneInterrupt); // attach the address of the flip function to the falling edge + switchTwo.fall(this, &HardwareIO::switchTwoInterrupt); // attach the address of the flip function to the falling edge + switchOneState=true; + switchTwoState=false; + switchOneChange=false; + switchTwoChange=false; + + // ATTENTION: initial state of the switch should correspond to the inital state of the threshold mode in the constructor of the laser trajectory objects (not nice): + setSwitchOneState(true); //equal to switchOneState=true, plus set led value. False means fixed threshold, true AUTO THRESHOLD (will be the default mode) + // NOTE: actually this interrupt switches are not used anymore to change threhold... + + // Initial state of the two state switch (and don't forget to set the internal pullup resistors!): + twoStateSwitch.mode(PullUp);// Use internal pullup for pushbutton + twoStateSwitchState=twoStateSwitch.read(); // attention: twoStateSwitch is actually a method (this is equal to twoStateSwitch.read()); + twoStateSwitchChange=true; // this will force reading the initial threshold mode the first time the program runs + + // Read and update pot value: + // updatePotValue(); // the value will be ajusted in the range 0-255 +} + +void HardwareIO::setSwitchOneState(bool newstate) { + switchOneState=newstate; + //ledSwitchOne=(switchOneState? 1 :0); +} + +// these ISR could do more (like debouncing). +// For the time being, I will debounce electrically with a small capacitor. +void HardwareIO::switchOneInterrupt() { + switchOneState=!switchOneState; + switchOneChange=true; +} + +void HardwareIO::switchTwoInterrupt() { + switchTwoState=!switchTwoState; + switchTwoChange=true; +} + +bool HardwareIO::switchOneCheck(bool& new_state) { + new_state=switchOneState; + if (switchOneChange) { + switchOneChange=false; + return(true); + } else + return(false); +} + +bool HardwareIO::switchTwoCheck(bool& new_state) { + new_state=switchTwoState; + if (switchTwoChange) { + switchTwoChange=false; + return(true); + } else return(false); +} + +// NOT interrupt-based switch: +bool HardwareIO::twoStateSwitchCheck(bool& new_state) { + new_state=twoStateSwitch; // note: twoStateSwitch is a method! + if (twoStateSwitchState==new_state) { + // this means that the switch did not change state: + return(false); + } else { + twoStateSwitchState=new_state; + return(true); + } +} + + +// THIS IS NOT WORKING!!!!!! +unsigned char HardwareIO::updatePotValue() { // this will update the pot value, and return it too. + //The value will be ajusted in the range 0-255 + //The 0.0v to 3.3v range of the AnalogIn is represented in software as a normalised floating point number from 0.0 to 1.0. + + // potValue=(unsigned char )(ainPot*255); + // Attention: go back to burst mode: + // lockin.setADC_forLockin(1); + // wait(1); + + //USING the adc library: + // unset fast adc for lockin, and set normal adc for conversion from analog input pin: + lockin.setADC_forLockin(0); + // wait(1); + + //Measure pin POT_ANALOG_INPUT + adc.select(POT_ANALOG_INPUT); + //Start ADC conversion + adc.start(); + //Wait for it to complete + while(!adc.done(POT_ANALOG_INPUT)); + potValue=adc.read(POT_ANALOG_INPUT); + + //Unset pin POT_ANALOG_INPUT + adc.setup(POT_ANALOG_INPUT,0); + + lockin.setADC_forLockin(1); + wait(0.5); + + return(potValue); +} + +//write on the first DAC, output A (mirror X) +void HardwareIO::writeOutX(unsigned short value){ + if(value > MAX_AD_MIRRORS) value = MAX_AD_MIRRORS; + if(value < MIN_AD_MIRRORS) value = MIN_AD_MIRRORS; + + value |= 0x7000; + value &= 0x7FFF; + + csDAC = 0; // this means the chip is enabled (negated logic), so CLK and SDI (data) can be transfered + spiDAC.write(value); + csDAC = 1; // rising the pin actually writes the data in the corresponding DAC register... +} + +//write on the first DAC, output B (mirror Y) +void HardwareIO::writeOutY(unsigned short value){ + if(value > MAX_AD_MIRRORS) value = MAX_AD_MIRRORS; + if(value < MIN_AD_MIRRORS) value = MIN_AD_MIRRORS; + + value |= 0xF000; + value &= 0xFFFF; + + csDAC = 0; + spiDAC.write(value); + csDAC = 1; +} + +void HardwareIO::setLaserLockinPower(int powerValue){ + if(powerValue > 0){ + lockin.setLaserPower(true); + } + else{ + lockin.setLaserPower(false); + } +} +// THE TTL controlled lasers: +// Note: the red one is not used here +void HardwareIO::setRedPower(int powerValue){ + if(powerValue > 0){ + Laser_Red = 1; + } + else{ + Laser_Red = 0; + } +} +void HardwareIO::setGreenPower(int powerValue){ + if(powerValue > 0){ + Laser_Green = 1; + } + else{ + Laser_Green = 0; + } +} +void HardwareIO::setBluePower(int powerValue){ + if(powerValue > 0){ + Laser_Blue = 1; + } + else{ + Laser_Blue = 0; + } +} + +void HardwareIO::setRGBPower(unsigned char color) { + lockin.setLaserPower((color&0x04)>0? true : false); // NOTE: here the "red" is the lockin laser, not the TTL one (not used yet) + Laser_Green=(color&0x02)>>1; + Laser_Blue =color&0x01; +} + +void HardwareIO::showLimitsMirrors(int seconds) { + unsigned short pointsPerLine=150; + int shiftX = (MAX_AD_MIRRORS - MIN_AD_MIRRORS) / pointsPerLine; + int shiftY = (MAX_AD_MIRRORS - MIN_AD_MIRRORS) / pointsPerLine; + + Laser_Green=1; + + //for (int repeat=0; repeat<times; repeat++) { + + Timer t; + t.start(); + while(t.read_ms()<seconds*1000) { + + writeOutX(MIN_AD_MIRRORS);writeOutY(MIN_AD_MIRRORS); + + for(int j=0; j<pointsPerLine; j++){ + wait_us(200);//delay between each points + writeOutY(j*shiftY + MIN_AD_MIRRORS); + } + + writeOutX(MIN_AD_MIRRORS);writeOutY(MAX_AD_MIRRORS); + for(int j=0; j<pointsPerLine; j++) { + wait_us(200);//delay between each points + writeOutX(j*shiftX + MIN_AD_MIRRORS); + } + + writeOutX(MAX_AD_MIRRORS);writeOutY(MAX_AD_MIRRORS); + for(int j=0; j<pointsPerLine; j++) { + wait_us(200);//delay between each points + writeOutY(-j*shiftX + MAX_AD_MIRRORS); + } + + writeOutX(MAX_AD_MIRRORS);writeOutY(MIN_AD_MIRRORS); + for(int j=0; j<pointsPerLine; j++) { + wait_us(200);//delay between each points + writeOutX(-j*shiftX + MAX_AD_MIRRORS); + } + + } + t.stop(); + Laser_Green=0; +} + +void HardwareIO::scan_serial(unsigned short pointsPerLine){ + //scan the total surface with a custom resolution + //send the lockin value for each point as a byte on the serial port to the PC + //use "scanSLP_save" to see the data on processing + // First, set the red laser on, and other off: + setRGBPower(0x4); + wait_us(1000); // just to give time to the lockin to set + + int shiftX = (MAX_AD_MIRRORS - MIN_AD_MIRRORS) / pointsPerLine; + int shiftY = (MAX_AD_MIRRORS - MIN_AD_MIRRORS) / pointsPerLine; + + for(int j=0; j<pointsPerLine; j++){ + writeOutX(MIN_AD_MIRRORS); + writeOutY(j*shiftY + MIN_AD_MIRRORS); + + wait_us(300);//begining of line delay + for(int i=0; i<pointsPerLine; i++){ + writeOutX(i*shiftX + MIN_AD_MIRRORS); + + wait_us(200);//delay between each points + + // SEND A VALUE BETWEEN 0 and 255: + pc.putc(int(255.0*lockin.getMedianValue()/4095));//printf("%dL",int(valueLockin*255));//pc.putc(int(lockin*255));// + } + } +} + +//load Look-up Table from LUT.TXT file +//or create the file with scanLUT() if not existing. +void HardwareIO::setLUT(){ + + FILE *fp = fopen(LUT_FILENAME, "r"); // Open file on the local file system for writing + if(fp){ + //load the file into the lut table; keep the SAME resolution! + fread(lut,sizeof(uint16),LUT_RESOLUTION*LUT_RESOLUTION,fp); + fclose(fp); + } + else{ + //fclose(fp); + //if the file "LUT.TXT" doesn't exist, create one with scanLUT() + lockin.setLaserPower(true); + scanLUT(); + } + +} + +//scan the total surface with a fixed 2^x resolution +//create the Look-Up Table used to "flatten" the scan according to the position +// +//To Do: maybe detect high frequency to be sure the area is clean and empty? +void HardwareIO::scanLUT(){ + + //reset lut table + for(int j=0; j<LUT_RESOLUTION; j++){ + for(int i=0; i<LUT_RESOLUTION; i++){ + lut[i][j] =0; + } + } + + int delayScanning = 300; //in us + + //define the distance between each points (from 0 to 4096) and the offset (here 0) + float shiftX = 1.0*(MAX_AD_MIRRORS - MIN_AD_MIRRORS) / (LUT_RESOLUTION-1); + float shiftY = 1.0*(MAX_AD_MIRRORS - MIN_AD_MIRRORS) / (LUT_RESOLUTION-1); + float offsetX = MIN_AD_MIRRORS; + float offsetY = MIN_AD_MIRRORS; + + //move the mirrors to the first position + writeOutX(MAX_AD_MIRRORS);writeOutY(MIN_AD_MIRRORS); + wait_us(500); + + float x, y; + + //scan the surface NB_SCANS times + //the total value in lut[i][j] shouldn't exceed uint16 !!! + for(int loop=0; loop<NB_SCANS; loop++){ + for(int j=0; j<LUT_RESOLUTION; j++){ + y = shiftY*j + offsetY ; + writeOutY(int(y)); + //scan from right to left + for(int i=LUT_RESOLUTION-1; i>=0; i--){ + x = shiftX*i + offsetX; + writeOutX(int(x)); + wait_us(delayScanning); + lut[i][j] += lockin_read(); + } + //re-scan from left to right + for(int i=0; i<LUT_RESOLUTION; i++){ + x = shiftX*i + offsetX; + writeOutX(int(x)); + wait_us(delayScanning); + lut[i][j] += lockin_read(); + } + } + } + + + //save tab in file + FILE *fp; +#ifdef LUT_FILENAME + fp = fopen(LUT_FILENAME, "w"); // Open file on the local file system for writing + fwrite(lut,sizeof(uint16),LUT_RESOLUTION*LUT_RESOLUTION,fp); + fclose(fp); //close the file (the mBed will appear connected again) +#endif + +#ifdef LUT_H_FILENAME + //save tab in Human readable file (not used by the program, this is just for checking) + // NOTE: we divide the content of the lut table by NB_SCANS, for easy reading (values should be between 0-4095) + fp = fopen(LUT_H_FILENAME, "w"); // Open file on the local file system for writing + fprintf(fp, "scan resolution: %d x %d\r\n",LUT_RESOLUTION, LUT_RESOLUTION); + for(int j=0; j<LUT_RESOLUTION; j++){ + for(int i=0; i<LUT_RESOLUTION; i++){ + fprintf(fp, "X=%d,\tY=%d,\tI=%d\t \r\n", int(shiftX*i + offsetX), int(shiftY*j + offsetY), int(1.0*lut[i][j]/NB_SCANS) ); + } + } + fclose(fp); //close the file (the mBed will appear connected again) +#endif + +} + + +//Return the lockin value "corrected with the Look-UpTable" - this means a RATIO between two reflectivities (and normally, this is <1). +float HardwareIO::lockInCorrectedValue(unsigned short x, unsigned short y){ +//*******Correction using DIRECT approximation +#ifdef LUT_DIRECT + return 2.0* NB_SCANS * lockin_read() / (lut[x >> LUT_BITS_SHIFT][y >> LUT_BITS_SHIFT]); // 2 * NB_SCANS is the number of recorded sample added to one position of the LUT (scan is performed twice: left-right and right-left) +#endif + +//*******Correction using BILINEAR approximation +#ifdef LUT_BILINEAR + unsigned short X = x >> LUT_BITS_SHIFT; //mirror "x" is 12bits, LUT "X" needs 4bits when lut is 17x17 + unsigned short Y = y >> LUT_BITS_SHIFT; //mirror "y" is 12bits, LUT "Y" needs 4bits when lut is 17x17 + float dx = 1.0*(x & LUT_BITS_MASK)/(LUT_BITS_MASK+1); //weight to apply on X (mask with 255 and norm) + float dy = 1.0*(y & LUT_BITS_MASK)/(LUT_BITS_MASK+1); //weight to apply on Y (mask with 255 and norm) + + //Wheighted mean approximation of the Look-Up Table at the position (x,y): + float wmLUT = (1-dy)*( (1-dx)*lut[X][Y] + dx*lut[X+1][Y] ) + dy*( (1-dx)*lut[X][Y+1] + dx*lut[X+1][Y+1] ); + + return 2.0* NB_SCANS * lockin_read() / wmLUT;// 2 * NB_SCANS is the number of recorded sample added to one position of the LUT (scan is performed twice: left-right and right-left) +#endif + +//*******Correction using LINEAR approximation +#ifdef LUT_LINEAR + unsigned short X = x >> LUT_BITS_SHIFT; //mirror "x" is 12bits, LUT "X" needs 4bits when lut is 17x17 + unsigned short Y = y >> LUT_BITS_SHIFT; //mirror "y" is 12bits, LUT "Y" needs 4bits when lut is 17x17 + float dx = 1.0*(x & LUT_BITS_MASK)/(LUT_BITS_MASK+1); //weight to apply on X (mask with 255 and norm) + float dy = 1.0*(y & LUT_BITS_MASK)/(LUT_BITS_MASK+1); //weight to apply on Y (mask with 255 and norm) + float linearLUT, dzx, dzy; + + if(dx>dy){ //if the position is on the "top-right" triangle + dzx = (lut[X+1][Y] - lut[X][Y]) * dx; + dzy = (lut[X+1][Y+1] - lut[X+1][Y]) * dy; + } + else{ //if the position is on the "bottom-left" triangle + dzy = (lut[X][Y+1] - lut[X][Y]) * dy; + dzx = (lut[X+1][Y+1] - lut[X][Y+1]) * dx; + } + + //linear approximation of the Look-Up Table at the position (x,y): + linearLUT = lut[X][Y] + dzx + dzy; + return 2.0* NB_SCANS * lockin_read() / linearLUT; // 2 * NB_SCANS is the number of recorded sample added to one position of the LUT (scan is performed twice: left-right and right-left) + +#endif + +//*******No corrections, just return the value divided by 4096 (this means we are assuming that the surface is everywhere perfectly reflective - we supposedly get the max value always) +#ifdef NO_LUT + return 1.0* lockin_read()/4096; +#endif + +}