Mercurial > hg > beaglert

comparison projects/d-box/sensors.cpp @ 0:8a575ba3ab52

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Initial commit.
author andrewm
date Fri, 31 Oct 2014 19:10:17 +0100
parents
children 901d205d1a3c
comparison
equal deleted inserted replaced
-1:000000000000 0:8a575ba3ab52
1 /*
2 * sensors.cpp
3 *
4 * Created on: May 28, 2014
5 * Author: Victor Zappi
6 */
7
8 #include <stdio.h>
9 #include <pthread.h>
10 #include <unistd.h>
11 #include <math.h>
12 #include <vector>
13 #include "prio.h"
14 #include "sensors.h"
15 #include "OscillatorBank.h"
16 #include "DboxSensors.h"
17
18
19 //----------------------------------------
20 // main extern variables
21 //----------------------------------------
22 extern vector<OscillatorBank*> gOscBanks;
23 extern int gCurrentOscBank;
24 extern int gNextOscBank;
25 extern bool gShouldStop;
26 extern int gVerbose;
27
28 float gSensor0LatestTouchPos = 0; // most recent pitch touch location [0-1] on sensor 0, used by render.cpp
29 int gSensor0LatestTouchNum = 0; // most recent num of touches on sensor 0, used by render.cpp
30 float gSensor1LatestTouchPos[5]; // most recent touche locations on sensor 1, used by render.cpp
31 //float gSensor1LatestTouchSizes[5];
32 int gSensor1LatestTouchCount; // most recent number touches on sensor 1, used by render.cpp
33 int gSensor1LatestTouchIndex = 0; // index of last touch in gSensor1LatestTouchPos[5], used by render.cpp
34 int gLastFSRValue = 1799; // most recent fsr value, used by render.cpp
35
36
37 DboxSensors Sensors;
38
39
40 //----------------------------------------
41 // var shared with logger
42 //----------------------------------------
43 int s0TouchNum = 0;
44 float s0Touches_[MAX_TOUCHES];
45 float s0Size_[MAX_TOUCHES];
46 int s0LastIndex;
47
48 int s1TouchNum = 0;
49 float s1Touches_[MAX_TOUCHES];
50 float s1Size_[MAX_TOUCHES];
51 int s1LastIndex;
52
53 int fsr = 1799;
54
55
56
57 using namespace std;
58
59 int initSensorLoop(int sensorAddress0, int sensorAddress1, bool useNewSensors)
60 {
61 int tk0_bus = 1;
62 int tk0_address = sensorAddress0;
63 int tk1_bus = 1;
64 int tk1_address = sensorAddress1;
65 int tk_file = 0;
66 int fsr_max = 1799;
67 int fsr_pinNum = 4;
68
69 if(gVerbose==1)
70 cout << "---------------->Init Control Thread" << endl;
71
72 if(Sensors.initSensors(tk0_bus, tk0_address, tk1_bus, tk1_address, tk_file, fsr_pinNum, fsr_max, useNewSensors)>0)
73 {
74 gShouldStop = 1;
75 cout << "control cannot start" << endl;
76 return -1;
77 }
78
79 for(int i=0; i<MAX_TOUCHES; i++)
80 {
81 s0Touches_[i] = 0.0;
82 s0Size_[i] = 0.0;
83
84 s1Touches_[i] = 0.0;
85 s1Size_[i] = 0.0;
86 }
87
88 return 0;
89 }
90
91 void sensorLoop(void *)
92 {
93 timeval start, end;
94 unsigned long elapsedTime;
95 //float touchSize = 0; // once used for timbre
96
97
98
99 float *s0Touches;
100 float *s0Size;
101 int s0PrevTouchNum = 0;
102 int s0SortedTouchIndices[MAX_TOUCHES];
103 float s0SortedTouches[MAX_TOUCHES];
104 float s0PrevSortedTouches[MAX_TOUCHES];
105
106 float *s1Touches;
107 float *s1Size;
108 int s1PrevTouchNum = 0;
109 int s1SortedTouchIndices[MAX_TOUCHES];
110 float s1SortedTouches[MAX_TOUCHES];
111 float s1PrevSortedTouches[MAX_TOUCHES];
112
113 float freqScaler = 0;
114 int fsrMin = 0;//50; // was 200
115 int fsrMax = 1799;//1300; // was 800
116 float vel = 0;
117 float prevVel = 0;
118 float filterMaxF = 0;
119 if(gVerbose==1)
120 dbox_printf("__________set Control Thread priority\n");
121
122 if(gVerbose==1)
123 dbox_printf("_________________Control Thread!\n");
124
125 // get freq scaler, cos freqs must be scaled according to the wavetable used in the oscillator bank
126 freqScaler = gOscBanks[gCurrentOscBank]->getFrequencyScaler();
127 filterMaxF = gOscBanks[gCurrentOscBank]->filterMaxF;
128
129 // init time vals
130 gettimeofday(&start, NULL);
131
132 // here we go, sensor loop until the end of the application
133 while(!gShouldStop)
134 {
135 gettimeofday(&end, NULL);
136 elapsedTime = ( (end.tv_sec*1000000+end.tv_usec) - (start.tv_sec*1000000+start.tv_usec) );
137 if( elapsedTime<4000 )
138 usleep(4000-elapsedTime);
139 else
140 dbox_printf("%d\n", (int)elapsedTime); // this print happens when something's gone bad...
141
142 if(Sensors.readSensors()==0)
143 {
144 s0TouchNum = Sensors.getTKTouchCount(0);
145 s0Touches = Sensors.getTKXPositions(0);
146 s0Size = Sensors.getTKTouchSize(0);
147
148 s1TouchNum = Sensors.getTKTouchCount(1);
149 s1Touches = Sensors.getTKXPositions(1);
150 s1Size = Sensors.getTKTouchSize(1);
151
152 for(int i=0; i<MAX_TOUCHES; i++)
153 {
154 s0Touches_[i] = s0Touches[i];
155 s0Size_[i] = s0Size[i];
156
157 s1Touches_[i] = s1Touches[i];
158 s1Size_[i] = s1Size[i];
159 }
160
161 gSensor0LatestTouchNum = s0TouchNum;
162 if(s0TouchNum > 0)
163 {
164 //-----------------------------------------------------------------------------------
165 // timbre, speed and pitch
166 //touchSize = 0; \\ once used for timbre
167
168 // if we have a number of touches different from previous round, track their order of arrival [calculated using distance comparison]
169 if(s0PrevTouchNum!=s0TouchNum)
170 {
171 float distances[MAX_TOUCHES*(MAX_TOUCHES-1)]; // maximum number of current+previous touches between rounds with different num of touches
172 int ids[MAX_TOUCHES*(MAX_TOUCHES-1)];
173 // calculate all distance permutations between previous and current touches
174 for(int i=0; i<s0TouchNum; i++)
175 {
176 for(int p=0; p<s0PrevTouchNum; p++)
177 {
178 int index = i*s0PrevTouchNum+p; // permutation id [says between which touches we are calculating distance]
179 distances[index] = fabs(s0Touches[i]-s0PrevSortedTouches[p]);
180 ids[index] = index;
181 if(index>0)
182 {
183 // sort, from min to max distance
184 float tmp;
185 while(distances[index]<distances[index-1])
186 {
187 tmp = ids[index-1];
188 ids[index-1] = ids[index];
189 ids[index] = tmp;
190
191 tmp = distances[index-1];
192 distances[index-1] = distances[index];
193 distances[index] = tmp;
194
195 index--;
196
197 if(index == 0)
198 break;
199 }
200 }
201 }
202 }
203
204 int sorted = 0;
205 bool currAssigned[MAX_TOUCHES] = {false};
206 bool prevAssigned[MAX_TOUCHES] = {false};
207
208 // track touches assigning index according to shortest distance
209 for(int i=0; i<s0TouchNum*s0PrevTouchNum; i++)
210 {
211 int currentIndex = ids[i]/s0PrevTouchNum;
212 int prevIndex = ids[i]%s0PrevTouchNum;
213 // avoid double assignment
214 if(!currAssigned[currentIndex] && !prevAssigned[prevIndex])
215 {
216 currAssigned[currentIndex] = true;
217 prevAssigned[prevIndex] = true;
218 s0SortedTouchIndices[currentIndex] = prevIndex;
219 sorted++;
220 }
221 }
222 // we still have to assign a free index to new touches
223 if(s0PrevTouchNum<s0TouchNum)
224 {
225 for(int i=0; i<s0TouchNum; i++)
226 {
227 if(!currAssigned[i])
228 s0SortedTouchIndices[i] = sorted++; // assign next free index
229
230 // update tracked value
231 s0SortedTouches[s0SortedTouchIndices[i]] = s0Touches[i];
232 s0PrevSortedTouches[i] = s0SortedTouches[i];
233 if(s0SortedTouchIndices[i]==s0TouchNum-1)
234 s0LastIndex = i;
235
236 // accumulate sizes for timbre
237 //touchSize += s0Size[i];
238 }
239 }
240 else // some touches have disappeared...
241 {
242 // ...we have to shift all indices...
243 for(int i=s0PrevTouchNum-1; i>=0; i--)
244 {
245 if(!prevAssigned[i])
246 {
247 for(int j=0; j<s0TouchNum; j++)
248 {
249 // ...only if touches that disappeared were before the current one
250 if(s0SortedTouchIndices[j]>i)
251 s0SortedTouchIndices[j]--;
252 }
253 }
254 }
255 // done! now update
256 for(int i=0; i<s0TouchNum; i++)
257 {
258 // update tracked value
259 s0SortedTouches[s0SortedTouchIndices[i]] = s0Touches[i];
260 s0PrevSortedTouches[i] = s0SortedTouches[i];
261 if(s0SortedTouchIndices[i]==s0TouchNum-1)
262 s0LastIndex = i;
263
264 // accumulate sizes for timbre
265 //touchSize += s0Size[i];
266 }
267 }
268 }
269 else // nothing's changed since last round
270 {
271 for(int i=0; i<s0TouchNum; i++)
272 {
273 // update tracked value
274 s0SortedTouches[s0SortedTouchIndices[i]] = s0Touches[i];
275 s0PrevSortedTouches[i] = s0SortedTouches[i];
276
277 // accumulate sizes for timbre
278 //touchSize += s0Size[i];
279 }
280 }
281
282 if(s0TouchNum == 0)
283 s0LastIndex = -1;
284
285 // timbre
286 //touchSize = (touchSize > 0.7) ? 1 : touchSize/0.7;
287 //gOscBanks[gCurrentOscBank]->hopNumTh = log((1-touchSize)+1)/log(2)*20000;
288 //gOscBanks[gCurrentOscBank]->hopNumTh = 0;
289
290
291 // pitch, controlled by last touch
292 //prevTouchPos = touch[touchIndex];
293 //touchPos = (s0SortedTouches[s0TouchNum-1]-0.5)/0.5; // from [0,1] to [-1,1]
294 gSensor0LatestTouchPos = s0SortedTouches[s0TouchNum-1];
295 //touchPos = s0Touches[0];
296 //gOscBanks[gCurrentOscBank]->pitchMultiplier = pow(2, touchPos);
297 //-----------------------------------------------------------------------------------
298
299
300
301 //-----------------------------------------------------------------------------------
302 // note on
303 //if(s0PrevTouchNum == 0)
304 // gOscBanks[gCurrentOscBank]->play();
305 // fsr = Sensors.getFSRVAlue();
306 fsr = gLastFSRValue;
307 //dbox_printf("fsr: %d\n", fsr);
308 if(!gOscBanks[gCurrentOscBank]->note)
309 {
310 vel = fsr;
311 vel /= (float)(fsrMax-fsrMin);
312
313 vel = 1-vel;
314 dbox_printf("Attack vel: %f\n", vel);
315 gOscBanks[gCurrentOscBank]->play(vel);
316 prevVel = vel;
317 }
318 else if(gOscBanks[gCurrentOscBank]->getEnvelopeState() != env_release)
319 {
320 fsr = (fsr > fsrMax) ? fsrMax : fsr;
321 vel = (fsr < fsrMin) ? fsrMin : fsr;
322 vel -= fsrMin;
323 vel /= (float)(fsrMax-fsrMin);
324 vel = 1-vel;
325 if(vel > prevVel)
326 {
327 gOscBanks[gCurrentOscBank]->afterTouch(vel);
328 prevVel = vel;
329 }
330 }
331 //-----------------------------------------------------------------------------------
332 }
333 else
334 {
335 //prevFsr = 1799;
336 //prevTouchPos = -1;
337 //-----------------------------------------------------------------------------------
338 // note off
339 if(s0PrevTouchNum > 0)
340 {
341 if(gOscBanks[gCurrentOscBank]->state==bank_playing)
342 gOscBanks[gCurrentOscBank]->stop();
343 }
344 //-----------------------------------------------------------------------------------
345 }
346
347
348
349 // sensor 2
350 //-----------------------------------------------------------------------------------
351 //filter - calculated even when no touches on first sensor, to filter also release tail
352 gOscBanks[gCurrentOscBank]->filterNum = s1TouchNum;
353
354 gSensor1LatestTouchCount = gOscBanks[gCurrentOscBank]->filterNum;
355 for(int i = 0; i < gSensor1LatestTouchCount; i++) {
356 gSensor1LatestTouchPos[i] = s1Touches[i];
357 //gSensor1LatestTouchSizes[i] = s1Size[i];
358 }
359
360 /* for(int i=0; i<gOscBanks[gCurrentOscBank]->filterNum; i++)
361 {
362 // touch pos is linear but freqs are log
363 gOscBanks[gCurrentOscBank]->filterFreqs[i] = ((exp(s0Touches[i]*4)-1)/(exp(4)-1))*filterMaxF*freqScaler;
364 //gOscBanks[gCurrentOscBank]->filterQ[i] = size[i]*5*(1+touch[i]*1000)*freqScaler;
365 gOscBanks[gCurrentOscBank]->filterQ[i] = s0Size[i];
366 if(gOscBanks[gCurrentOscBank]->filterFreqs[i]>500*freqScaler)
367 gOscBanks[gCurrentOscBank]->filterPadding[i] = 1+100000*( (gOscBanks[gCurrentOscBank]->filterFreqs[i]-500*freqScaler)/(filterMaxF-500)*freqScaler );
368 else
369 gOscBanks[gCurrentOscBank]->filterPadding[i] = 1;
370 }*/
371
372 // each touch on sensor 2 is a notch filter, whose Q is determined by touch size
373 for(int i=0; i<gOscBanks[gCurrentOscBank]->filterNum; i++)
374 {
375 // map touch pos [which is linear] on freqs exponentially
376 float freq = ((exp(s1Touches[i]*4)-1)/EXP_DENOM)*filterMaxF;
377 gOscBanks[gCurrentOscBank]->filterFreqs[i] = freq*freqScaler;
378 // also size is mapped exponentially on Q
379 float siz = (exp(s1Size[i])-1)/1.71828;
380 gOscBanks[gCurrentOscBank]->filterQ[i] = siz*( (filterMaxF-freq)/filterMaxF * 0.9 + 0.1 ); // size weight on Q decreases with frequency
381 }
382 //-----------------------------------------------------------------------------------
383
384
385
386 //-----------------------------------------------------------------------------------
387 // sort touches on sensor 2
388 if(s1TouchNum > 0)
389 {
390 // if we have a number of touches different from previous round, track their order of arrival [calculated using distance comparison]
391 if(s1PrevTouchNum!=s1TouchNum)
392 {
393 float distances[MAX_TOUCHES*(MAX_TOUCHES-1)]; // maximum number of current+previous touches between rounds with different num of touches
394 int ids[MAX_TOUCHES*(MAX_TOUCHES-1)];
395 // calculate all distance permutations between previous and current touches
396 for(int i=0; i<s1TouchNum; i++)
397 {
398 for(int p=0; p<s1PrevTouchNum; p++)
399 {
400 int index = i*s1PrevTouchNum+p; // permutation id [says between which touches we are calculating distance]
401 distances[index] = fabs(s1Touches[i]-s1PrevSortedTouches[p]);
402 ids[index] = index;
403 if(index>0)
404 {
405 // sort, from min to max distance
406 float tmp;
407 while(distances[index]<distances[index-1])
408 {
409 tmp = ids[index-1];
410 ids[index-1] = ids[index];
411 ids[index] = tmp;
412
413 tmp = distances[index-1];
414 distances[index-1] = distances[index];
415 distances[index] = tmp;
416
417 index--;
418
419 if(index == 0)
420 break;
421 }
422 }
423 }
424 }
425
426 int sorted = 0;
427 bool currAssigned[MAX_TOUCHES] = {false};
428 bool prevAssigned[MAX_TOUCHES] = {false};
429
430 // track touches assigning index according to shortest distance
431 for(int i=0; i<s1TouchNum*s1PrevTouchNum; i++)
432 {
433 int currentIndex = ids[i]/s1PrevTouchNum;
434 int prevIndex = ids[i]%s1PrevTouchNum;
435 // avoid double assignment
436 if(!currAssigned[currentIndex] && !prevAssigned[prevIndex])
437 {
438 currAssigned[currentIndex] = true;
439 prevAssigned[prevIndex] = true;
440 s1SortedTouchIndices[currentIndex] = prevIndex;
441 sorted++;
442 }
443 }
444 // we still have to assign a free index to new touches
445 if(s1PrevTouchNum<s1TouchNum)
446 {
447 for(int i=0; i<s1TouchNum; i++)
448 {
449 if(!currAssigned[i])
450 s1SortedTouchIndices[i] = sorted++; // assign next free index
451
452 // update tracked value
453 s1SortedTouches[s1SortedTouchIndices[i]] = s1Touches[i];
454 s1PrevSortedTouches[i] = s1SortedTouches[i];
455 if(s1SortedTouchIndices[i]==s1TouchNum-1)
456 s1LastIndex = i;
457 }
458 }
459 else // some touches have disappeared...
460 {
461 // ...we have to shift all indices...
462 for(int i=s1PrevTouchNum-1; i>=0; i--)
463 {
464 if(!prevAssigned[i])
465 {
466 for(int j=0; j<s1TouchNum; j++)
467 {
468 // ...only if touches that disappeared were before the current one
469 if(s1SortedTouchIndices[j]>i)
470 s1SortedTouchIndices[j]--;
471 }
472 }
473 }
474 // done! now update
475 for(int i=0; i<s1TouchNum; i++)
476 {
477 // update tracked value
478 s1SortedTouches[s1SortedTouchIndices[i]] = s1Touches[i];
479 s1PrevSortedTouches[i] = s1SortedTouches[i];
480 if(s1SortedTouchIndices[i]==s1TouchNum-1)
481 s1LastIndex = i;
482 }
483 }
484 }
485 else // nothing's changed since last round
486 {
487 for(int i=0; i<s1TouchNum; i++)
488 {
489 // update tracked value
490 s1SortedTouches[s1SortedTouchIndices[i]] = s1Touches[i];
491 s1PrevSortedTouches[i] = s1SortedTouches[i];
492 }
493 }
494 }
495
496 if(s1TouchNum > 0)
497 {
498 gSensor1LatestTouchIndex = s1LastIndex;
499 }
500 else
501 s1LastIndex = -1;
502
503 /* dbox_printf("-----------------------------\nnum: %d, latest: %d\n", s1TouchNum, gSensor1LatestTouchIndex);
504 for(int i=0; i<s1TouchNum; i++)
505 dbox_printf("\t%f\n", gSensor1LatestTouchPos[i]);
506 dbox_printf("------\n");
507 for(int i=0; i<s1TouchNum; i++)
508 dbox_printf("\t%f\n", s1SortedTouches[i]);*/
509
510
511
512 // update variables for both sensors
513 s0PrevTouchNum = s0TouchNum;
514 s1PrevTouchNum = s1TouchNum;
515 }
516 else
517 dbox_printf("Come on instrument!\n"); //break
518
519 gettimeofday(&start, NULL);
520 }
521
522 dbox_printf("sensor thread ended\n");
523 }
524
525 void *keyboardLoop(void *)
526 {
527 if(gVerbose==1)
528 cout << "_________________Keyboard Control Thread!" << endl;
529
530 char keyStroke = '.';
531 cout << "Press q to quit." << endl;
532
533 float speed;
534
535 do
536 {
537 keyStroke = getchar();
538 while(getchar()!='\n'); // to read the first stroke
539
540 switch (keyStroke)
541 {
542 //----------------------------------------------------------------------------
543 case 'a':
544 gOscBanks[gCurrentOscBank]->hopNumTh = 0;
545 gOscBanks[gCurrentOscBank]->play(1);
546 //cout << "Note on" << endl;
547 break;
548 case 's':
549 if(gOscBanks[gCurrentOscBank]->state==bank_playing)
550 {
551 gOscBanks[gCurrentOscBank]->stop();
552 //cout << "Note off" << endl;
553 }
554 break;
555 //----------------------------------------------------------------------------
556 case '[':
557 gOscBanks[gCurrentOscBank]->freqMovement-=0.05;
558 if(gOscBanks[gCurrentOscBank]->freqMovement<0)
559 gOscBanks[gCurrentOscBank]->freqMovement = 0;
560 //cout << "gOscBanks[gCurrentOscBank]->FreqMov: " << gOscBanks[gCurrentOscBank]->freqMovement << endl;
561 break;
562 case ']':
563 gOscBanks[gCurrentOscBank]->freqMovement+=0.05;
564 if(gOscBanks[gCurrentOscBank]->freqMovement>1)
565 gOscBanks[gCurrentOscBank]->freqMovement = 1;
566 //cout << "gOscBanks[gCurrentOscBank]->FreqMov: " << gOscBanks[gCurrentOscBank]->freqMovement << endl;
567 break;
568 //----------------------------------------------------------------------------
569 case '<':
570 speed = gOscBanks[gCurrentOscBank]->getSpeed() - 0.1 ;
571 gOscBanks[gCurrentOscBank]->setSpeed(speed);
572 dbox_printf("Speed: %f\n", speed);
573
574 break;
575 case '>':
576 speed = gOscBanks[gCurrentOscBank]->getSpeed() + 0.1 ;
577 gOscBanks[gCurrentOscBank]->setSpeed(speed);
578 dbox_printf("Speed: %f\n", speed);
579 break;
580 case '0':
581 speed = 0.1;
582 gOscBanks[gCurrentOscBank]->setSpeed(speed);
583 dbox_printf("Speed: %f\n", speed);
584 break;
585 case '1':
586 speed = 0.5;
587 gOscBanks[gCurrentOscBank]->setSpeed(speed);
588 dbox_printf("Speed: %f\n", speed);
589 break;
590 case '2':
591 speed = 1;
592 gOscBanks[gCurrentOscBank]->setSpeed(speed);
593 dbox_printf("Speed: %f\n", speed);
594 break;
595 case '3':
596 speed = 2;
597 gOscBanks[gCurrentOscBank]->setSpeed(speed);
598 dbox_printf("Speed: %f\n", speed);
599 break;
600 case '4':
601 speed = 3;
602 gOscBanks[gCurrentOscBank]->setSpeed(speed);
603 dbox_printf("Speed: %f\n", speed);
604 break;
605 //----------------------------------------------------------------------------
606 case 'z':
607 gOscBanks[gCurrentOscBank]->setJumpHop(0);
608 break;
609 case 'x':
610 gOscBanks[gCurrentOscBank]->setJumpHop(100);
611 break;
612 case 'c':
613 gOscBanks[gCurrentOscBank]->setJumpHop(600);
614 break;
615 case 'v':
616 gOscBanks[gCurrentOscBank]->setJumpHop(1100);
617 break;
618 case 'b':
619 gOscBanks[gCurrentOscBank]->setJumpHop(2000);
620 break;
621 case 'n':
622 gOscBanks[gCurrentOscBank]->setJumpHop(gOscBanks[gCurrentOscBank]->getLastHop());
623 break;
624 //----------------------------------------------------------------------------
625 case 'q':
626 gShouldStop = true;
627 break;
628 case 'o':
629 gNextOscBank = (gCurrentOscBank + 1) % gOscBanks.size();
630 break;
631 default:
632 break;
633 //----------------------------------------------------------------------------
634 }
635 usleep(1000); /* Wait 1ms to avoid checking too quickly */
636 }
637 while (keyStroke!='q');
638
639 cout << "keyboard thread ended" << endl;
640
641 return (void *)0;
642 }