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