Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of scoreLight_Advanced 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 |
