Wasin PT / Mbed 2 deprecated REAL_Goertzely_DISCO

GOERTZELY - DISCO | stm32f746g-disco (microcontroller) | calculate relative magnitude of each standard tuning guitar strings

Dependencies:   TS_DISCO_F746NG mbed LCD_DISCO_F746NG BSP_DISCO_F746NG

main.cpp@3:ccc0103c200e, 2019-05-12 (annotated)

Committer:
WasinPT
Date:
Sun May 12 05:46:45 2019 +0000
Revision:
3:ccc0103c200e
Parent:
0:9933f7db9a9b 
Wasin Pongtawin

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bcostm 0:9933f7db9a9b 1 #include "mbed.h"
bcostm 0:9933f7db9a9b 2 #include "TS_DISCO_F746NG.h"
bcostm 0:9933f7db9a9b 3 #include "LCD_DISCO_F746NG.h"
bcostm 0:9933f7db9a9b 4
bcostm 0:9933f7db9a9b 5 LCD_DISCO_F746NG lcd;
bcostm 0:9933f7db9a9b 6 TS_DISCO_F746NG ts;
WasinPT 3:ccc0103c200e 7 InterruptIn button1(D0);
WasinPT 3:ccc0103c200e 8 AnalogIn Ain(A0);
WasinPT 3:ccc0103c200e 9 TS_StateTypeDef TS_State;
WasinPT 3:ccc0103c200e 10 Serial pc(USBTX, USBRX);
WasinPT 3:ccc0103c200e 11 void button1pressed() {
WasinPT 3:ccc0103c200e 12 wait(0.5);
WasinPT 3:ccc0103c200e 13 lcd.Clear(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 14 NVIC_SystemReset();
WasinPT 3:ccc0103c200e 15 }
bcostm 0:9933f7db9a9b 16
WasinPT 3:ccc0103c200e 17 int main()
bcostm 0:9933f7db9a9b 18 {
WasinPT 3:ccc0103c200e 19 pc.baud(9600);
WasinPT 3:ccc0103c200e 20 pc.format(8, SerialBase::None, 1); //default
WasinPT 3:ccc0103c200e 21 float high, high1, low, low1; //in_tune, in_tune1, in_tune2, in_tune3;
WasinPT 3:ccc0103c200e 22 int note;
WasinPT 3:ccc0103c200e 23 float Buffer[12005];;
WasinPT 3:ccc0103c200e 24 float PI=3.1415;
WasinPT 3:ccc0103c200e 25 int SAMPLE_RATE = 24000;
WasinPT 3:ccc0103c200e 26 float goertzelFilter4;
WasinPT 3:ccc0103c200e 27 char rac;
WasinPT 3:ccc0103c200e 28 int in;
WasinPT 3:ccc0103c200e 29 ///////////////////////////////////////////////////////
WasinPT 3:ccc0103c200e 30 ////////////////////
WasinPT 3:ccc0103c200e 31 //Interupt for Switching Strings
WasinPT 3:ccc0103c200e 32 button1.mode(PullDown);
WasinPT 3:ccc0103c200e 33 button1.rise(&button1pressed);
WasinPT 3:ccc0103c200e 34 int Counter = 0;
WasinPT 3:ccc0103c200e 35 lcd.Clear(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 36 lcd.SetBackColor(LCD_COLOR_DARKGREEN);
WasinPT 3:ccc0103c200e 37 lcd.SetTextColor(LCD_COLOR_YELLOW);
WasinPT 3:ccc0103c200e 38 //
WasinPT 3:ccc0103c200e 39 lcd.SetTextColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 40 lcd.FillRect(1,1,20,20);
WasinPT 3:ccc0103c200e 41 //AntiHead
WasinPT 3:ccc0103c200e 42 lcd.SetTextColor(LCD_COLOR_LIGHTBLUE);
WasinPT 3:ccc0103c200e 43 lcd.FillCircle(111,112,27);
WasinPT 3:ccc0103c200e 44 lcd.FillCircle(111,160,27);
WasinPT 3:ccc0103c200e 45 lcd.FillCircle(116,85,9);
WasinPT 3:ccc0103c200e 46 lcd.FillCircle(116,187,9);
WasinPT 3:ccc0103c200e 47 //Head
WasinPT 3:ccc0103c200e 48 lcd.SetTextColor(LCD_COLOR_LIGHTBLUE);
WasinPT 3:ccc0103c200e 49 lcd.FillRect(114,70,100,133);
WasinPT 3:ccc0103c200e 50 lcd.FillRect(205,76,57,120);
WasinPT 3:ccc0103c200e 51 lcd.FillRect(262,70,100,133);
WasinPT 3:ccc0103c200e 52 lcd.FillRect(262,70,100,133);
WasinPT 3:ccc0103c200e 53 lcd.SetTextColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 54 lcd.FillCircle(362,70,25);
WasinPT 3:ccc0103c200e 55 lcd.FillCircle(362,203,25);
WasinPT 3:ccc0103c200e 56 lcd.SetBackColor(LCD_COLOR_LIGHTBLUE);
WasinPT 3:ccc0103c200e 57 lcd.SetTextColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 58 lcd.DisplayStringAt(0,LINE(5),(uint8_t*)"GOERTZELY-disco",CENTER_MODE);
WasinPT 3:ccc0103c200e 59 lcd.SetTextColor(LCD_COLOR_WHITE);
WasinPT 3:ccc0103c200e 60 lcd.FillRect(362,95,12,84);
WasinPT 3:ccc0103c200e 61 lcd.SetTextColor(LCD_COLOR_BROWN);
WasinPT 3:ccc0103c200e 62 lcd.FillRect(374,95,106,84);
WasinPT 3:ccc0103c200e 63 lcd.SetTextColor(LCD_COLOR_LIGHTGRAY);
WasinPT 3:ccc0103c200e 64 lcd.FillRect(426,95,4,84);
WasinPT 3:ccc0103c200e 65 lcd.FillCircle(400,137,8);
WasinPT 3:ccc0103c200e 66 lcd.FillCircle(452,137,8);
WasinPT 3:ccc0103c200e 67 //TuningSlot
WasinPT 3:ccc0103c200e 68 lcd.SetTextColor(LCD_COLOR_LIGHTGRAY); //TOP
WasinPT 3:ccc0103c200e 69 lcd.FillRect(298,50,9,20);
WasinPT 3:ccc0103c200e 70 lcd.FillRect(236,54,9,22);
WasinPT 3:ccc0103c200e 71 lcd.FillRect(174,50,9,20);
WasinPT 3:ccc0103c200e 72 // //Bottom
WasinPT 3:ccc0103c200e 73 lcd.FillRect(298,203,9,20);
WasinPT 3:ccc0103c200e 74 lcd.FillRect(236,196,9,22);
WasinPT 3:ccc0103c200e 75 lcd.FillRect(174,203,9,20);
WasinPT 3:ccc0103c200e 76
WasinPT 3:ccc0103c200e 77 lcd.SetTextColor(LCD_COLOR_YELLOW);
WasinPT 3:ccc0103c200e 78 lcd.FillRect(277,15,52,35);//E4
WasinPT 3:ccc0103c200e 79 lcd.FillRect(215,19,52,37);//B3
WasinPT 3:ccc0103c200e 80 lcd.FillRect(153,15,52,35);//G3
WasinPT 3:ccc0103c200e 81 lcd.FillRect(277,223,52,35);//E2
WasinPT 3:ccc0103c200e 82 lcd.FillRect(215,216,52,37);//A2
WasinPT 3:ccc0103c200e 83 lcd.FillRect(153,223,52,35);//D3
WasinPT 3:ccc0103c200e 84
WasinPT 3:ccc0103c200e 85 //Note
WasinPT 3:ccc0103c200e 86 lcd.SetBackColor(LCD_COLOR_YELLOW);
WasinPT 3:ccc0103c200e 87 lcd.SetTextColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 88 lcd.DisplayStringAt(278,23,(uint8_t*)"G3",RIGHT_MODE);
WasinPT 3:ccc0103c200e 89 lcd.DisplayStringAt(216,29,(uint8_t*)"B3",RIGHT_MODE);
WasinPT 3:ccc0103c200e 90 lcd.DisplayStringAt(154,23,(uint8_t*)"E4",RIGHT_MODE);
WasinPT 3:ccc0103c200e 91
WasinPT 3:ccc0103c200e 92 lcd.DisplayStringAt(278,229,(uint8_t*)"D3",RIGHT_MODE);
WasinPT 3:ccc0103c200e 93 lcd.DisplayStringAt(216,223,(uint8_t*)"A2",RIGHT_MODE);
WasinPT 3:ccc0103c200e 94 lcd.DisplayStringAt(154,229,(uint8_t*)"E2",RIGHT_MODE);
WasinPT 3:ccc0103c200e 95
bcostm 0:9933f7db9a9b 96
WasinPT 3:ccc0103c200e 97 while (note<50) {
WasinPT 3:ccc0103c200e 98 ts.GetState(&TS_State);
WasinPT 3:ccc0103c200e 99 if(((TS_State.touchX[0]>277)&&(TS_State.touchX[0]<329))
WasinPT 3:ccc0103c200e 100 &&((TS_State.touchY[0]>223)&&(TS_State.touchY[0]<258))) //6
WasinPT 3:ccc0103c200e 101 {
WasinPT 3:ccc0103c200e 102 lcd.SetTextColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 103 lcd.FillRect(277,223,52,35);
WasinPT 3:ccc0103c200e 104 lcd.SetBackColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 105 lcd.SetTextColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 106 lcd.DisplayStringAt(154,229,(uint8_t*)"E2",RIGHT_MODE);
WasinPT 3:ccc0103c200e 107 note = 82;
WasinPT 3:ccc0103c200e 108 rac = 'E';
WasinPT 3:ccc0103c200e 109 in = 2;
WasinPT 3:ccc0103c200e 110
WasinPT 3:ccc0103c200e 111 }
WasinPT 3:ccc0103c200e 112 if(((TS_State.touchX[0]>215)&&(TS_State.touchX[0]<267))
WasinPT 3:ccc0103c200e 113 &&((TS_State.touchY[0]>216)&&(TS_State.touchY[0]<253))) //5
WasinPT 3:ccc0103c200e 114 {
WasinPT 3:ccc0103c200e 115 lcd.SetTextColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 116 lcd.FillRect(215,216,52,37);
WasinPT 3:ccc0103c200e 117 lcd.SetBackColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 118 lcd.SetTextColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 119 lcd.DisplayStringAt(216,223,(uint8_t*)"A2",RIGHT_MODE);
WasinPT 3:ccc0103c200e 120 note = 110;
WasinPT 3:ccc0103c200e 121 rac = 'A';
WasinPT 3:ccc0103c200e 122 in = 2;
WasinPT 3:ccc0103c200e 123
WasinPT 3:ccc0103c200e 124 }
WasinPT 3:ccc0103c200e 125 if(((TS_State.touchX[0]>153)&&(TS_State.touchX[0]<205))
WasinPT 3:ccc0103c200e 126 &&((TS_State.touchY[0]>223)&&(TS_State.touchY[0]<258))) //4
WasinPT 3:ccc0103c200e 127 {
WasinPT 3:ccc0103c200e 128 lcd.SetTextColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 129 lcd.FillRect(153,223,52,35);
WasinPT 3:ccc0103c200e 130 lcd.SetBackColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 131 lcd.SetTextColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 132 lcd.DisplayStringAt(278,229,(uint8_t*)"D3",RIGHT_MODE);
WasinPT 3:ccc0103c200e 133 note = 147;
WasinPT 3:ccc0103c200e 134 rac = 'D';
WasinPT 3:ccc0103c200e 135 in = 3;
WasinPT 3:ccc0103c200e 136 }
WasinPT 3:ccc0103c200e 137 if(((TS_State.touchX[0]>153)&&(TS_State.touchX[0]<205))
WasinPT 3:ccc0103c200e 138 &&((TS_State.touchY[0]>15)&&(TS_State.touchY[0]<50))) //3
WasinPT 3:ccc0103c200e 139 {
WasinPT 3:ccc0103c200e 140 lcd.SetTextColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 141 lcd.FillRect(153,15,52,35);
WasinPT 3:ccc0103c200e 142 lcd.SetBackColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 143 lcd.SetTextColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 144 lcd.DisplayStringAt(278,23,(uint8_t*)"G3",RIGHT_MODE);
WasinPT 3:ccc0103c200e 145 note = 196;
WasinPT 3:ccc0103c200e 146 rac = 'G';
WasinPT 3:ccc0103c200e 147 in = 3;
WasinPT 3:ccc0103c200e 148 }
WasinPT 3:ccc0103c200e 149 if(((TS_State.touchX[0]>215)&&(TS_State.touchX[0]<267))
WasinPT 3:ccc0103c200e 150 &&((TS_State.touchY[0]>19)&&(TS_State.touchY[0]<56))) //2
WasinPT 3:ccc0103c200e 151 {
WasinPT 3:ccc0103c200e 152 lcd.SetTextColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 153 lcd.FillRect(215,19,52,37);
WasinPT 3:ccc0103c200e 154 lcd.SetBackColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 155 lcd.SetTextColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 156 lcd.DisplayStringAt(216,29,(uint8_t*)"B3",RIGHT_MODE);
WasinPT 3:ccc0103c200e 157 note = 247;
WasinPT 3:ccc0103c200e 158 rac = 'B';
WasinPT 3:ccc0103c200e 159 in = 3;
WasinPT 3:ccc0103c200e 160 }
WasinPT 3:ccc0103c200e 161 if(((TS_State.touchX[0]>277)&&(TS_State.touchX[0]<329))
WasinPT 3:ccc0103c200e 162 &&((TS_State.touchY[0]>15)&&(TS_State.touchY[0]<50))) //1
WasinPT 3:ccc0103c200e 163 {
WasinPT 3:ccc0103c200e 164 lcd.SetTextColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 165 lcd.FillRect(277,15,52,35);
WasinPT 3:ccc0103c200e 166 lcd.SetBackColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 167 lcd.SetTextColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 168 lcd.DisplayStringAt(154,23,(uint8_t*)"E4",RIGHT_MODE);
WasinPT 3:ccc0103c200e 169 note = 330;
WasinPT 3:ccc0103c200e 170 rac = 'E';
WasinPT 3:ccc0103c200e 171 in = 4;
WasinPT 3:ccc0103c200e 172 }
WasinPT 3:ccc0103c200e 173 }
WasinPT 3:ccc0103c200e 174
WasinPT 3:ccc0103c200e 175 wait(0.5);
WasinPT 3:ccc0103c200e 176
WasinPT 3:ccc0103c200e 177
WasinPT 3:ccc0103c200e 178 while(1){////// mother while ///////////////////
WasinPT 3:ccc0103c200e 179
WasinPT 3:ccc0103c200e 180 lcd.Clear(LCD_COLOR_LIGHTRED);
WasinPT 3:ccc0103c200e 181 lcd.SetBackColor(LCD_COLOR_LIGHTRED);
WasinPT 3:ccc0103c200e 182 lcd.SetTextColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 183 lcd.DisplayStringAt(0, LINE(6), (uint8_t *)"...1...", CENTER_MODE);
WasinPT 3:ccc0103c200e 184 wait(1);
WasinPT 3:ccc0103c200e 185 lcd.Clear(LCD_COLOR_LIGHTRED);
WasinPT 3:ccc0103c200e 186 lcd.SetBackColor(LCD_COLOR_LIGHTRED);
WasinPT 3:ccc0103c200e 187 lcd.SetTextColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 188 lcd.DisplayStringAt(0, LINE(6), (uint8_t *)"...2...", CENTER_MODE);
WasinPT 3:ccc0103c200e 189 wait(1);
WasinPT 3:ccc0103c200e 190 lcd.Clear(LCD_COLOR_LIGHTRED);
WasinPT 3:ccc0103c200e 191 lcd.SetBackColor(LCD_COLOR_LIGHTRED);
WasinPT 3:ccc0103c200e 192 lcd.SetTextColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 193 lcd.DisplayStringAt(0, LINE(6), (uint8_t *)"...3...", CENTER_MODE);
bcostm 0:9933f7db9a9b 194 wait(1);
bcostm 0:9933f7db9a9b 195
WasinPT 3:ccc0103c200e 196 button1.mode(PullDown);
WasinPT 3:ccc0103c200e 197 button1.rise(&button1pressed);
WasinPT 3:ccc0103c200e 198 Counter = 0;
WasinPT 3:ccc0103c200e 199 lcd.Clear(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 200 for(int i=0;i<10000;i=i+1)
WasinPT 3:ccc0103c200e 201 {
WasinPT 3:ccc0103c200e 202 Buffer[i] = Ain.read();
WasinPT 3:ccc0103c200e 203 Counter = Counter+1;
WasinPT 3:ccc0103c200e 204 char str[20],con[20];
WasinPT 3:ccc0103c200e 205 lcd.SetBackColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 206 lcd.SetTextColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 207 sprintf(con," Please Keep Picking : %c%d ",rac,in);
WasinPT 3:ccc0103c200e 208 lcd.DisplayStringAt(0, LINE(4), (uint8_t *)con, CENTER_MODE);
WasinPT 3:ccc0103c200e 209 sprintf(str,"We did %d sample",Counter);
WasinPT 3:ccc0103c200e 210 lcd.DisplayStringAt(0, LINE(6), (uint8_t *)str, CENTER_MODE);
WasinPT 3:ccc0103c200e 211 wait_us(42);
bcostm 0:9933f7db9a9b 212 }
bcostm 0:9933f7db9a9b 213
WasinPT 3:ccc0103c200e 214 /////////////////CALCULATION//////////
WasinPT 3:ccc0103c200e 215 high = 0;
WasinPT 3:ccc0103c200e 216 low = 0;
WasinPT 3:ccc0103c200e 217 ////HIGH//////
WasinPT 3:ccc0103c200e 218
WasinPT 3:ccc0103c200e 219 for (int i=3; i<46; i+=3)
WasinPT 3:ccc0103c200e 220 {
WasinPT 3:ccc0103c200e 221 float freq = note + i;
WasinPT 3:ccc0103c200e 222 int N = Counter;
WasinPT 3:ccc0103c200e 223 float s_prev = 0.0;
WasinPT 3:ccc0103c200e 224 float s_prev2 = 0.0;
WasinPT 3:ccc0103c200e 225 float coeff,normalizedfreq,s;
WasinPT 3:ccc0103c200e 226 normalizedfreq = freq / SAMPLE_RATE;
WasinPT 3:ccc0103c200e 227 coeff = 2*cos(2*PI*normalizedfreq);
WasinPT 3:ccc0103c200e 228 for (int i=0; i<=N ; i=i+1) {
WasinPT 3:ccc0103c200e 229 s = Buffer[i] + coeff * s_prev - s_prev2;
WasinPT 3:ccc0103c200e 230 s_prev2 = s_prev;
WasinPT 3:ccc0103c200e 231 s_prev = s;
WasinPT 3:ccc0103c200e 232 }
WasinPT 3:ccc0103c200e 233 float goertzelFilter1 = s_prev2*s_prev2+s_prev*s_prev-coeff*s_prev*s_prev2;
WasinPT 3:ccc0103c200e 234 high1 = goertzelFilter1;
WasinPT 3:ccc0103c200e 235 if (high1 > high)
WasinPT 3:ccc0103c200e 236 {
WasinPT 3:ccc0103c200e 237 high=high1;
WasinPT 3:ccc0103c200e 238 }
WasinPT 3:ccc0103c200e 239 }
WasinPT 3:ccc0103c200e 240 //lcd.Clear(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 241 //lcd.SetBackColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 242 //lcd.SetTextColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 243 //lcd.DisplayStringAt(0, LINE(6), (uint8_t *)"...1...", CENTER_MODE);
WasinPT 3:ccc0103c200e 244 wait(0.1);
WasinPT 3:ccc0103c200e 245 ////LOW//////
WasinPT 3:ccc0103c200e 246 for (int i=3; i<46; i+=3)
WasinPT 3:ccc0103c200e 247 {
WasinPT 3:ccc0103c200e 248 float freq = note - i;
WasinPT 3:ccc0103c200e 249 int N = Counter;
WasinPT 3:ccc0103c200e 250 float s_prev = 0.0;
WasinPT 3:ccc0103c200e 251 float s_prev2 = 0.0;
WasinPT 3:ccc0103c200e 252 float coeff,normalizedfreq,s;
WasinPT 3:ccc0103c200e 253 normalizedfreq = freq / SAMPLE_RATE;
WasinPT 3:ccc0103c200e 254 coeff = 2*cos(2*PI*normalizedfreq);
WasinPT 3:ccc0103c200e 255 for (int i=0; i<=N ; i=i+1) {
WasinPT 3:ccc0103c200e 256 s = Buffer[i] + coeff * s_prev - s_prev2;
WasinPT 3:ccc0103c200e 257 s_prev2 = s_prev;
WasinPT 3:ccc0103c200e 258 s_prev = s;
WasinPT 3:ccc0103c200e 259 }
WasinPT 3:ccc0103c200e 260 float goertzelFilter2 = s_prev2*s_prev2+s_prev*s_prev-coeff*s_prev*s_prev2;
WasinPT 3:ccc0103c200e 261 low1 = goertzelFilter2;
WasinPT 3:ccc0103c200e 262 if (low1 > low)
WasinPT 3:ccc0103c200e 263 {
WasinPT 3:ccc0103c200e 264 low=low1;
bcostm 0:9933f7db9a9b 265 }
bcostm 0:9933f7db9a9b 266 }
WasinPT 3:ccc0103c200e 267 // lcd.Clear(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 268 //lcd.SetBackColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 269 //lcd.SetTextColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 270 //lcd.DisplayStringAt(0, LINE(6), (uint8_t *)" ..2.. ", CENTER_MODE);
WasinPT 3:ccc0103c200e 271 wait(0.1);
WasinPT 3:ccc0103c200e 272
WasinPT 3:ccc0103c200e 273 /////////goertzelFilter3/////////////
WasinPT 3:ccc0103c200e 274 float freq = note + 1;
WasinPT 3:ccc0103c200e 275 int N = Counter;
WasinPT 3:ccc0103c200e 276 float s_prev = 0.0;
WasinPT 3:ccc0103c200e 277 float s_prev2 = 0.0;
WasinPT 3:ccc0103c200e 278 float coeff,normalizedfreq,s;
WasinPT 3:ccc0103c200e 279 normalizedfreq = freq / SAMPLE_RATE;
WasinPT 3:ccc0103c200e 280 coeff = 2*cos(2*PI*normalizedfreq);
WasinPT 3:ccc0103c200e 281 for (int i=0; i<=N ; i=i+1)
WasinPT 3:ccc0103c200e 282 {
WasinPT 3:ccc0103c200e 283 s = Buffer[i] + coeff * s_prev - s_prev2;
WasinPT 3:ccc0103c200e 284 s_prev2 = s_prev;
WasinPT 3:ccc0103c200e 285 s_prev = s;
WasinPT 3:ccc0103c200e 286 }
WasinPT 3:ccc0103c200e 287 float goertzelFilter3 = s_prev2*s_prev2+s_prev*s_prev-coeff*s_prev*s_prev2;
WasinPT 3:ccc0103c200e 288
WasinPT 3:ccc0103c200e 289 //lcd.Clear(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 290 //lcd.SetBackColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 291 //lcd.SetTextColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 292 //lcd.DisplayStringAt(0, LINE(6), (uint8_t *)"...3...", CENTER_MODE);
WasinPT 3:ccc0103c200e 293 wait(0.1);
WasinPT 3:ccc0103c200e 294
WasinPT 3:ccc0103c200e 295 /////////goertzelFilter4/////////////
WasinPT 3:ccc0103c200e 296 freq = note ;
WasinPT 3:ccc0103c200e 297 N = Counter;
WasinPT 3:ccc0103c200e 298 s_prev = 0.0;
WasinPT 3:ccc0103c200e 299 s_prev2 = 0.0;
WasinPT 3:ccc0103c200e 300 normalizedfreq = freq / SAMPLE_RATE;
WasinPT 3:ccc0103c200e 301 coeff = 2*cos(2*PI*normalizedfreq);
WasinPT 3:ccc0103c200e 302 for (int i=0; i<=N ; i=i+1){
WasinPT 3:ccc0103c200e 303 s = Buffer[i] + coeff * s_prev - s_prev2;
WasinPT 3:ccc0103c200e 304 s_prev2 = s_prev;
WasinPT 3:ccc0103c200e 305 s_prev = s;
WasinPT 3:ccc0103c200e 306 }
WasinPT 3:ccc0103c200e 307 goertzelFilter4 = s_prev2*s_prev2+s_prev*s_prev-coeff*s_prev*s_prev2;
WasinPT 3:ccc0103c200e 308 //lcd.Clear(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 309 //lcd.SetBackColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 310 //lcd.SetTextColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 311 //lcd.DisplayStringAt(0, LINE(6), (uint8_t *)" ..4.. ", CENTER_MODE);
WasinPT 3:ccc0103c200e 312 wait(0.1);
WasinPT 3:ccc0103c200e 313
WasinPT 3:ccc0103c200e 314
WasinPT 3:ccc0103c200e 315 /////////goertzelFilter5/////////////
WasinPT 3:ccc0103c200e 316 //freq = note - 1;
WasinPT 3:ccc0103c200e 317 // N = Counter;
WasinPT 3:ccc0103c200e 318 // s_prev = 0.0;
WasinPT 3:ccc0103c200e 319 // s_prev2 = 0.0;
WasinPT 3:ccc0103c200e 320 // normalizedfreq = freq / SAMPLE_RATE;
WasinPT 3:ccc0103c200e 321 // coeff = 2*cos(2*PI*normalizedfreq);
WasinPT 3:ccc0103c200e 322 //for (int i=0; i<=N ; i=i+1){
WasinPT 3:ccc0103c200e 323 // s = Buffer[i] + coeff * s_prev - s_prev2;
WasinPT 3:ccc0103c200e 324 // s_prev2 = s_prev;
WasinPT 3:ccc0103c200e 325 // s_prev = s;
WasinPT 3:ccc0103c200e 326 //}
WasinPT 3:ccc0103c200e 327 //float goertzelFilter5 = s_prev2*s_prev2+s_prev*s_prev-coeff*s_prev*s_prev2;
WasinPT 3:ccc0103c200e 328 // lcd.Clear(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 329 //lcd.SetBackColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 330 //lcd.SetTextColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 331 //lcd.DisplayStringAt(0, LINE(6), (uint8_t *)"...5...", CENTER_MODE);
WasinPT 3:ccc0103c200e 332 // wait(0.4);
WasinPT 3:ccc0103c200e 333
WasinPT 3:ccc0103c200e 334
WasinPT 3:ccc0103c200e 335 //in_tune1 = goertzelFilter3;
WasinPT 3:ccc0103c200e 336 //in_tune2 = goertzelFilter4;
WasinPT 3:ccc0103c200e 337 //in_tune3 = goertzelFilter5;
WasinPT 3:ccc0103c200e 338
WasinPT 3:ccc0103c200e 339 // if ((in_tune1 > in_tune2) && (in_tune1 > in_tune3))
WasinPT 3:ccc0103c200e 340 //{in_tune = in_tune1;}
WasinPT 3:ccc0103c200e 341 // else if ((in_tune2 > in_tune1) && (in_tune2 > in_tune3))
WasinPT 3:ccc0103c200e 342 //{in_tune = in_tune2;}
WasinPT 3:ccc0103c200e 343 //else {in_tune = in_tune3;}
WasinPT 3:ccc0103c200e 344 //printf("high = %.2f, low = %.2f, in_tune = %.2f", high, low, in_tune);
WasinPT 3:ccc0103c200e 345
WasinPT 3:ccc0103c200e 346
WasinPT 3:ccc0103c200e 347 //if ((in_tune > high) && (in_tune > low)) {
WasinPT 3:ccc0103c200e 348 // lcd.Clear(LCD_COLOR_GREEN);
WasinPT 3:ccc0103c200e 349 // lcd.SetBackColor(LCD_COLOR_GREEN);
WasinPT 3:ccc0103c200e 350 // lcd.SetTextColor(LCD_COLOR_BLACK);
WasinPT 3:ccc0103c200e 351 // lcd.DisplayStringAt(0,LINE(5),(uint8_t*)"NOW INTUNE!!!!",CENTER_MODE);
WasinPT 3:ccc0103c200e 352 //wait(1.5);
WasinPT 3:ccc0103c200e 353 // }
WasinPT 3:ccc0103c200e 354 //else if (high > in_tune) {
WasinPT 3:ccc0103c200e 355 // lcd.Clear(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 356 //wait(1.5);}
WasinPT 3:ccc0103c200e 357 //else if (low > in_tune) {
WasinPT 3:ccc0103c200e 358 // lcd.Clear(LCD_COLOR_BLUE);
WasinPT 3:ccc0103c200e 359 // wait(1.5);}
WasinPT 3:ccc0103c200e 360 // else{}
WasinPT 3:ccc0103c200e 361
WasinPT 3:ccc0103c200e 362 lcd.Clear(LCD_COLOR_YELLOW);
WasinPT 3:ccc0103c200e 363 lcd.SetBackColor(LCD_COLOR_YELLOW);
WasinPT 3:ccc0103c200e 364 lcd.SetTextColor(LCD_COLOR_RED);
WasinPT 3:ccc0103c200e 365 char str1[20],str2[20],str3[20];
WasinPT 3:ccc0103c200e 366 sprintf(str1," Relative Magnitude ");
WasinPT 3:ccc0103c200e 367 lcd.DisplayStringAt(0, LINE(3), (uint8_t *)str1, CENTER_MODE);
WasinPT 3:ccc0103c200e 368 sprintf(str3," Note : %c%d ",rac,in);
WasinPT 3:ccc0103c200e 369 lcd.DisplayStringAt(0, LINE(5), (uint8_t *)str3, CENTER_MODE);
WasinPT 3:ccc0103c200e 370 sprintf(str2," %f ",goertzelFilter4);
WasinPT 3:ccc0103c200e 371 lcd.DisplayStringAt(0, LINE(7), (uint8_t *)str2, CENTER_MODE);
WasinPT 3:ccc0103c200e 372 wait(4);
WasinPT 3:ccc0103c200e 373 }
WasinPT 3:ccc0103c200e 374
WasinPT 3:ccc0103c200e 375 }