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