towa saionzi / Mbed 2 deprecated Nucleo_MicBiQuadSender

マイクのアレ

Dependencies:   mbed

main.cpp@0:6417f011fa48, 2017-08-22 (annotated)

Committer:
saionziTowa
Date:
Tue Aug 22 11:16:55 2017 +0000
Revision:
0:6417f011fa48
??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
saionziTowa 0:6417f011fa48 1 #include "mbed.h"
saionziTowa 0:6417f011fa48 2
saionziTowa 0:6417f011fa48 3 Serial pc(SERIAL_TX,SERIAL_RX);
saionziTowa 0:6417f011fa48 4 DigitalOut myled(LED1);
saionziTowa 0:6417f011fa48 5 AnalogIn mic(A5);
saionziTowa 0:6417f011fa48 6 float readed;
saionziTowa 0:6417f011fa48 7 Timer T;
saionziTowa 0:6417f011fa48 8 unsigned long oldtime;
saionziTowa 0:6417f011fa48 9
saionziTowa 0:6417f011fa48 10 //*********************Setting of BandPass Fillter****************************
saionziTowa 0:6417f011fa48 11 /*
saionziTowa 0:6417f011fa48 12 float samplerate=93432.89;
saionziTowa 0:6417f011fa48 13 const float freq=4261;//3942; //4261:new 3942:old
saionziTowa 0:6417f011fa48 14 const float bw=0.3f;
saionziTowa 0:6417f011fa48 15 */
saionziTowa 0:6417f011fa48 16
saionziTowa 0:6417f011fa48 17
saionziTowa 0:6417f011fa48 18
saionziTowa 0:6417f011fa48 19 float samplerate=91634.59;
saionziTowa 0:6417f011fa48 20 const float freq=5200;//5200; //5700:new 5200:old
saionziTowa 0:6417f011fa48 21 const float bw=0.5f;
saionziTowa 0:6417f011fa48 22
saionziTowa 0:6417f011fa48 23
saionziTowa 0:6417f011fa48 24 float omega=2.0f*3.14159265f*freq/samplerate;
saionziTowa 0:6417f011fa48 25 float alpha=sin(omega)*sinh(log10(2.0f)/2.0f*bw*omega/sin(omega));
saionziTowa 0:6417f011fa48 26 float a0=1.0f+alpha;
saionziTowa 0:6417f011fa48 27 float a1=-2.0f*cos(omega);
saionziTowa 0:6417f011fa48 28 float a2=1.0f-alpha;
saionziTowa 0:6417f011fa48 29 float b0=alpha;
saionziTowa 0:6417f011fa48 30 float b1=0.0f;
saionziTowa 0:6417f011fa48 31 float b2=-1.0f*alpha;
saionziTowa 0:6417f011fa48 32 //*****************************************************************************
saionziTowa 0:6417f011fa48 33 float inbuf[2001];
saionziTowa 0:6417f011fa48 34 float outbuf[2001];
saionziTowa 0:6417f011fa48 35 int dbuf[2001];
saionziTowa 0:6417f011fa48 36
saionziTowa 0:6417f011fa48 37 float amp[40];
saionziTowa 0:6417f011fa48 38 float amplog[100];
saionziTowa 0:6417f011fa48 39
saionziTowa 0:6417f011fa48 40 long counter=0;
saionziTowa 0:6417f011fa48 41 int mastercounter=0;
saionziTowa 0:6417f011fa48 42 int amplogcounter=0;
saionziTowa 0:6417f011fa48 43
saionziTowa 0:6417f011fa48 44
saionziTowa 0:6417f011fa48 45 int main() {
saionziTowa 0:6417f011fa48 46 T.start();
saionziTowa 0:6417f011fa48 47 pc.baud(115200);
saionziTowa 0:6417f011fa48 48 while(1) {
saionziTowa 0:6417f011fa48 49 //float in1,in2,out1,out2,input,output;
saionziTowa 0:6417f011fa48 50 static float in1=0;
saionziTowa 0:6417f011fa48 51 static float in2=0;
saionziTowa 0:6417f011fa48 52 static float out1=0;
saionziTowa 0:6417f011fa48 53 static float out2=0;
saionziTowa 0:6417f011fa48 54 static float input=0;
saionziTowa 0:6417f011fa48 55 static float output=0;
saionziTowa 0:6417f011fa48 56
saionziTowa 0:6417f011fa48 57 //while(T.read_us()<(oldtime+100));
saionziTowa 0:6417f011fa48 58 input=mic.read();
saionziTowa 0:6417f011fa48 59 //oldtime=T.read_us();
saionziTowa 0:6417f011fa48 60 output=b0/a0*input+b1/a0*in1+b2/a0*in2-a1/a0*out1-a2/a0*out2;
saionziTowa 0:6417f011fa48 61 in2=in1;
saionziTowa 0:6417f011fa48 62 in1=input;
saionziTowa 0:6417f011fa48 63 out2=out1;
saionziTowa 0:6417f011fa48 64 out1=output;
saionziTowa 0:6417f011fa48 65
saionziTowa 0:6417f011fa48 66 dbuf[counter]=T.read_us()-oldtime;
saionziTowa 0:6417f011fa48 67 inbuf[counter]=input;
saionziTowa 0:6417f011fa48 68 outbuf[counter]=output;
saionziTowa 0:6417f011fa48 69 oldtime=T.read_us();
saionziTowa 0:6417f011fa48 70 counter++;
saionziTowa 0:6417f011fa48 71
saionziTowa 0:6417f011fa48 72
saionziTowa 0:6417f011fa48 73 if(counter>2001){
saionziTowa 0:6417f011fa48 74 for(int i=0;i<40;i++){
saionziTowa 0:6417f011fa48 75 float max=-1;
saionziTowa 0:6417f011fa48 76 float min=1;
saionziTowa 0:6417f011fa48 77 for(int j=0;j<50;j++){
saionziTowa 0:6417f011fa48 78 if(outbuf[i*50+j+1]>max)max=outbuf[i*50+j+1];
saionziTowa 0:6417f011fa48 79 else if(outbuf[i*50+j+1]<min)min=outbuf[i*50+j+1];
saionziTowa 0:6417f011fa48 80 }
saionziTowa 0:6417f011fa48 81 amp[i]=(max-min)/2;
saionziTowa 0:6417f011fa48 82 //pc.printf("%f %f\n",max,min);
saionziTowa 0:6417f011fa48 83 }
saionziTowa 0:6417f011fa48 84
saionziTowa 0:6417f011fa48 85 /*
saionziTowa 0:6417f011fa48 86 float max=-1;
saionziTowa 0:6417f011fa48 87 float min=1;
saionziTowa 0:6417f011fa48 88 bool maxf=false;
saionziTowa 0:6417f011fa48 89 bool minf=false;
saionziTowa 0:6417f011fa48 90 int ampcount;
saionziTowa 0:6417f011fa48 91 for(int i=0;i<2000;i++){
saionziTowa 0:6417f011fa48 92 if(outbuf[i]-outbuf[i+1]<0&&outbuf[i+1]-outbuf[i+2]>0) {
saionziTowa 0:6417f011fa48 93 max=outbuf[i+1];
saionziTowa 0:6417f011fa48 94 maxf=true;
saionziTowa 0:6417f011fa48 95 }
saionziTowa 0:6417f011fa48 96 else if(outbuf[i]-outbuf[i+1]>0&&outbuf[i+1]-outbuf[i+2]<0){
saionziTowa 0:6417f011fa48 97 min=outbuf[i+1];
saionziTowa 0:6417f011fa48 98 minf=true;
saionziTowa 0:6417f011fa48 99 }
saionziTowa 0:6417f011fa48 100 if(minf&&maxf){
saionziTowa 0:6417f011fa48 101 minf=false;
saionziTowa 0:6417f011fa48 102 maxf=false;
saionziTowa 0:6417f011fa48 103 amp[ampcount%40]=max-min;
saionziTowa 0:6417f011fa48 104 ampcount++;
saionziTowa 0:6417f011fa48 105 //pc.printf("%f %f\n",max,min);
saionziTowa 0:6417f011fa48 106 }
saionziTowa 0:6417f011fa48 107 }*/
saionziTowa 0:6417f011fa48 108
saionziTowa 0:6417f011fa48 109 float buf=0;
saionziTowa 0:6417f011fa48 110 for(int i=20;i<40;i++){
saionziTowa 0:6417f011fa48 111 buf+=amp[i];
saionziTowa 0:6417f011fa48 112 }
saionziTowa 0:6417f011fa48 113
saionziTowa 0:6417f011fa48 114
saionziTowa 0:6417f011fa48 115 amplog[amplogcounter]=buf;
saionziTowa 0:6417f011fa48 116 amplogcounter++;
saionziTowa 0:6417f011fa48 117 if(amplogcounter==40){
saionziTowa 0:6417f011fa48 118 amplogcounter=0;
saionziTowa 0:6417f011fa48 119 buf=0;
saionziTowa 0:6417f011fa48 120 for(int i=0;i<40;i++){
saionziTowa 0:6417f011fa48 121 if(amplog[i]>buf) buf=amplog[i];
saionziTowa 0:6417f011fa48 122 //pc.printf("%f\n",amplog[i]/10);
saionziTowa 0:6417f011fa48 123 }
saionziTowa 0:6417f011fa48 124 pc.putc('S');
saionziTowa 0:6417f011fa48 125 pc.putc((int)(buf/10*6*20000)%256);
saionziTowa 0:6417f011fa48 126 pc.putc((int)(buf/10*6*20000)/256);
saionziTowa 0:6417f011fa48 127 //pc.printf("%f\n",buf/10);
saionziTowa 0:6417f011fa48 128 }
saionziTowa 0:6417f011fa48 129
saionziTowa 0:6417f011fa48 130 //pc.putc('S');
saionziTowa 0:6417f011fa48 131 //pc.printf("%f\n",buf/10);
saionziTowa 0:6417f011fa48 132 //pc.putc((int)(buf/10*2*20000)%256);
saionziTowa 0:6417f011fa48 133 //pc.putc((int)(buf/10*2*20000)/256);
saionziTowa 0:6417f011fa48 134 //pc.printf("%d",(int)(buf/10*2*20000)%256);
saionziTowa 0:6417f011fa48 135 //pc.printf("%d\n",(int)(buf/10*2*20000)/256);
saionziTowa 0:6417f011fa48 136 //for(int i=0;i<1100;i++) pc.printf("%d %f\n",dbuf[i+1],inbuf[i+1]);
saionziTowa 0:6417f011fa48 137 counter=0;
saionziTowa 0:6417f011fa48 138 T.reset();
saionziTowa 0:6417f011fa48 139 oldtime=T.read_us();
saionziTowa 0:6417f011fa48 140
saionziTowa 0:6417f011fa48 141 float avd=0;
saionziTowa 0:6417f011fa48 142 for(int i=0;i<100;i++){
saionziTowa 0:6417f011fa48 143 avd+=dbuf[i+1];
saionziTowa 0:6417f011fa48 144 }
saionziTowa 0:6417f011fa48 145 avd=avd/100;
saionziTowa 0:6417f011fa48 146 samplerate=1000/avd*1000;
saionziTowa 0:6417f011fa48 147 //pc.printf("%f\n",samplerate);
saionziTowa 0:6417f011fa48 148 omega=2.0f*3.14159265f*freq/samplerate;
saionziTowa 0:6417f011fa48 149 alpha=sin(omega)*sinh(log10(2.0f)/2.0f*bw*omega/sin(omega));
saionziTowa 0:6417f011fa48 150 a0=1.0f+alpha;
saionziTowa 0:6417f011fa48 151 a1=-2.0f*cos(omega);
saionziTowa 0:6417f011fa48 152 a2=1.0f-alpha;
saionziTowa 0:6417f011fa48 153 b0=alpha;
saionziTowa 0:6417f011fa48 154 b1=0.0f;
saionziTowa 0:6417f011fa48 155 b2=-1.0f*alpha;
saionziTowa 0:6417f011fa48 156
saionziTowa 0:6417f011fa48 157 }
saionziTowa 0:6417f011fa48 158 }
saionziTowa 0:6417f011fa48 159 }