Laser Sensing Display for UI interfaces in the real world
Fork of skinGames_forktest by
hardwareIO/hardwareIO.cpp@0:345b3bc7a0ea, 2012-03-28 (annotated)
- Committer:
- mbedalvaro
- Date:
- Wed Mar 28 14:40:01 2012 +0000
- Revision:
- 0:345b3bc7a0ea
- Child:
- 10:6f8e48dca1bd
This version (using rigid frame, base and child classes, etc) works, but the blob is strangely smaller. Need to check this.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mbedalvaro | 0:345b3bc7a0ea | 1 | #include "hardwareIO.h" |
mbedalvaro | 0:345b3bc7a0ea | 2 | |
mbedalvaro | 0:345b3bc7a0ea | 3 | HardwareIO IO; // preintantiation of cross-file global object IO |
mbedalvaro | 0:345b3bc7a0ea | 4 | |
mbedalvaro | 0:345b3bc7a0ea | 5 | // -------------------------------------- (0) SETUP ALL IO (call this in the setup() function in main program) |
mbedalvaro | 0:345b3bc7a0ea | 6 | |
mbedalvaro | 0:345b3bc7a0ea | 7 | Serial pc(USBTX, USBRX); // tx, rx |
mbedalvaro | 0:345b3bc7a0ea | 8 | LocalFileSystem local("local"); // Create the local filesystem under the name "local" |
mbedalvaro | 0:345b3bc7a0ea | 9 | |
mbedalvaro | 0:345b3bc7a0ea | 10 | SPI spiDAC(MOSI_PIN, MISO_PIN, SCK_PIN); // mosi, miso, sclk |
mbedalvaro | 0:345b3bc7a0ea | 11 | DigitalOut csDAC(CS_DAC_MIRRORS); |
mbedalvaro | 0:345b3bc7a0ea | 12 | |
mbedalvaro | 0:345b3bc7a0ea | 13 | DigitalOut Laser_Red(LASER_RED_PIN); |
mbedalvaro | 0:345b3bc7a0ea | 14 | DigitalOut Laser_Green(LASER_GREEN_PIN); |
mbedalvaro | 0:345b3bc7a0ea | 15 | DigitalOut Laser_Blue(LASER_BLUE_PIN); |
mbedalvaro | 0:345b3bc7a0ea | 16 | |
mbedalvaro | 0:345b3bc7a0ea | 17 | void HardwareIO::init(void) { |
mbedalvaro | 0:345b3bc7a0ea | 18 | Laser_Red = 1; |
mbedalvaro | 0:345b3bc7a0ea | 19 | Laser_Green = 0; |
mbedalvaro | 0:345b3bc7a0ea | 20 | Laser_Blue = 0; |
mbedalvaro | 0:345b3bc7a0ea | 21 | |
mbedalvaro | 0:345b3bc7a0ea | 22 | //Serial Communication setup: |
mbedalvaro | 0:345b3bc7a0ea | 23 | pc.baud(921600);//115200);// |
mbedalvaro | 0:345b3bc7a0ea | 24 | |
mbedalvaro | 0:345b3bc7a0ea | 25 | // Setup for lock-in amplifier and pwm references: |
mbedalvaro | 0:345b3bc7a0ea | 26 | lockin.init(); |
mbedalvaro | 0:345b3bc7a0ea | 27 | |
mbedalvaro | 0:345b3bc7a0ea | 28 | // Setup the spi for 8 bit data, high steady state clock, |
mbedalvaro | 0:345b3bc7a0ea | 29 | // second edge capture, with a 10MHz clock rate |
mbedalvaro | 0:345b3bc7a0ea | 30 | csDAC = 1; |
mbedalvaro | 0:345b3bc7a0ea | 31 | spiDAC.format(16,0); |
mbedalvaro | 0:345b3bc7a0ea | 32 | spiDAC.frequency(16000000); |
mbedalvaro | 0:345b3bc7a0ea | 33 | |
mbedalvaro | 0:345b3bc7a0ea | 34 | // default initial mirror position: |
mbedalvaro | 0:345b3bc7a0ea | 35 | writeOutX(CENTER_AD_MIRROR_X); |
mbedalvaro | 0:345b3bc7a0ea | 36 | writeOutY(CENTER_AD_MIRROR_Y); |
mbedalvaro | 0:345b3bc7a0ea | 37 | |
mbedalvaro | 0:345b3bc7a0ea | 38 | // Load LUT table: |
mbedalvaro | 0:345b3bc7a0ea | 39 | setLUT(); |
mbedalvaro | 0:345b3bc7a0ea | 40 | } |
mbedalvaro | 0:345b3bc7a0ea | 41 | |
mbedalvaro | 0:345b3bc7a0ea | 42 | //write on the first DAC, output A (mirror X) |
mbedalvaro | 0:345b3bc7a0ea | 43 | void HardwareIO::writeOutX(int value){ |
mbedalvaro | 0:345b3bc7a0ea | 44 | if(value > MAX_AD_MIRRORS) value = MAX_AD_MIRRORS; |
mbedalvaro | 0:345b3bc7a0ea | 45 | if(value < MIN_AD_MIRRORS) value = MIN_AD_MIRRORS; |
mbedalvaro | 0:345b3bc7a0ea | 46 | |
mbedalvaro | 0:345b3bc7a0ea | 47 | value |= 0x7000; |
mbedalvaro | 0:345b3bc7a0ea | 48 | value &= 0x7FFF; |
mbedalvaro | 0:345b3bc7a0ea | 49 | |
mbedalvaro | 0:345b3bc7a0ea | 50 | csDAC = 0; |
mbedalvaro | 0:345b3bc7a0ea | 51 | spiDAC.write(value); |
mbedalvaro | 0:345b3bc7a0ea | 52 | csDAC = 1; |
mbedalvaro | 0:345b3bc7a0ea | 53 | } |
mbedalvaro | 0:345b3bc7a0ea | 54 | |
mbedalvaro | 0:345b3bc7a0ea | 55 | //write on the first DAC, output B (mirror Y) |
mbedalvaro | 0:345b3bc7a0ea | 56 | void HardwareIO::writeOutY(int value){ |
mbedalvaro | 0:345b3bc7a0ea | 57 | if(value > MAX_AD_MIRRORS) value = MAX_AD_MIRRORS; |
mbedalvaro | 0:345b3bc7a0ea | 58 | if(value < MIN_AD_MIRRORS) value = MIN_AD_MIRRORS; |
mbedalvaro | 0:345b3bc7a0ea | 59 | |
mbedalvaro | 0:345b3bc7a0ea | 60 | value |= 0xF000; |
mbedalvaro | 0:345b3bc7a0ea | 61 | value &= 0xFFFF; |
mbedalvaro | 0:345b3bc7a0ea | 62 | |
mbedalvaro | 0:345b3bc7a0ea | 63 | csDAC = 0; |
mbedalvaro | 0:345b3bc7a0ea | 64 | spiDAC.write(value); |
mbedalvaro | 0:345b3bc7a0ea | 65 | csDAC = 1; |
mbedalvaro | 0:345b3bc7a0ea | 66 | } |
mbedalvaro | 0:345b3bc7a0ea | 67 | |
mbedalvaro | 0:345b3bc7a0ea | 68 | void HardwareIO::setRedPower(int powerValue){ |
mbedalvaro | 0:345b3bc7a0ea | 69 | if(powerValue > 0){ |
mbedalvaro | 0:345b3bc7a0ea | 70 | lockin.setLaserPower(true); |
mbedalvaro | 0:345b3bc7a0ea | 71 | } |
mbedalvaro | 0:345b3bc7a0ea | 72 | else{ |
mbedalvaro | 0:345b3bc7a0ea | 73 | lockin.setLaserPower(false); |
mbedalvaro | 0:345b3bc7a0ea | 74 | } |
mbedalvaro | 0:345b3bc7a0ea | 75 | } |
mbedalvaro | 0:345b3bc7a0ea | 76 | void HardwareIO::setGreenPower(int powerValue){ |
mbedalvaro | 0:345b3bc7a0ea | 77 | if(powerValue > 0){ |
mbedalvaro | 0:345b3bc7a0ea | 78 | Laser_Green = 1; |
mbedalvaro | 0:345b3bc7a0ea | 79 | } |
mbedalvaro | 0:345b3bc7a0ea | 80 | else{ |
mbedalvaro | 0:345b3bc7a0ea | 81 | Laser_Green = 0; |
mbedalvaro | 0:345b3bc7a0ea | 82 | } |
mbedalvaro | 0:345b3bc7a0ea | 83 | } |
mbedalvaro | 0:345b3bc7a0ea | 84 | void HardwareIO::setBluePower(int powerValue){ |
mbedalvaro | 0:345b3bc7a0ea | 85 | if(powerValue > 0){ |
mbedalvaro | 0:345b3bc7a0ea | 86 | Laser_Blue = 1; |
mbedalvaro | 0:345b3bc7a0ea | 87 | } |
mbedalvaro | 0:345b3bc7a0ea | 88 | else{ |
mbedalvaro | 0:345b3bc7a0ea | 89 | Laser_Blue = 0; |
mbedalvaro | 0:345b3bc7a0ea | 90 | } |
mbedalvaro | 0:345b3bc7a0ea | 91 | } |
mbedalvaro | 0:345b3bc7a0ea | 92 | |
mbedalvaro | 0:345b3bc7a0ea | 93 | void HardwareIO::setRGBPower(char color) { |
mbedalvaro | 0:345b3bc7a0ea | 94 | lockin.setLaserPower(color&0x04); |
mbedalvaro | 0:345b3bc7a0ea | 95 | Laser_Green=(color&0x02)>>1; |
mbedalvaro | 0:345b3bc7a0ea | 96 | Laser_Blue =(color&0x01); |
mbedalvaro | 0:345b3bc7a0ea | 97 | } |
mbedalvaro | 0:345b3bc7a0ea | 98 | |
mbedalvaro | 0:345b3bc7a0ea | 99 | void HardwareIO::scan_serial(int pointsPerLine){ |
mbedalvaro | 0:345b3bc7a0ea | 100 | //scan the total surface with a custom resolution |
mbedalvaro | 0:345b3bc7a0ea | 101 | //send the lockin value for each point as a byte on the serial port to the PC |
mbedalvaro | 0:345b3bc7a0ea | 102 | //use "scanSLP_save" to see the data on processing |
mbedalvaro | 0:345b3bc7a0ea | 103 | |
mbedalvaro | 0:345b3bc7a0ea | 104 | |
mbedalvaro | 0:345b3bc7a0ea | 105 | int shiftX = (MAX_AD_MIRRORS - MIN_AD_MIRRORS) / pointsPerLine; |
mbedalvaro | 0:345b3bc7a0ea | 106 | int shiftY = (MAX_AD_MIRRORS - MIN_AD_MIRRORS) / pointsPerLine; |
mbedalvaro | 0:345b3bc7a0ea | 107 | |
mbedalvaro | 0:345b3bc7a0ea | 108 | for(int j=0; j<pointsPerLine; j++){ |
mbedalvaro | 0:345b3bc7a0ea | 109 | writeOutX(MIN_AD_MIRRORS); |
mbedalvaro | 0:345b3bc7a0ea | 110 | writeOutY(j*shiftY + MIN_AD_MIRRORS); |
mbedalvaro | 0:345b3bc7a0ea | 111 | |
mbedalvaro | 0:345b3bc7a0ea | 112 | wait_us(300);//begining of line delay |
mbedalvaro | 0:345b3bc7a0ea | 113 | for(int i=0; i<pointsPerLine; i++){ |
mbedalvaro | 0:345b3bc7a0ea | 114 | writeOutX(i*shiftX + MIN_AD_MIRRORS); |
mbedalvaro | 0:345b3bc7a0ea | 115 | |
mbedalvaro | 0:345b3bc7a0ea | 116 | wait_us(100);//delay between each points |
mbedalvaro | 0:345b3bc7a0ea | 117 | pc.putc(int(255.0*lockin.getMedianValue()/4095));//printf("%dL",int(valueLockin*255));//pc.putc(int(lockin*255));// |
mbedalvaro | 0:345b3bc7a0ea | 118 | } |
mbedalvaro | 0:345b3bc7a0ea | 119 | } |
mbedalvaro | 0:345b3bc7a0ea | 120 | } |
mbedalvaro | 0:345b3bc7a0ea | 121 | |
mbedalvaro | 0:345b3bc7a0ea | 122 | //load Look-up Table from LUT.TXT file |
mbedalvaro | 0:345b3bc7a0ea | 123 | //or create the file with scanLUT() if not existing. |
mbedalvaro | 0:345b3bc7a0ea | 124 | void HardwareIO::setLUT(){ |
mbedalvaro | 0:345b3bc7a0ea | 125 | |
mbedalvaro | 0:345b3bc7a0ea | 126 | FILE *fp = fopen(LUT_FILENAME, "r"); // Open file on the local file system for writing |
mbedalvaro | 0:345b3bc7a0ea | 127 | if(fp){ |
mbedalvaro | 0:345b3bc7a0ea | 128 | //load the file into the lut table; keep the SAME resolution! |
mbedalvaro | 0:345b3bc7a0ea | 129 | fread(lut,sizeof(uint16),LUT_RESOLUTION*LUT_RESOLUTION,fp); |
mbedalvaro | 0:345b3bc7a0ea | 130 | fclose(fp); |
mbedalvaro | 0:345b3bc7a0ea | 131 | } |
mbedalvaro | 0:345b3bc7a0ea | 132 | else{ |
mbedalvaro | 0:345b3bc7a0ea | 133 | //fclose(fp); |
mbedalvaro | 0:345b3bc7a0ea | 134 | //if the file "LUT.TXT" doesn't exist, create one with scanLUT() |
mbedalvaro | 0:345b3bc7a0ea | 135 | lockin.setLaserPower(true); |
mbedalvaro | 0:345b3bc7a0ea | 136 | scanLUT(); |
mbedalvaro | 0:345b3bc7a0ea | 137 | } |
mbedalvaro | 0:345b3bc7a0ea | 138 | |
mbedalvaro | 0:345b3bc7a0ea | 139 | } |
mbedalvaro | 0:345b3bc7a0ea | 140 | |
mbedalvaro | 0:345b3bc7a0ea | 141 | //scan the total surface with a fixed 2^x resolution |
mbedalvaro | 0:345b3bc7a0ea | 142 | //create the Look-Up Table used to "flatten" the scan according to the position |
mbedalvaro | 0:345b3bc7a0ea | 143 | // |
mbedalvaro | 0:345b3bc7a0ea | 144 | //To Do: maybe detect high frequency to be sure the area is clean and empty? |
mbedalvaro | 0:345b3bc7a0ea | 145 | void HardwareIO::scanLUT(){ |
mbedalvaro | 0:345b3bc7a0ea | 146 | |
mbedalvaro | 0:345b3bc7a0ea | 147 | //reset lut table |
mbedalvaro | 0:345b3bc7a0ea | 148 | for(int j=0; j<LUT_RESOLUTION; j++){ |
mbedalvaro | 0:345b3bc7a0ea | 149 | for(int i=0; i<LUT_RESOLUTION; i++){ |
mbedalvaro | 0:345b3bc7a0ea | 150 | lut[i][j] =0; |
mbedalvaro | 0:345b3bc7a0ea | 151 | } |
mbedalvaro | 0:345b3bc7a0ea | 152 | } |
mbedalvaro | 0:345b3bc7a0ea | 153 | |
mbedalvaro | 0:345b3bc7a0ea | 154 | int delayScanning = 300; //in us |
mbedalvaro | 0:345b3bc7a0ea | 155 | |
mbedalvaro | 0:345b3bc7a0ea | 156 | //define the distance between each points (from 0 to 4096) and the offset (here 0) |
mbedalvaro | 0:345b3bc7a0ea | 157 | float shiftX = 1.0*(MAX_AD_MIRRORS - MIN_AD_MIRRORS) / (LUT_RESOLUTION-1); |
mbedalvaro | 0:345b3bc7a0ea | 158 | float shiftY = 1.0*(MAX_AD_MIRRORS - MIN_AD_MIRRORS) / (LUT_RESOLUTION-1); |
mbedalvaro | 0:345b3bc7a0ea | 159 | float offsetX = MIN_AD_MIRRORS; |
mbedalvaro | 0:345b3bc7a0ea | 160 | float offsetY = MIN_AD_MIRRORS; |
mbedalvaro | 0:345b3bc7a0ea | 161 | |
mbedalvaro | 0:345b3bc7a0ea | 162 | //move the mirrors to the first position |
mbedalvaro | 0:345b3bc7a0ea | 163 | writeOutX(MAX_AD_MIRRORS);writeOutY(MIN_AD_MIRRORS); |
mbedalvaro | 0:345b3bc7a0ea | 164 | wait_us(500); |
mbedalvaro | 0:345b3bc7a0ea | 165 | |
mbedalvaro | 0:345b3bc7a0ea | 166 | float x, y; |
mbedalvaro | 0:345b3bc7a0ea | 167 | |
mbedalvaro | 0:345b3bc7a0ea | 168 | //scan the surface 8 times |
mbedalvaro | 0:345b3bc7a0ea | 169 | //the total value in lut[i][j] shouldn't exceed uint16 !!! |
mbedalvaro | 0:345b3bc7a0ea | 170 | for(int loop=0; loop<8; loop++){ |
mbedalvaro | 0:345b3bc7a0ea | 171 | for(int j=0; j<LUT_RESOLUTION; j++){ |
mbedalvaro | 0:345b3bc7a0ea | 172 | y = shiftY*j + offsetY ; |
mbedalvaro | 0:345b3bc7a0ea | 173 | writeOutY(int(y)); |
mbedalvaro | 0:345b3bc7a0ea | 174 | //scan from right to left |
mbedalvaro | 0:345b3bc7a0ea | 175 | for(int i=LUT_RESOLUTION-1; i>=0; i--){ |
mbedalvaro | 0:345b3bc7a0ea | 176 | x = shiftX*i + offsetX; |
mbedalvaro | 0:345b3bc7a0ea | 177 | writeOutX(int(x)); |
mbedalvaro | 0:345b3bc7a0ea | 178 | wait_us(delayScanning); |
mbedalvaro | 0:345b3bc7a0ea | 179 | lut[i][j] += lockin_read(); |
mbedalvaro | 0:345b3bc7a0ea | 180 | } |
mbedalvaro | 0:345b3bc7a0ea | 181 | //re-scan from left to right |
mbedalvaro | 0:345b3bc7a0ea | 182 | for(int i=0; i<LUT_RESOLUTION; i++){ |
mbedalvaro | 0:345b3bc7a0ea | 183 | x = shiftX*i + offsetX; |
mbedalvaro | 0:345b3bc7a0ea | 184 | writeOutX(int(x)); |
mbedalvaro | 0:345b3bc7a0ea | 185 | wait_us(delayScanning); |
mbedalvaro | 0:345b3bc7a0ea | 186 | lut[i][j] += lockin_read(); |
mbedalvaro | 0:345b3bc7a0ea | 187 | } |
mbedalvaro | 0:345b3bc7a0ea | 188 | } |
mbedalvaro | 0:345b3bc7a0ea | 189 | } |
mbedalvaro | 0:345b3bc7a0ea | 190 | |
mbedalvaro | 0:345b3bc7a0ea | 191 | |
mbedalvaro | 0:345b3bc7a0ea | 192 | //save tab in file |
mbedalvaro | 0:345b3bc7a0ea | 193 | FILE *fp; |
mbedalvaro | 0:345b3bc7a0ea | 194 | #ifdef LUT_FILENAME |
mbedalvaro | 0:345b3bc7a0ea | 195 | fp = fopen(LUT_FILENAME, "w"); // Open file on the local file system for writing |
mbedalvaro | 0:345b3bc7a0ea | 196 | fwrite(lut,sizeof(uint16),LUT_RESOLUTION*LUT_RESOLUTION,fp); |
mbedalvaro | 0:345b3bc7a0ea | 197 | fclose(fp); //close the file (the mBed will appear connected again) |
mbedalvaro | 0:345b3bc7a0ea | 198 | #endif |
mbedalvaro | 0:345b3bc7a0ea | 199 | |
mbedalvaro | 0:345b3bc7a0ea | 200 | #ifdef LUT_H_FILENAME |
mbedalvaro | 0:345b3bc7a0ea | 201 | //save tab in human readable file |
mbedalvaro | 0:345b3bc7a0ea | 202 | fp = fopen(LUT_H_FILENAME, "w"); // Open file on the local file system for writing |
mbedalvaro | 0:345b3bc7a0ea | 203 | fprintf(fp, "scan resolution: %d x %d\r\n",LUT_RESOLUTION, LUT_RESOLUTION); |
mbedalvaro | 0:345b3bc7a0ea | 204 | for(int j=0; j<LUT_RESOLUTION; j++){ |
mbedalvaro | 0:345b3bc7a0ea | 205 | for(int i=0; i<LUT_RESOLUTION; i++){ |
mbedalvaro | 0:345b3bc7a0ea | 206 | fprintf(fp, "X=%d,\tY=%d,\tI=%d\t \r\n", int(shiftX*i + offsetX), int(shiftY*j + offsetY), lut[i][j] ); |
mbedalvaro | 0:345b3bc7a0ea | 207 | } |
mbedalvaro | 0:345b3bc7a0ea | 208 | } |
mbedalvaro | 0:345b3bc7a0ea | 209 | fclose(fp); //close the file (the mBed will appear connected again) |
mbedalvaro | 0:345b3bc7a0ea | 210 | #endif |
mbedalvaro | 0:345b3bc7a0ea | 211 | |
mbedalvaro | 0:345b3bc7a0ea | 212 | } |
mbedalvaro | 0:345b3bc7a0ea | 213 | |
mbedalvaro | 0:345b3bc7a0ea | 214 | |
mbedalvaro | 0:345b3bc7a0ea | 215 | //return the lockin value corrected with the Look-UpTable |
mbedalvaro | 0:345b3bc7a0ea | 216 | float HardwareIO::lockInCorrectedValue(int x, int y){ |
mbedalvaro | 0:345b3bc7a0ea | 217 | //*******Correction using DIRECT approximation |
mbedalvaro | 0:345b3bc7a0ea | 218 | #ifdef LUT_DIRECT |
mbedalvaro | 0:345b3bc7a0ea | 219 | return 16.0 * lockin_read() / (lut[x >> LUT_BITS_SHIFT][y >> LUT_BITS_SHIFT]); |
mbedalvaro | 0:345b3bc7a0ea | 220 | |
mbedalvaro | 0:345b3bc7a0ea | 221 | #else |
mbedalvaro | 0:345b3bc7a0ea | 222 | //*******Correction using BILINEAR approximation |
mbedalvaro | 0:345b3bc7a0ea | 223 | #ifdef LUT_BILINEAR |
mbedalvaro | 0:345b3bc7a0ea | 224 | int X = x >> LUT_BITS_SHIFT; //mirror "x" is 12bits, LUT "X" needs 4bits when lut is 17x17 |
mbedalvaro | 0:345b3bc7a0ea | 225 | int Y = y >> LUT_BITS_SHIFT; //mirror "y" is 12bits, LUT "Y" needs 4bits when lut is 17x17 |
mbedalvaro | 0:345b3bc7a0ea | 226 | float dx = 1.0*(x & LUT_BITS_MASK)/(LUT_BITS_MASK+1); //weight to apply on X (mask with 255 and norm) |
mbedalvaro | 0:345b3bc7a0ea | 227 | float dy = 1.0*(y & LUT_BITS_MASK)/(LUT_BITS_MASK+1); //weight to apply on Y (mask with 255 and norm) |
mbedalvaro | 0:345b3bc7a0ea | 228 | |
mbedalvaro | 0:345b3bc7a0ea | 229 | //Wheighted mean approximation of the Look-Up Table at the position (x,y): |
mbedalvaro | 0:345b3bc7a0ea | 230 | 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] ); |
mbedalvaro | 0:345b3bc7a0ea | 231 | |
mbedalvaro | 0:345b3bc7a0ea | 232 | return 16.0 * lockin_read() / wmLUT;//16.0 is the number of recorded sample added to one position of the LUT |
mbedalvaro | 0:345b3bc7a0ea | 233 | |
mbedalvaro | 0:345b3bc7a0ea | 234 | #else |
mbedalvaro | 0:345b3bc7a0ea | 235 | //*******Correction using LINEAR approximation |
mbedalvaro | 0:345b3bc7a0ea | 236 | #ifdef LUT_LINEAR |
mbedalvaro | 0:345b3bc7a0ea | 237 | int X = x >> LUT_BITS_SHIFT; //mirror "x" is 12bits, LUT "X" needs 4bits when lut is 17x17 |
mbedalvaro | 0:345b3bc7a0ea | 238 | int Y = y >> LUT_BITS_SHIFT; //mirror "y" is 12bits, LUT "Y" needs 4bits when lut is 17x17 |
mbedalvaro | 0:345b3bc7a0ea | 239 | float dx = 1.0*(x & LUT_BITS_MASK)/(LUT_BITS_MASK+1); //weight to apply on X (mask with 255 and norm) |
mbedalvaro | 0:345b3bc7a0ea | 240 | float dy = 1.0*(y & LUT_BITS_MASK)/(LUT_BITS_MASK+1); //weight to apply on Y (mask with 255 and norm) |
mbedalvaro | 0:345b3bc7a0ea | 241 | float linearLUT, dzx, dzy; |
mbedalvaro | 0:345b3bc7a0ea | 242 | |
mbedalvaro | 0:345b3bc7a0ea | 243 | if(dx>dy){ //if the position is on the "top-right" triangle |
mbedalvaro | 0:345b3bc7a0ea | 244 | dzx = (lut[X+1][Y] - lut[X][Y]) * dx; |
mbedalvaro | 0:345b3bc7a0ea | 245 | dzy = (lut[X+1][Y+1] - lut[X+1][Y]) * dy; |
mbedalvaro | 0:345b3bc7a0ea | 246 | } |
mbedalvaro | 0:345b3bc7a0ea | 247 | else{ //if the position is on the "bottom-left" triangle |
mbedalvaro | 0:345b3bc7a0ea | 248 | dzy = (lut[X][Y+1] - lut[X][Y]) * dy; |
mbedalvaro | 0:345b3bc7a0ea | 249 | dzx = (lut[X+1][Y+1] - lut[X][Y+1]) * dx; |
mbedalvaro | 0:345b3bc7a0ea | 250 | } |
mbedalvaro | 0:345b3bc7a0ea | 251 | |
mbedalvaro | 0:345b3bc7a0ea | 252 | //linear approximation of the Look-Up Table at the position (x,y): |
mbedalvaro | 0:345b3bc7a0ea | 253 | linearLUT = lut[X][Y] + dzx + dzy; |
mbedalvaro | 0:345b3bc7a0ea | 254 | return 16.0 * lockin_read() / linearLUT; //16.0 is the number of recorded sample added to one position of the LUT |
mbedalvaro | 0:345b3bc7a0ea | 255 | |
mbedalvaro | 0:345b3bc7a0ea | 256 | #else |
mbedalvaro | 0:345b3bc7a0ea | 257 | //*******No corrections, just return the normalized value |
mbedalvaro | 0:345b3bc7a0ea | 258 | return lockin_read()/4096; |
mbedalvaro | 0:345b3bc7a0ea | 259 | |
mbedalvaro | 0:345b3bc7a0ea | 260 | #endif //LUT_LINEAR |
mbedalvaro | 0:345b3bc7a0ea | 261 | #endif //LUT_BILINEAR |
mbedalvaro | 0:345b3bc7a0ea | 262 | #endif //LUT_DIRECT |
mbedalvaro | 0:345b3bc7a0ea | 263 | } |
mbedalvaro | 0:345b3bc7a0ea | 264 |