Laser Sensing Display for UI interfaces in the real world
Fork of skinGames_forktest by
hardwareIO/lockin.cpp@47:199042980678, 2014-04-17 (annotated)
- Committer:
- mbedalvaro
- Date:
- Thu Apr 17 08:04:14 2014 +0000
- Revision:
- 47:199042980678
- Parent:
- 41:74e24a0e6e50
publishing for sharing with Ken Iwasaki
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mbedalvaro | 41:74e24a0e6e50 | 1 | #include "lockin.h" |
mbedalvaro | 41:74e24a0e6e50 | 2 | |
mbedalvaro | 41:74e24a0e6e50 | 3 | Lockin lockin=Lockin();//pre-instanciation of object lockin with inter-file scope (declared extern in .h file) |
mbedalvaro | 41:74e24a0e6e50 | 4 | |
mbedalvaro | 41:74e24a0e6e50 | 5 | |
mbedalvaro | 41:74e24a0e6e50 | 6 | // NOTE: the ADC interrupt catching function is not a method of the Lockin class, hence the use of the pre-instantiated object "lockin": |
mbedalvaro | 41:74e24a0e6e50 | 7 | void catchInterupt(uint32_t value){ |
mbedalvaro | 41:74e24a0e6e50 | 8 | lockin.buffer_pos=(lockin.buffer_pos+1)%BUFFER_SIZE; |
mbedalvaro | 41:74e24a0e6e50 | 9 | lockin.buffer[lockin.buffer_pos] = (value>>4)&0xFFF; // this is 12 bit precision ADC (0 to 4095), can be stored in an "unsigned short" (two bytes) |
mbedalvaro | 41:74e24a0e6e50 | 10 | } |
mbedalvaro | 41:74e24a0e6e50 | 11 | |
mbedalvaro | 41:74e24a0e6e50 | 12 | // PWM generation is configure as double edge |
mbedalvaro | 41:74e24a0e6e50 | 13 | // MR0 (Match Register 0) control the frequency |
mbedalvaro | 41:74e24a0e6e50 | 14 | // 'pwm2' uses MR1 and MR2 (rising and falling edges) |
mbedalvaro | 41:74e24a0e6e50 | 15 | // 'pwm4' uses MR3 and MR4 (rising and falling edges) |
mbedalvaro | 41:74e24a0e6e50 | 16 | // 'pwm1' and 'pwm3' cannot be used since they share the same Match Register |
mbedalvaro | 41:74e24a0e6e50 | 17 | // for the moment, all PWM pin are set as output: |
mbedalvaro | 41:74e24a0e6e50 | 18 | PwmOut pwm1(p26); |
mbedalvaro | 41:74e24a0e6e50 | 19 | PwmOut pwm2(LOCKIN_LASER_PIN); //USED: this is pin p25, the LOCKIN_LASER_PIN |
mbedalvaro | 41:74e24a0e6e50 | 20 | PwmOut pwm3(p24); |
mbedalvaro | 41:74e24a0e6e50 | 21 | PwmOut pwm4(LOCKIN_REF_PIN); //USED: this is pin p22, the LOCKIN_REF_PIN |
mbedalvaro | 41:74e24a0e6e50 | 22 | PwmOut pwm5(p22); |
mbedalvaro | 41:74e24a0e6e50 | 23 | PwmOut pwm6(p21); |
mbedalvaro | 41:74e24a0e6e50 | 24 | |
mbedalvaro | 41:74e24a0e6e50 | 25 | //Lockin::Lockin(){} |
mbedalvaro | 41:74e24a0e6e50 | 26 | |
mbedalvaro | 41:74e24a0e6e50 | 27 | void Lockin::init(){ |
mbedalvaro | 41:74e24a0e6e50 | 28 | |
mbedalvaro | 41:74e24a0e6e50 | 29 | //configure PWM for the laser and the Lockin |
mbedalvaro | 41:74e24a0e6e50 | 30 | refFreq = 147; |
mbedalvaro | 41:74e24a0e6e50 | 31 | offsetRef = 40; |
mbedalvaro | 41:74e24a0e6e50 | 32 | halfRefFreq = refFreq / 2; |
mbedalvaro | 41:74e24a0e6e50 | 33 | |
mbedalvaro | 41:74e24a0e6e50 | 34 | refFrequency = 653; //init the lock-in frequency at 653 kHz |
mbedalvaro | 41:74e24a0e6e50 | 35 | phaseShiftLaser = 0.546; //offset of 54% for the laser signal |
mbedalvaro | 41:74e24a0e6e50 | 36 | phaseShiftLockin = 0; //no offset for the lock-in reference |
mbedalvaro | 41:74e24a0e6e50 | 37 | initPWM(); |
mbedalvaro | 41:74e24a0e6e50 | 38 | |
mbedalvaro | 41:74e24a0e6e50 | 39 | //configure ADC: |
mbedalvaro | 41:74e24a0e6e50 | 40 | clearBuffer(); |
mbedalvaro | 41:74e24a0e6e50 | 41 | |
mbedalvaro | 41:74e24a0e6e50 | 42 | // SET ADC IN BURST MODE: |
mbedalvaro | 41:74e24a0e6e50 | 43 | lockin.setADC_forLockin(1); |
mbedalvaro | 41:74e24a0e6e50 | 44 | } |
mbedalvaro | 41:74e24a0e6e50 | 45 | |
mbedalvaro | 41:74e24a0e6e50 | 46 | void Lockin::setADC_forLockin(int mode) { |
mbedalvaro | 41:74e24a0e6e50 | 47 | if (mode>0) { |
mbedalvaro | 41:74e24a0e6e50 | 48 | adc.startmode(0,0); |
mbedalvaro | 41:74e24a0e6e50 | 49 | adc.burst(1); |
mbedalvaro | 41:74e24a0e6e50 | 50 | adc.setup(LOCKIN_ADC_PIN, 1); |
mbedalvaro | 41:74e24a0e6e50 | 51 | adc.select(LOCKIN_ADC_PIN); |
mbedalvaro | 41:74e24a0e6e50 | 52 | adc.interrupt_state(LOCKIN_ADC_PIN, 1); |
mbedalvaro | 41:74e24a0e6e50 | 53 | adc.append(LOCKIN_ADC_PIN, catchInterupt); |
mbedalvaro | 41:74e24a0e6e50 | 54 | } else { |
mbedalvaro | 41:74e24a0e6e50 | 55 | //adc.startmode(0,0); |
mbedalvaro | 41:74e24a0e6e50 | 56 | adc.burst(0); |
mbedalvaro | 41:74e24a0e6e50 | 57 | adc.setup(LOCKIN_ADC_PIN, 0); |
mbedalvaro | 41:74e24a0e6e50 | 58 | //adc.select(LOCKIN_ADC_PIN); |
mbedalvaro | 41:74e24a0e6e50 | 59 | adc.interrupt_state(LOCKIN_ADC_PIN, 0); |
mbedalvaro | 41:74e24a0e6e50 | 60 | //adc.append(LOCKIN_ADC_PIN, catchInterupt); |
mbedalvaro | 41:74e24a0e6e50 | 61 | } |
mbedalvaro | 41:74e24a0e6e50 | 62 | } |
mbedalvaro | 41:74e24a0e6e50 | 63 | |
mbedalvaro | 41:74e24a0e6e50 | 64 | void Lockin::initPWM(){ |
mbedalvaro | 41:74e24a0e6e50 | 65 | |
mbedalvaro | 41:74e24a0e6e50 | 66 | float halfPeriod = 0.5 * MBEDFREQUENCY / refFrequency; // half shared periof |
mbedalvaro | 41:74e24a0e6e50 | 67 | _currentMR[0] = int(1.0 * MBEDFREQUENCY / refFrequency); //save the current value of MR0 (shared periof) //147 |
mbedalvaro | 41:74e24a0e6e50 | 68 | _currentMR[1] = int(phaseShiftLaser * halfPeriod); //save the current value of MR1 //40 |
mbedalvaro | 41:74e24a0e6e50 | 69 | _currentMR[2] = int(_currentMR[1] + halfPeriod); //save the current value of MR2 //40+73 |
mbedalvaro | 41:74e24a0e6e50 | 70 | _currentMR[3] = int(phaseShiftLockin * halfPeriod); //save the current value of MR1 //0 |
mbedalvaro | 41:74e24a0e6e50 | 71 | _currentMR[4] = int(_currentMR[3] + halfPeriod); //save the current value of MR2 //73 |
mbedalvaro | 41:74e24a0e6e50 | 72 | |
mbedalvaro | 41:74e24a0e6e50 | 73 | |
mbedalvaro | 41:74e24a0e6e50 | 74 | // set PWM: |
mbedalvaro | 41:74e24a0e6e50 | 75 | LPC_PWM1->TCR = (1 << 1); // Reset counter, disable PWM |
mbedalvaro | 41:74e24a0e6e50 | 76 | LPC_SC->PCLKSEL0 &= ~(0x3 << 12); |
mbedalvaro | 41:74e24a0e6e50 | 77 | LPC_SC->PCLKSEL0 |= (1 << 12); // Set peripheral clock divider to /1, i.e. system clock |
mbedalvaro | 41:74e24a0e6e50 | 78 | |
mbedalvaro | 41:74e24a0e6e50 | 79 | LPC_PWM1->PCR |= 0x0014; // Double edge PWM for PWM2,4 |
mbedalvaro | 41:74e24a0e6e50 | 80 | |
mbedalvaro | 41:74e24a0e6e50 | 81 | LPC_PWM1->MR0 = _currentMR[0]; // Match Register 0 is shared period counter for all PWM1 |
mbedalvaro | 41:74e24a0e6e50 | 82 | |
mbedalvaro | 41:74e24a0e6e50 | 83 | LPC_PWM1->MR1 = _currentMR[1]; // Match Register 1 is laser rising edge counter |
mbedalvaro | 41:74e24a0e6e50 | 84 | LPC_PWM1->MR2 = _currentMR[2]; // Match Register 2 is laser falling edge counter |
mbedalvaro | 41:74e24a0e6e50 | 85 | LPC_PWM1->MR3 = _currentMR[3]; // Match Register 3 is lock-in rising edge counter |
mbedalvaro | 41:74e24a0e6e50 | 86 | LPC_PWM1->MR4 = _currentMR[4]; // Match Register 4 is lock-in falling edge counter |
mbedalvaro | 41:74e24a0e6e50 | 87 | |
mbedalvaro | 41:74e24a0e6e50 | 88 | LPC_PWM1->LER |= 1; // Start updating at next period start |
mbedalvaro | 41:74e24a0e6e50 | 89 | LPC_PWM1->TCR = (1 << 0) || (1 << 3); // Enable counter and PWM |
mbedalvaro | 41:74e24a0e6e50 | 90 | } |
mbedalvaro | 41:74e24a0e6e50 | 91 | |
mbedalvaro | 41:74e24a0e6e50 | 92 | //change the frequency of the PWM after initPWM() |
mbedalvaro | 41:74e24a0e6e50 | 93 | void Lockin::setPWMFrequency(float freq){ |
mbedalvaro | 41:74e24a0e6e50 | 94 | refFrequency = freq; |
mbedalvaro | 41:74e24a0e6e50 | 95 | _currentMR[0] = int(MBEDFREQUENCY / refFrequency); //save the current value of MR0 |
mbedalvaro | 41:74e24a0e6e50 | 96 | LPC_PWM1->MR0 = _currentMR[0]; //update PWM shared period register |
mbedalvaro | 41:74e24a0e6e50 | 97 | LPC_PWM1->LER |= 1; //update PWM |
mbedalvaro | 41:74e24a0e6e50 | 98 | } |
mbedalvaro | 41:74e24a0e6e50 | 99 | |
mbedalvaro | 41:74e24a0e6e50 | 100 | //change the phase shift of the sensing laser after initPWM() |
mbedalvaro | 41:74e24a0e6e50 | 101 | void Lockin::setLaserPhaseShift(float phaseShift){ |
mbedalvaro | 41:74e24a0e6e50 | 102 | phaseShiftLaser = phaseShift; |
mbedalvaro | 41:74e24a0e6e50 | 103 | float halfPeriod = 0.5 * MBEDFREQUENCY / refFrequency; |
mbedalvaro | 41:74e24a0e6e50 | 104 | _currentMR[1] = int(phaseShiftLaser * halfPeriod); //save the current value of MR1 |
mbedalvaro | 41:74e24a0e6e50 | 105 | _currentMR[2] = _currentMR[1] + halfPeriod; //save the current value of MR2 |
mbedalvaro | 41:74e24a0e6e50 | 106 | |
mbedalvaro | 41:74e24a0e6e50 | 107 | LPC_PWM1->MR1 = _currentMR[1]; //update Laser rising edge match register |
mbedalvaro | 41:74e24a0e6e50 | 108 | LPC_PWM1->MR2 = _currentMR[2]; //update Laser faling edge match register |
mbedalvaro | 41:74e24a0e6e50 | 109 | } |
mbedalvaro | 41:74e24a0e6e50 | 110 | |
mbedalvaro | 41:74e24a0e6e50 | 111 | //change the phase shift of the lock-in after initPWM() |
mbedalvaro | 41:74e24a0e6e50 | 112 | void Lockin::setLockinPhaseShift(float phaseShift){ |
mbedalvaro | 41:74e24a0e6e50 | 113 | phaseShiftLockin = phaseShift; |
mbedalvaro | 41:74e24a0e6e50 | 114 | float halfPeriod = 0.5 * MBEDFREQUENCY / refFrequency; |
mbedalvaro | 41:74e24a0e6e50 | 115 | _currentMR[3] = int(phaseShiftLockin * halfPeriod); //save the current value of MR1 |
mbedalvaro | 41:74e24a0e6e50 | 116 | _currentMR[4] = _currentMR[3] + halfPeriod; //save the current value of MR2 |
mbedalvaro | 41:74e24a0e6e50 | 117 | |
mbedalvaro | 41:74e24a0e6e50 | 118 | LPC_PWM1->MR3 = _currentMR[3]; //update lock-in rising edge match register |
mbedalvaro | 41:74e24a0e6e50 | 119 | LPC_PWM1->MR4 = _currentMR[4]; //update lock-in faling edge match register |
mbedalvaro | 41:74e24a0e6e50 | 120 | } |
mbedalvaro | 41:74e24a0e6e50 | 121 | |
mbedalvaro | 41:74e24a0e6e50 | 122 | |
mbedalvaro | 41:74e24a0e6e50 | 123 | void Lockin::setLaserPower(bool power){ |
mbedalvaro | 41:74e24a0e6e50 | 124 | if(power){ |
mbedalvaro | 41:74e24a0e6e50 | 125 | LPC_PWM1->MR1 = _currentMR[1]; |
mbedalvaro | 41:74e24a0e6e50 | 126 | LPC_PWM1->MR2 = _currentMR[2]; |
mbedalvaro | 41:74e24a0e6e50 | 127 | LPC_PWM1->LER |= 1; // update PWM at the next period |
mbedalvaro | 41:74e24a0e6e50 | 128 | } |
mbedalvaro | 41:74e24a0e6e50 | 129 | else{ |
mbedalvaro | 41:74e24a0e6e50 | 130 | LPC_PWM1->MR1 = 0; //set rising edge at 0 |
mbedalvaro | 41:74e24a0e6e50 | 131 | LPC_PWM1->MR2 = 0; //set falling edge at 0 |
mbedalvaro | 41:74e24a0e6e50 | 132 | LPC_PWM1->LER |= 1; // update PWM at the next period |
mbedalvaro | 41:74e24a0e6e50 | 133 | } |
mbedalvaro | 41:74e24a0e6e50 | 134 | } |
mbedalvaro | 41:74e24a0e6e50 | 135 | |
mbedalvaro | 41:74e24a0e6e50 | 136 | void Lockin::clearBuffer(){ |
mbedalvaro | 41:74e24a0e6e50 | 137 | for(int i=0; i<BUFFER_SIZE; i++){ |
mbedalvaro | 41:74e24a0e6e50 | 138 | buffer[i] = 0; |
mbedalvaro | 41:74e24a0e6e50 | 139 | } |
mbedalvaro | 41:74e24a0e6e50 | 140 | buffer_pos = BUFFER_SIZE; |
mbedalvaro | 41:74e24a0e6e50 | 141 | } |
mbedalvaro | 41:74e24a0e6e50 | 142 | |
mbedalvaro | 41:74e24a0e6e50 | 143 | /* |
mbedalvaro | 41:74e24a0e6e50 | 144 | void Lockin::catchInterupt(uint32_t value){ |
mbedalvaro | 41:74e24a0e6e50 | 145 | buffer_pos++; |
mbedalvaro | 41:74e24a0e6e50 | 146 | buffer_pos%=BUFFER_SIZE; |
mbedalvaro | 41:74e24a0e6e50 | 147 | buffer[buffer_pos] = value; |
mbedalvaro | 41:74e24a0e6e50 | 148 | } |
mbedalvaro | 41:74e24a0e6e50 | 149 | */ |
mbedalvaro | 41:74e24a0e6e50 | 150 | |
mbedalvaro | 41:74e24a0e6e50 | 151 | //****** aquisition method *****// |
mbedalvaro | 41:74e24a0e6e50 | 152 | unsigned short Lockin::getLastValue(){ |
mbedalvaro | 41:74e24a0e6e50 | 153 | return buffer[buffer_pos]; |
mbedalvaro | 41:74e24a0e6e50 | 154 | } |
mbedalvaro | 41:74e24a0e6e50 | 155 | |
mbedalvaro | 41:74e24a0e6e50 | 156 | unsigned short Lockin::getSmoothValue(){ |
mbedalvaro | 41:74e24a0e6e50 | 157 | unsigned short smoothValue = buffer[0]; |
mbedalvaro | 41:74e24a0e6e50 | 158 | for(int i=1; i<BUFFER_SIZE; i++){ |
mbedalvaro | 41:74e24a0e6e50 | 159 | smoothValue += buffer[i]; |
mbedalvaro | 41:74e24a0e6e50 | 160 | } |
mbedalvaro | 41:74e24a0e6e50 | 161 | smoothValue = (unsigned short)(smoothValue/BUFFER_SIZE); // note: we could have more precision (sub-12 bit), but it's not required and would imply using a float as output |
mbedalvaro | 41:74e24a0e6e50 | 162 | |
mbedalvaro | 41:74e24a0e6e50 | 163 | return smoothValue; |
mbedalvaro | 41:74e24a0e6e50 | 164 | } |
mbedalvaro | 41:74e24a0e6e50 | 165 | |
mbedalvaro | 41:74e24a0e6e50 | 166 | unsigned short Lockin::getMedianValue(){ |
mbedalvaro | 41:74e24a0e6e50 | 167 | //this method applies a median filter to the buffer |
mbedalvaro | 41:74e24a0e6e50 | 168 | //It reduces the salt-and-pepper noise |
mbedalvaro | 41:74e24a0e6e50 | 169 | //It seems that this noise is very strong on certain mBed board, but not all... |
mbedalvaro | 41:74e24a0e6e50 | 170 | |
mbedalvaro | 41:74e24a0e6e50 | 171 | // unsigned short orderedBuffer[BUFFER_SIZE_MEDIAN]; |
mbedalvaro | 41:74e24a0e6e50 | 172 | |
mbedalvaro | 41:74e24a0e6e50 | 173 | //sort half of the buffer: |
mbedalvaro | 41:74e24a0e6e50 | 174 | |
mbedalvaro | 41:74e24a0e6e50 | 175 | //copy buffer |
mbedalvaro | 41:74e24a0e6e50 | 176 | for(int i=0; i<BUFFER_SIZE_MEDIAN; i++){ |
mbedalvaro | 41:74e24a0e6e50 | 177 | orderedBuffer[i] = buffer[(buffer_pos+BUFFER_SIZE-i+DELAY_BUFFER_MEDIAN)%BUFFER_SIZE]; |
mbedalvaro | 41:74e24a0e6e50 | 178 | } |
mbedalvaro | 41:74e24a0e6e50 | 179 | |
mbedalvaro | 41:74e24a0e6e50 | 180 | //order buffer |
mbedalvaro | 41:74e24a0e6e50 | 181 | for(int i=0; i<BUFFER_SIZE_MEDIAN-1; i++){ |
mbedalvaro | 41:74e24a0e6e50 | 182 | int minPos = i; |
mbedalvaro | 41:74e24a0e6e50 | 183 | |
mbedalvaro | 41:74e24a0e6e50 | 184 | //get min |
mbedalvaro | 41:74e24a0e6e50 | 185 | for(int j=i+1; j<BUFFER_SIZE_MEDIAN; j++){ |
mbedalvaro | 41:74e24a0e6e50 | 186 | if(orderedBuffer[j] < orderedBuffer[minPos]) minPos = j; |
mbedalvaro | 41:74e24a0e6e50 | 187 | } |
mbedalvaro | 41:74e24a0e6e50 | 188 | |
mbedalvaro | 41:74e24a0e6e50 | 189 | //swap min to the right position |
mbedalvaro | 41:74e24a0e6e50 | 190 | if(minPos != i){ |
mbedalvaro | 41:74e24a0e6e50 | 191 | int tmpMin = orderedBuffer[minPos]; |
mbedalvaro | 41:74e24a0e6e50 | 192 | orderedBuffer[minPos] = orderedBuffer[i]; |
mbedalvaro | 41:74e24a0e6e50 | 193 | orderedBuffer[i] = tmpMin; |
mbedalvaro | 41:74e24a0e6e50 | 194 | } |
mbedalvaro | 41:74e24a0e6e50 | 195 | } |
mbedalvaro | 41:74e24a0e6e50 | 196 | |
mbedalvaro | 41:74e24a0e6e50 | 197 | return orderedBuffer[BUFFER_SIZE_MEDIAN/2]; |
mbedalvaro | 41:74e24a0e6e50 | 198 | } |
mbedalvaro | 41:74e24a0e6e50 | 199 |