Alvaro Cassinelli / Mbed 2 deprecated laserUI

Laser Sensing Display for UI interfaces in the real world

Dependencies:   mbed

Fork of skinGames_forktest by Alvaro Cassinelli

hardwareIO/hardwareIO.cpp@31:5f039cbddee8, 2012-09-23 (annotated)

Committer:
mbedalvaro
Date:
Sun Sep 23 10:11:43 2012 +0000
Revision:
31:5f039cbddee8
Child:
34:1244fa3f2559
this is quite nice, but  I am going to make a deep modification of the bouncing principle: instead of depending on the penetration, it will just be a factor over the speed (perfect elastic bouncing being factorElastic=1, and perfectly absorption=0);

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedalvaro 31:5f039cbddee8 1 #include "hardwareIO.h"
mbedalvaro 31:5f039cbddee8 2
mbedalvaro 31:5f039cbddee8 3 HardwareIO IO; // preintantiation of cross-file global object IO
mbedalvaro 31:5f039cbddee8 4
mbedalvaro 31:5f039cbddee8 5 // -------------------------------------- (0) SETUP ALL IO (call this in the setup() function in main program)
mbedalvaro 31:5f039cbddee8 6
mbedalvaro 31:5f039cbddee8 7 Serial pc(USBTX, USBRX); // tx, rx
mbedalvaro 31:5f039cbddee8 8 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
mbedalvaro 31:5f039cbddee8 9
mbedalvaro 31:5f039cbddee8 10 SPI spiDAC(MOSI_PIN, MISO_PIN, SCK_PIN); // mosi, miso, sclk
mbedalvaro 31:5f039cbddee8 11 DigitalOut csDAC(CS_DAC_MIRRORS);
mbedalvaro 31:5f039cbddee8 12
mbedalvaro 31:5f039cbddee8 13 AnalogIn ain(POT_ANALOG_INPUT);
mbedalvaro 31:5f039cbddee8 14
mbedalvaro 31:5f039cbddee8 15 DigitalOut Laser_Red(LASER_RED_PIN); // NOTE: this is NOT the lock in sensing laser (actually, not used yet)
mbedalvaro 31:5f039cbddee8 16 DigitalOut Laser_Green(LASER_GREEN_PIN);
mbedalvaro 31:5f039cbddee8 17 DigitalOut Laser_Blue(LASER_BLUE_PIN);
mbedalvaro 31:5f039cbddee8 18
mbedalvaro 31:5f039cbddee8 19 void HardwareIO::init(void) {
mbedalvaro 31:5f039cbddee8 20 Laser_Red = 0; // note: this is not the lockin-laser!
mbedalvaro 31:5f039cbddee8 21 Laser_Green = 0;
mbedalvaro 31:5f039cbddee8 22 Laser_Blue = 0;
mbedalvaro 31:5f039cbddee8 23
mbedalvaro 31:5f039cbddee8 24 //Serial Communication setup:
mbedalvaro 31:5f039cbddee8 25 pc.baud(115200);//
mbedalvaro 31:5f039cbddee8 26 // pc.baud(921600);//115200);//
mbedalvaro 31:5f039cbddee8 27
mbedalvaro 31:5f039cbddee8 28 // Setup for lock-in amplifier and pwm references:
mbedalvaro 31:5f039cbddee8 29 lockin.init();
mbedalvaro 31:5f039cbddee8 30
mbedalvaro 31:5f039cbddee8 31 // Setup the spi for 8 bit data, high steady state clock,
mbedalvaro 31:5f039cbddee8 32 // second edge capture, with a 10MHz clock rate
mbedalvaro 31:5f039cbddee8 33 csDAC = 1;
mbedalvaro 31:5f039cbddee8 34 spiDAC.format(16,0);
mbedalvaro 31:5f039cbddee8 35 spiDAC.frequency(16000000);
mbedalvaro 31:5f039cbddee8 36
mbedalvaro 31:5f039cbddee8 37 // default initial mirror position:
mbedalvaro 31:5f039cbddee8 38 writeOutX(CENTER_AD_MIRROR_X);
mbedalvaro 31:5f039cbddee8 39 writeOutY(CENTER_AD_MIRROR_Y);
mbedalvaro 31:5f039cbddee8 40
mbedalvaro 31:5f039cbddee8 41 // Load LUT table:
mbedalvaro 31:5f039cbddee8 42 setLUT();
mbedalvaro 31:5f039cbddee8 43 }
mbedalvaro 31:5f039cbddee8 44
mbedalvaro 31:5f039cbddee8 45 //write on the first DAC, output A (mirror X)
mbedalvaro 31:5f039cbddee8 46 void HardwareIO::writeOutX(unsigned short value){
mbedalvaro 31:5f039cbddee8 47 if(value > MAX_AD_MIRRORS) value = MAX_AD_MIRRORS;
mbedalvaro 31:5f039cbddee8 48 if(value < MIN_AD_MIRRORS) value = MIN_AD_MIRRORS;
mbedalvaro 31:5f039cbddee8 49
mbedalvaro 31:5f039cbddee8 50 value |= 0x7000;
mbedalvaro 31:5f039cbddee8 51 value &= 0x7FFF;
mbedalvaro 31:5f039cbddee8 52
mbedalvaro 31:5f039cbddee8 53 csDAC = 0;
mbedalvaro 31:5f039cbddee8 54 spiDAC.write(value);
mbedalvaro 31:5f039cbddee8 55 csDAC = 1;
mbedalvaro 31:5f039cbddee8 56 }
mbedalvaro 31:5f039cbddee8 57
mbedalvaro 31:5f039cbddee8 58 //write on the first DAC, output B (mirror Y)
mbedalvaro 31:5f039cbddee8 59 void HardwareIO::writeOutY(unsigned short value){
mbedalvaro 31:5f039cbddee8 60 if(value > MAX_AD_MIRRORS) value = MAX_AD_MIRRORS;
mbedalvaro 31:5f039cbddee8 61 if(value < MIN_AD_MIRRORS) value = MIN_AD_MIRRORS;
mbedalvaro 31:5f039cbddee8 62
mbedalvaro 31:5f039cbddee8 63 value |= 0xF000;
mbedalvaro 31:5f039cbddee8 64 value &= 0xFFFF;
mbedalvaro 31:5f039cbddee8 65
mbedalvaro 31:5f039cbddee8 66 csDAC = 0;
mbedalvaro 31:5f039cbddee8 67 spiDAC.write(value);
mbedalvaro 31:5f039cbddee8 68 csDAC = 1;
mbedalvaro 31:5f039cbddee8 69 }
mbedalvaro 31:5f039cbddee8 70
mbedalvaro 31:5f039cbddee8 71 void HardwareIO::setRedPower(int powerValue){
mbedalvaro 31:5f039cbddee8 72 if(powerValue > 0){
mbedalvaro 31:5f039cbddee8 73 lockin.setLaserPower(true);
mbedalvaro 31:5f039cbddee8 74 }
mbedalvaro 31:5f039cbddee8 75 else{
mbedalvaro 31:5f039cbddee8 76 lockin.setLaserPower(false);
mbedalvaro 31:5f039cbddee8 77 }
mbedalvaro 31:5f039cbddee8 78 }
mbedalvaro 31:5f039cbddee8 79 void HardwareIO::setGreenPower(int powerValue){
mbedalvaro 31:5f039cbddee8 80 if(powerValue > 0){
mbedalvaro 31:5f039cbddee8 81 Laser_Green = 1;
mbedalvaro 31:5f039cbddee8 82 }
mbedalvaro 31:5f039cbddee8 83 else{
mbedalvaro 31:5f039cbddee8 84 Laser_Green = 0;
mbedalvaro 31:5f039cbddee8 85 }
mbedalvaro 31:5f039cbddee8 86 }
mbedalvaro 31:5f039cbddee8 87 void HardwareIO::setBluePower(int powerValue){
mbedalvaro 31:5f039cbddee8 88 if(powerValue > 0){
mbedalvaro 31:5f039cbddee8 89 Laser_Blue = 1;
mbedalvaro 31:5f039cbddee8 90 }
mbedalvaro 31:5f039cbddee8 91 else{
mbedalvaro 31:5f039cbddee8 92 Laser_Blue = 0;
mbedalvaro 31:5f039cbddee8 93 }
mbedalvaro 31:5f039cbddee8 94 }
mbedalvaro 31:5f039cbddee8 95
mbedalvaro 31:5f039cbddee8 96 void HardwareIO::setRGBPower(unsigned char color) {
mbedalvaro 31:5f039cbddee8 97 lockin.setLaserPower((color&0x04)>0? true : false);
mbedalvaro 31:5f039cbddee8 98 Laser_Green=(color&0x02)>>1;
mbedalvaro 31:5f039cbddee8 99 Laser_Blue =color&0x01;
mbedalvaro 31:5f039cbddee8 100 }
mbedalvaro 31:5f039cbddee8 101
mbedalvaro 31:5f039cbddee8 102 void HardwareIO::showLimitsMirrors(int times) {
mbedalvaro 31:5f039cbddee8 103 unsigned short pointsPerLine=150;
mbedalvaro 31:5f039cbddee8 104 int shiftX = (MAX_AD_MIRRORS - MIN_AD_MIRRORS) / pointsPerLine;
mbedalvaro 31:5f039cbddee8 105 int shiftY = (MAX_AD_MIRRORS - MIN_AD_MIRRORS) / pointsPerLine;
mbedalvaro 31:5f039cbddee8 106
mbedalvaro 31:5f039cbddee8 107 Laser_Green=1;
mbedalvaro 31:5f039cbddee8 108
mbedalvaro 31:5f039cbddee8 109 //for (int repeat=0; repeat<times; repeat++) {
mbedalvaro 31:5f039cbddee8 110
mbedalvaro 31:5f039cbddee8 111 Timer t;
mbedalvaro 31:5f039cbddee8 112 t.start();
mbedalvaro 31:5f039cbddee8 113 while(t.read_ms()<times*1000) {
mbedalvaro 31:5f039cbddee8 114
mbedalvaro 31:5f039cbddee8 115 writeOutX(MIN_AD_MIRRORS);writeOutY(MIN_AD_MIRRORS);
mbedalvaro 31:5f039cbddee8 116
mbedalvaro 31:5f039cbddee8 117 for(int j=0; j<pointsPerLine; j++){
mbedalvaro 31:5f039cbddee8 118 wait_us(200);//delay between each points
mbedalvaro 31:5f039cbddee8 119 writeOutY(j*shiftY + MIN_AD_MIRRORS);
mbedalvaro 31:5f039cbddee8 120 }
mbedalvaro 31:5f039cbddee8 121
mbedalvaro 31:5f039cbddee8 122 writeOutX(MIN_AD_MIRRORS);writeOutY(MAX_AD_MIRRORS);
mbedalvaro 31:5f039cbddee8 123 for(int j=0; j<pointsPerLine; j++) {
mbedalvaro 31:5f039cbddee8 124 wait_us(200);//delay between each points
mbedalvaro 31:5f039cbddee8 125 writeOutX(j*shiftX + MIN_AD_MIRRORS);
mbedalvaro 31:5f039cbddee8 126 }
mbedalvaro 31:5f039cbddee8 127
mbedalvaro 31:5f039cbddee8 128 writeOutX(MAX_AD_MIRRORS);writeOutY(MAX_AD_MIRRORS);
mbedalvaro 31:5f039cbddee8 129 for(int j=0; j<pointsPerLine; j++) {
mbedalvaro 31:5f039cbddee8 130 wait_us(200);//delay between each points
mbedalvaro 31:5f039cbddee8 131 writeOutY(-j*shiftX + MAX_AD_MIRRORS);
mbedalvaro 31:5f039cbddee8 132 }
mbedalvaro 31:5f039cbddee8 133
mbedalvaro 31:5f039cbddee8 134 writeOutX(MAX_AD_MIRRORS);writeOutY(MIN_AD_MIRRORS);
mbedalvaro 31:5f039cbddee8 135 for(int j=0; j<pointsPerLine; j++) {
mbedalvaro 31:5f039cbddee8 136 wait_us(200);//delay between each points
mbedalvaro 31:5f039cbddee8 137 writeOutX(-j*shiftX + MAX_AD_MIRRORS);
mbedalvaro 31:5f039cbddee8 138 }
mbedalvaro 31:5f039cbddee8 139
mbedalvaro 31:5f039cbddee8 140 }
mbedalvaro 31:5f039cbddee8 141 t.stop();
mbedalvaro 31:5f039cbddee8 142 Laser_Green=0;
mbedalvaro 31:5f039cbddee8 143 }
mbedalvaro 31:5f039cbddee8 144
mbedalvaro 31:5f039cbddee8 145 void HardwareIO::scan_serial(unsigned short pointsPerLine){
mbedalvaro 31:5f039cbddee8 146 //scan the total surface with a custom resolution
mbedalvaro 31:5f039cbddee8 147 //send the lockin value for each point as a byte on the serial port to the PC
mbedalvaro 31:5f039cbddee8 148 //use "scanSLP_save" to see the data on processing
mbedalvaro 31:5f039cbddee8 149
mbedalvaro 31:5f039cbddee8 150
mbedalvaro 31:5f039cbddee8 151 int shiftX = (MAX_AD_MIRRORS - MIN_AD_MIRRORS) / pointsPerLine;
mbedalvaro 31:5f039cbddee8 152 int shiftY = (MAX_AD_MIRRORS - MIN_AD_MIRRORS) / pointsPerLine;
mbedalvaro 31:5f039cbddee8 153
mbedalvaro 31:5f039cbddee8 154 for(int j=0; j<pointsPerLine; j++){
mbedalvaro 31:5f039cbddee8 155 writeOutX(MIN_AD_MIRRORS);
mbedalvaro 31:5f039cbddee8 156 writeOutY(j*shiftY + MIN_AD_MIRRORS);
mbedalvaro 31:5f039cbddee8 157
mbedalvaro 31:5f039cbddee8 158 wait_us(300);//begining of line delay
mbedalvaro 31:5f039cbddee8 159 for(int i=0; i<pointsPerLine; i++){
mbedalvaro 31:5f039cbddee8 160 writeOutX(i*shiftX + MIN_AD_MIRRORS);
mbedalvaro 31:5f039cbddee8 161
mbedalvaro 31:5f039cbddee8 162 wait_us(200);//delay between each points
mbedalvaro 31:5f039cbddee8 163
mbedalvaro 31:5f039cbddee8 164 // SEND A VALUE BETWEEN 0 and 255:
mbedalvaro 31:5f039cbddee8 165 pc.putc(int(255.0*lockin.getMedianValue()/4095));//printf("%dL",int(valueLockin*255));//pc.putc(int(lockin*255));//
mbedalvaro 31:5f039cbddee8 166 }
mbedalvaro 31:5f039cbddee8 167 }
mbedalvaro 31:5f039cbddee8 168 }
mbedalvaro 31:5f039cbddee8 169
mbedalvaro 31:5f039cbddee8 170 //load Look-up Table from LUT.TXT file
mbedalvaro 31:5f039cbddee8 171 //or create the file with scanLUT() if not existing.
mbedalvaro 31:5f039cbddee8 172 void HardwareIO::setLUT(){
mbedalvaro 31:5f039cbddee8 173
mbedalvaro 31:5f039cbddee8 174 FILE *fp = fopen(LUT_FILENAME, "r"); // Open file on the local file system for writing
mbedalvaro 31:5f039cbddee8 175 if(fp){
mbedalvaro 31:5f039cbddee8 176 //load the file into the lut table; keep the SAME resolution!
mbedalvaro 31:5f039cbddee8 177 fread(lut,sizeof(uint16),LUT_RESOLUTION*LUT_RESOLUTION,fp);
mbedalvaro 31:5f039cbddee8 178 fclose(fp);
mbedalvaro 31:5f039cbddee8 179 }
mbedalvaro 31:5f039cbddee8 180 else{
mbedalvaro 31:5f039cbddee8 181 //fclose(fp);
mbedalvaro 31:5f039cbddee8 182 //if the file "LUT.TXT" doesn't exist, create one with scanLUT()
mbedalvaro 31:5f039cbddee8 183 lockin.setLaserPower(true);
mbedalvaro 31:5f039cbddee8 184 scanLUT();
mbedalvaro 31:5f039cbddee8 185 }
mbedalvaro 31:5f039cbddee8 186
mbedalvaro 31:5f039cbddee8 187 }
mbedalvaro 31:5f039cbddee8 188
mbedalvaro 31:5f039cbddee8 189 //scan the total surface with a fixed 2^x resolution
mbedalvaro 31:5f039cbddee8 190 //create the Look-Up Table used to "flatten" the scan according to the position
mbedalvaro 31:5f039cbddee8 191 //
mbedalvaro 31:5f039cbddee8 192 //To Do: maybe detect high frequency to be sure the area is clean and empty?
mbedalvaro 31:5f039cbddee8 193 void HardwareIO::scanLUT(){
mbedalvaro 31:5f039cbddee8 194
mbedalvaro 31:5f039cbddee8 195 //reset lut table
mbedalvaro 31:5f039cbddee8 196 for(int j=0; j<LUT_RESOLUTION; j++){
mbedalvaro 31:5f039cbddee8 197 for(int i=0; i<LUT_RESOLUTION; i++){
mbedalvaro 31:5f039cbddee8 198 lut[i][j] =0;
mbedalvaro 31:5f039cbddee8 199 }
mbedalvaro 31:5f039cbddee8 200 }
mbedalvaro 31:5f039cbddee8 201
mbedalvaro 31:5f039cbddee8 202 int delayScanning = 300; //in us
mbedalvaro 31:5f039cbddee8 203
mbedalvaro 31:5f039cbddee8 204 //define the distance between each points (from 0 to 4096) and the offset (here 0)
mbedalvaro 31:5f039cbddee8 205 float shiftX = 1.0*(MAX_AD_MIRRORS - MIN_AD_MIRRORS) / (LUT_RESOLUTION-1);
mbedalvaro 31:5f039cbddee8 206 float shiftY = 1.0*(MAX_AD_MIRRORS - MIN_AD_MIRRORS) / (LUT_RESOLUTION-1);
mbedalvaro 31:5f039cbddee8 207 float offsetX = MIN_AD_MIRRORS;
mbedalvaro 31:5f039cbddee8 208 float offsetY = MIN_AD_MIRRORS;
mbedalvaro 31:5f039cbddee8 209
mbedalvaro 31:5f039cbddee8 210 //move the mirrors to the first position
mbedalvaro 31:5f039cbddee8 211 writeOutX(MAX_AD_MIRRORS);writeOutY(MIN_AD_MIRRORS);
mbedalvaro 31:5f039cbddee8 212 wait_us(500);
mbedalvaro 31:5f039cbddee8 213
mbedalvaro 31:5f039cbddee8 214 float x, y;
mbedalvaro 31:5f039cbddee8 215
mbedalvaro 31:5f039cbddee8 216 //scan the surface NB_SCANS times
mbedalvaro 31:5f039cbddee8 217 //the total value in lut[i][j] shouldn't exceed uint16 !!!
mbedalvaro 31:5f039cbddee8 218 for(int loop=0; loop<NB_SCANS; loop++){
mbedalvaro 31:5f039cbddee8 219 for(int j=0; j<LUT_RESOLUTION; j++){
mbedalvaro 31:5f039cbddee8 220 y = shiftY*j + offsetY ;
mbedalvaro 31:5f039cbddee8 221 writeOutY(int(y));
mbedalvaro 31:5f039cbddee8 222 //scan from right to left
mbedalvaro 31:5f039cbddee8 223 for(int i=LUT_RESOLUTION-1; i>=0; i--){
mbedalvaro 31:5f039cbddee8 224 x = shiftX*i + offsetX;
mbedalvaro 31:5f039cbddee8 225 writeOutX(int(x));
mbedalvaro 31:5f039cbddee8 226 wait_us(delayScanning);
mbedalvaro 31:5f039cbddee8 227 lut[i][j] += lockin_read();
mbedalvaro 31:5f039cbddee8 228 }
mbedalvaro 31:5f039cbddee8 229 //re-scan from left to right
mbedalvaro 31:5f039cbddee8 230 for(int i=0; i<LUT_RESOLUTION; i++){
mbedalvaro 31:5f039cbddee8 231 x = shiftX*i + offsetX;
mbedalvaro 31:5f039cbddee8 232 writeOutX(int(x));
mbedalvaro 31:5f039cbddee8 233 wait_us(delayScanning);
mbedalvaro 31:5f039cbddee8 234 lut[i][j] += lockin_read();
mbedalvaro 31:5f039cbddee8 235 }
mbedalvaro 31:5f039cbddee8 236 }
mbedalvaro 31:5f039cbddee8 237 }
mbedalvaro 31:5f039cbddee8 238
mbedalvaro 31:5f039cbddee8 239
mbedalvaro 31:5f039cbddee8 240 //save tab in file
mbedalvaro 31:5f039cbddee8 241 FILE *fp;
mbedalvaro 31:5f039cbddee8 242 #ifdef LUT_FILENAME
mbedalvaro 31:5f039cbddee8 243 fp = fopen(LUT_FILENAME, "w"); // Open file on the local file system for writing
mbedalvaro 31:5f039cbddee8 244 fwrite(lut,sizeof(uint16),LUT_RESOLUTION*LUT_RESOLUTION,fp);
mbedalvaro 31:5f039cbddee8 245 fclose(fp); //close the file (the mBed will appear connected again)
mbedalvaro 31:5f039cbddee8 246 #endif
mbedalvaro 31:5f039cbddee8 247
mbedalvaro 31:5f039cbddee8 248 #ifdef LUT_H_FILENAME
mbedalvaro 31:5f039cbddee8 249 //save tab in Human readable file (not used by the program, this is just for checking)
mbedalvaro 31:5f039cbddee8 250 // NOTE: we divide the content of the lut table by NB_SCANS, for easy reading (values should be between 0-4095)
mbedalvaro 31:5f039cbddee8 251 fp = fopen(LUT_H_FILENAME, "w"); // Open file on the local file system for writing
mbedalvaro 31:5f039cbddee8 252 fprintf(fp, "scan resolution: %d x %d\r\n",LUT_RESOLUTION, LUT_RESOLUTION);
mbedalvaro 31:5f039cbddee8 253 for(int j=0; j<LUT_RESOLUTION; j++){
mbedalvaro 31:5f039cbddee8 254 for(int i=0; i<LUT_RESOLUTION; i++){
mbedalvaro 31:5f039cbddee8 255 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) );
mbedalvaro 31:5f039cbddee8 256 }
mbedalvaro 31:5f039cbddee8 257 }
mbedalvaro 31:5f039cbddee8 258 fclose(fp); //close the file (the mBed will appear connected again)
mbedalvaro 31:5f039cbddee8 259 #endif
mbedalvaro 31:5f039cbddee8 260
mbedalvaro 31:5f039cbddee8 261 }
mbedalvaro 31:5f039cbddee8 262
mbedalvaro 31:5f039cbddee8 263
mbedalvaro 31:5f039cbddee8 264 //Return the lockin value "corrected with the Look-UpTable" - this means a RATIO between two reflectivities (and normally, this is <1).
mbedalvaro 31:5f039cbddee8 265 float HardwareIO::lockInCorrectedValue(unsigned short x, unsigned short y){
mbedalvaro 31:5f039cbddee8 266 //*******Correction using DIRECT approximation
mbedalvaro 31:5f039cbddee8 267 #ifdef LUT_DIRECT
mbedalvaro 31:5f039cbddee8 268 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)
mbedalvaro 31:5f039cbddee8 269 #endif
mbedalvaro 31:5f039cbddee8 270
mbedalvaro 31:5f039cbddee8 271 //*******Correction using BILINEAR approximation
mbedalvaro 31:5f039cbddee8 272 #ifdef LUT_BILINEAR
mbedalvaro 31:5f039cbddee8 273 unsigned short X = x >> LUT_BITS_SHIFT; //mirror "x" is 12bits, LUT "X" needs 4bits when lut is 17x17
mbedalvaro 31:5f039cbddee8 274 unsigned short Y = y >> LUT_BITS_SHIFT; //mirror "y" is 12bits, LUT "Y" needs 4bits when lut is 17x17
mbedalvaro 31:5f039cbddee8 275 float dx = 1.0*(x & LUT_BITS_MASK)/(LUT_BITS_MASK+1); //weight to apply on X (mask with 255 and norm)
mbedalvaro 31:5f039cbddee8 276 float dy = 1.0*(y & LUT_BITS_MASK)/(LUT_BITS_MASK+1); //weight to apply on Y (mask with 255 and norm)
mbedalvaro 31:5f039cbddee8 277
mbedalvaro 31:5f039cbddee8 278 //Wheighted mean approximation of the Look-Up Table at the position (x,y):
mbedalvaro 31:5f039cbddee8 279 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 31:5f039cbddee8 280
mbedalvaro 31:5f039cbddee8 281 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)
mbedalvaro 31:5f039cbddee8 282 #endif
mbedalvaro 31:5f039cbddee8 283
mbedalvaro 31:5f039cbddee8 284 //*******Correction using LINEAR approximation
mbedalvaro 31:5f039cbddee8 285 #ifdef LUT_LINEAR
mbedalvaro 31:5f039cbddee8 286 unsigned short X = x >> LUT_BITS_SHIFT; //mirror "x" is 12bits, LUT "X" needs 4bits when lut is 17x17
mbedalvaro 31:5f039cbddee8 287 unsigned short Y = y >> LUT_BITS_SHIFT; //mirror "y" is 12bits, LUT "Y" needs 4bits when lut is 17x17
mbedalvaro 31:5f039cbddee8 288 float dx = 1.0*(x & LUT_BITS_MASK)/(LUT_BITS_MASK+1); //weight to apply on X (mask with 255 and norm)
mbedalvaro 31:5f039cbddee8 289 float dy = 1.0*(y & LUT_BITS_MASK)/(LUT_BITS_MASK+1); //weight to apply on Y (mask with 255 and norm)
mbedalvaro 31:5f039cbddee8 290 float linearLUT, dzx, dzy;
mbedalvaro 31:5f039cbddee8 291
mbedalvaro 31:5f039cbddee8 292 if(dx>dy){ //if the position is on the "top-right" triangle
mbedalvaro 31:5f039cbddee8 293 dzx = (lut[X+1][Y] - lut[X][Y]) * dx;
mbedalvaro 31:5f039cbddee8 294 dzy = (lut[X+1][Y+1] - lut[X+1][Y]) * dy;
mbedalvaro 31:5f039cbddee8 295 }
mbedalvaro 31:5f039cbddee8 296 else{ //if the position is on the "bottom-left" triangle
mbedalvaro 31:5f039cbddee8 297 dzy = (lut[X][Y+1] - lut[X][Y]) * dy;
mbedalvaro 31:5f039cbddee8 298 dzx = (lut[X+1][Y+1] - lut[X][Y+1]) * dx;
mbedalvaro 31:5f039cbddee8 299 }
mbedalvaro 31:5f039cbddee8 300
mbedalvaro 31:5f039cbddee8 301 //linear approximation of the Look-Up Table at the position (x,y):
mbedalvaro 31:5f039cbddee8 302 linearLUT = lut[X][Y] + dzx + dzy;
mbedalvaro 31:5f039cbddee8 303 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)
mbedalvaro 31:5f039cbddee8 304
mbedalvaro 31:5f039cbddee8 305 #endif
mbedalvaro 31:5f039cbddee8 306
mbedalvaro 31:5f039cbddee8 307 //*******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)
mbedalvaro 31:5f039cbddee8 308 #ifdef NO_LUT
mbedalvaro 31:5f039cbddee8 309 return 1.0* lockin_read()/4096;
mbedalvaro 31:5f039cbddee8 310 #endif
mbedalvaro 31:5f039cbddee8 311
mbedalvaro 31:5f039cbddee8 312 }