Elektronikprojekt Grupp 13 / Mbed 2 deprecated dsp_test

Dependencies:   DHT mbed

dsp_test.cpp@1:d0884279b41d, 2017-05-12 (annotated)

Committer:
elt14lpo
Date:
Fri May 12 06:27:31 2017 +0000
Revision:
1:d0884279b41d
Parent:
0:c81dc1cb885e
Child:
2:1161fea84522
;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
elt14lpo 0:c81dc1cb885e 1 #include "mbed.h"
elt14lpo 0:c81dc1cb885e 2 #include <iostream> /* cout */
elt14lpo 0:c81dc1cb885e 3 //#include <stdio.h> /* printf */
elt14lpo 0:c81dc1cb885e 4 #include <math.h> /* sin */
elt14lpo 0:c81dc1cb885e 5 #include <vector>
elt14lpo 0:c81dc1cb885e 6 #include <stdlib.h> /* abs */
elt14lpo 0:c81dc1cb885e 7 #include <stdio.h>
elt14lpo 0:c81dc1cb885e 8 #include <AnalogIn.h>
elt14lpo 0:c81dc1cb885e 9 #include <stdint.h>
elt14lpo 0:c81dc1cb885e 10 #include <DHT.h>
elt14lpo 0:c81dc1cb885e 11 #include<sstream>
elt14lpo 0:c81dc1cb885e 12 #include "arm_correlate_f32.h"
elt14lpo 0:c81dc1cb885e 13 //#include "arm_correlate_fast_q15.h"
elt14lpo 0:c81dc1cb885e 14
elt14lpo 0:c81dc1cb885e 15 //using namespace std;
elt14lpo 0:c81dc1cb885e 16
elt14lpo 0:c81dc1cb885e 17 /* DEBUG FUNCTION
elt14lpo 0:c81dc1cb885e 18 // ersätter Debug(xyz) med xyz , där xyz är din kod
elt14lpo 0:c81dc1cb885e 19 //För att aktivera:
elt14lpo 0:c81dc1cb885e 20 #define Debug(xyz) xyz
elt14lpo 0:c81dc1cb885e 21
elt14lpo 0:c81dc1cb885e 22 //För att "stänga av":
elt14lpo 0:c81dc1cb885e 23 #define Debug(xyz)
elt14lpo 0:c81dc1cb885e 24
elt14lpo 0:c81dc1cb885e 25 //I din kod, skriv din debug kod liknande så här:
elt14lpo 0:c81dc1cb885e 26 Debug( std::cout << "My text: " << myVariable << std::endl; );
elt14lpo 0:c81dc1cb885e 27
elt14lpo 0:c81dc1cb885e 28 */
elt14lpo 0:c81dc1cb885e 29
elt14lpo 0:c81dc1cb885e 30 #define Debug(x) x
elt14lpo 0:c81dc1cb885e 31 #define DebugPrintState(y) y
elt14lpo 0:c81dc1cb885e 32 #define DebugArcSin(z) z
elt14lpo 0:c81dc1cb885e 33
elt14lpo 0:c81dc1cb885e 34
elt14lpo 0:c81dc1cb885e 35 //----------VARIABLES HERE
elt14lpo 0:c81dc1cb885e 36 const int dataLength = 1000;
elt14lpo 0:c81dc1cb885e 37 const int captureLength = 50;
elt14lpo 0:c81dc1cb885e 38 double temp = 22;
elt14lpo 0:c81dc1cb885e 39 double hum = 10;
elt14lpo 0:c81dc1cb885e 40 double micDist = 0.250; //meters
elt14lpo 0:c81dc1cb885e 41 double threshold_1 = 0; //value when going to active mode channel 1 //old hardcoded value = 330
elt14lpo 0:c81dc1cb885e 42 double threshold_2 = 0; //value when going to active mode channel 2 //old hardcoded value = 200
elt14lpo 0:c81dc1cb885e 43 double threshold_adjust = 15; //used to adjust threshold, + for less sensitivity, - for increased sensitivity
elt14lpo 0:c81dc1cb885e 44 bool calibratedStatus = false; //flag to make sure Nuclueo only calibrated once for background noise
elt14lpo 0:c81dc1cb885e 45 bool checkTemp = false; //flag - true to checktemp, false to use predefined values
elt14lpo 0:c81dc1cb885e 46 int positionOfMaxVal_1;
elt14lpo 0:c81dc1cb885e 47 int positionOfMaxVal_2;
elt14lpo 0:c81dc1cb885e 48 const double PI = 3.14159265358979323846;
elt14lpo 0:c81dc1cb885e 49
elt14lpo 0:c81dc1cb885e 50 // State machine
elt14lpo 0:c81dc1cb885e 51 int STATE;
elt14lpo 0:c81dc1cb885e 52 //const int NONE = -1;
elt14lpo 0:c81dc1cb885e 53 const int IDLE = 0;
elt14lpo 0:c81dc1cb885e 54 const int CALIBRATE = 1;
elt14lpo 0:c81dc1cb885e 55 const int TESTNEW = 2;
elt14lpo 0:c81dc1cb885e 56 const int CALC = 3;
elt14lpo 0:c81dc1cb885e 57 const int CALC_ERROR = 4;
elt14lpo 0:c81dc1cb885e 58 const int SEND = 5;
elt14lpo 0:c81dc1cb885e 59 //const int WAIT = 9;
elt14lpo 0:c81dc1cb885e 60
elt14lpo 0:c81dc1cb885e 61 //dataLength behövs kanske inte, vector klassen kan växa med behov
elt14lpo 0:c81dc1cb885e 62 float channel_1 [dataLength];
elt14lpo 0:c81dc1cb885e 63 float channel_2 [dataLength];
elt14lpo 0:c81dc1cb885e 64 int timestamps_1 [dataLength];
elt14lpo 0:c81dc1cb885e 65 int timestamps_2 [dataLength];
elt14lpo 0:c81dc1cb885e 66 float capture_1 [captureLength];
elt14lpo 0:c81dc1cb885e 67 float *p1 = capture_1;
elt14lpo 0:c81dc1cb885e 68 float capture_2 [captureLength];
elt14lpo 0:c81dc1cb885e 69 float *p2 = capture_2;
elt14lpo 0:c81dc1cb885e 70 float capturestamps_1 [captureLength];
elt14lpo 0:c81dc1cb885e 71 float capturestamps_2 [captureLength];
elt14lpo 1:d0884279b41d 72 float dsp_res [dataLength];
elt14lpo 0:c81dc1cb885e 73 float *p_res = dsp_res;
elt14lpo 0:c81dc1cb885e 74
elt14lpo 0:c81dc1cb885e 75 int positiontest = 0;
elt14lpo 0:c81dc1cb885e 76 int test = 9;
elt14lpo 0:c81dc1cb885e 77 std::vector<double> delaytest(test);
elt14lpo 0:c81dc1cb885e 78
elt14lpo 0:c81dc1cb885e 79
elt14lpo 0:c81dc1cb885e 80 AnalogIn mic1(A0);
elt14lpo 0:c81dc1cb885e 81 AnalogIn mic2(A1);
elt14lpo 0:c81dc1cb885e 82 AnalogIn mic3(A2);
elt14lpo 0:c81dc1cb885e 83 DHT sensor(A3, DHT11);
elt14lpo 0:c81dc1cb885e 84
elt14lpo 0:c81dc1cb885e 85 //TIMER
elt14lpo 0:c81dc1cb885e 86 Timer t;
elt14lpo 0:c81dc1cb885e 87
elt14lpo 0:c81dc1cb885e 88 //led can be used for status
elt14lpo 0:c81dc1cb885e 89 DigitalOut led1(LED1);
elt14lpo 0:c81dc1cb885e 90
elt14lpo 0:c81dc1cb885e 91
elt14lpo 0:c81dc1cb885e 92 //----------FUNCTIONS HERE
elt14lpo 0:c81dc1cb885e 93 //Calculating distance between sound and camera
elt14lpo 0:c81dc1cb885e 94 double calcDist(double t, double v)
elt14lpo 0:c81dc1cb885e 95 {
elt14lpo 0:c81dc1cb885e 96 double s = t*v;
elt14lpo 0:c81dc1cb885e 97 return s;
elt14lpo 0:c81dc1cb885e 98 }
elt14lpo 0:c81dc1cb885e 99
elt14lpo 0:c81dc1cb885e 100 //Calculating angle in radians, D distance between mic1 and mic2
elt14lpo 0:c81dc1cb885e 101 double calcAng(double s, double D)
elt14lpo 0:c81dc1cb885e 102 {
elt14lpo 0:c81dc1cb885e 103 return asin(s/D) + PI/2;
elt14lpo 0:c81dc1cb885e 104 }
elt14lpo 0:c81dc1cb885e 105
elt14lpo 0:c81dc1cb885e 106 //Assuming the input value is temp as a number in degrees celcius and humidity as procent
elt14lpo 0:c81dc1cb885e 107 double calcSoundSpeed(double temp, double hum)
elt14lpo 0:c81dc1cb885e 108 {
elt14lpo 0:c81dc1cb885e 109 //Calculations were done in Matlab
elt14lpo 0:c81dc1cb885e 110 double speed = 331.1190 + 0.6016*temp + 0.0126*hum;
elt14lpo 0:c81dc1cb885e 111 return speed;
elt14lpo 0:c81dc1cb885e 112 }
elt14lpo 0:c81dc1cb885e 113
elt14lpo 0:c81dc1cb885e 114 //translate angle to number for camera
elt14lpo 0:c81dc1cb885e 115 string convertAngToCamNbr(double ang)
elt14lpo 0:c81dc1cb885e 116 {
elt14lpo 0:c81dc1cb885e 117 ang = ang*(180 / PI) + 45; //radianer till grader
elt14lpo 0:c81dc1cb885e 118 double angValues = 270;
elt14lpo 0:c81dc1cb885e 119 int stepValues = 50000;
elt14lpo 0:c81dc1cb885e 120 string tiltNumber = " 18000"; //hårdkodat Camera Pan värde
elt14lpo 0:c81dc1cb885e 121
elt14lpo 0:c81dc1cb885e 122 double oneAng = stepValues/angValues;
elt14lpo 0:c81dc1cb885e 123 double cameraAngNumber = ang*oneAng;
elt14lpo 0:c81dc1cb885e 124 int panInt = (int)(cameraAngNumber); //double to int
elt14lpo 0:c81dc1cb885e 125 //int to string
elt14lpo 0:c81dc1cb885e 126 string panNumber;
elt14lpo 0:c81dc1cb885e 127 ostringstream convert;
elt14lpo 0:c81dc1cb885e 128 convert << panInt;
elt14lpo 0:c81dc1cb885e 129 panNumber = convert.str();
elt14lpo 0:c81dc1cb885e 130
elt14lpo 0:c81dc1cb885e 131 string send = panNumber + tiltNumber;
elt14lpo 0:c81dc1cb885e 132 return send;
elt14lpo 0:c81dc1cb885e 133 }
elt14lpo 0:c81dc1cb885e 134
elt14lpo 0:c81dc1cb885e 135
elt14lpo 0:c81dc1cb885e 136 //calc time delay by finding peak values in 2 vectors
elt14lpo 0:c81dc1cb885e 137 //channel = 1 or 2
elt14lpo 0:c81dc1cb885e 138 int FindPeak(int channel)
elt14lpo 0:c81dc1cb885e 139 {
elt14lpo 0:c81dc1cb885e 140 float *channel_curr; //temporary vector with channel voltage values
elt14lpo 0:c81dc1cb885e 141
elt14lpo 0:c81dc1cb885e 142 //if channel 1 then set current channel to channel 1
elt14lpo 0:c81dc1cb885e 143 if (channel == 1) {
elt14lpo 0:c81dc1cb885e 144 channel_curr = capture_1;
elt14lpo 0:c81dc1cb885e 145 } else channel_curr = capture_2;
elt14lpo 0:c81dc1cb885e 146
elt14lpo 0:c81dc1cb885e 147 //reset max value & sum value
elt14lpo 0:c81dc1cb885e 148 double valueMax = 0;
elt14lpo 0:c81dc1cb885e 149
elt14lpo 0:c81dc1cb885e 150 //reset array position
elt14lpo 0:c81dc1cb885e 151 int positionOfMaxVal = 0;
elt14lpo 0:c81dc1cb885e 152
elt14lpo 0:c81dc1cb885e 153 //find largest value & mark that position in vectors
elt14lpo 0:c81dc1cb885e 154 for (int position = 0; position < captureLength; position++) {
elt14lpo 0:c81dc1cb885e 155 double val = abs(channel_curr[position]);
elt14lpo 0:c81dc1cb885e 156 if (val > valueMax ) {
elt14lpo 0:c81dc1cb885e 157 valueMax = val;
elt14lpo 0:c81dc1cb885e 158 positionOfMaxVal = position;
elt14lpo 0:c81dc1cb885e 159 }
elt14lpo 0:c81dc1cb885e 160 }
elt14lpo 0:c81dc1cb885e 161 return positionOfMaxVal;
elt14lpo 0:c81dc1cb885e 162 }
elt14lpo 0:c81dc1cb885e 163
elt14lpo 0:c81dc1cb885e 164 double FindTimeDelay(int positionOfMaxVal_1, int positionOfMaxVal_2)
elt14lpo 0:c81dc1cb885e 165 {
elt14lpo 0:c81dc1cb885e 166 double timemax_1 = capturestamps_1[positionOfMaxVal_1];
elt14lpo 0:c81dc1cb885e 167 double timemax_2 = capturestamps_2[positionOfMaxVal_2];
elt14lpo 0:c81dc1cb885e 168 double delay = timemax_1 - timemax_2;
elt14lpo 0:c81dc1cb885e 169 return delay; //if negative near microphone 1, if positive near micropnone 2
elt14lpo 0:c81dc1cb885e 170 }
elt14lpo 0:c81dc1cb885e 171
elt14lpo 0:c81dc1cb885e 172
elt14lpo 0:c81dc1cb885e 173 //get voltage value which represents audio amplitude from microphone
elt14lpo 0:c81dc1cb885e 174 double getAudioValue(AnalogIn micX)
elt14lpo 0:c81dc1cb885e 175 {
elt14lpo 0:c81dc1cb885e 176 return 1000*micX.read();
elt14lpo 0:c81dc1cb885e 177 }
elt14lpo 0:c81dc1cb885e 178
elt14lpo 0:c81dc1cb885e 179
elt14lpo 0:c81dc1cb885e 180 bool overThreshold(double micValue_1, double micValue_2)
elt14lpo 0:c81dc1cb885e 181 {
elt14lpo 0:c81dc1cb885e 182 if ((micValue_1 > threshold_1) || (micValue_2 > threshold_2)) {
elt14lpo 0:c81dc1cb885e 183 return true;
elt14lpo 0:c81dc1cb885e 184 } else return false;
elt14lpo 0:c81dc1cb885e 185 }
elt14lpo 0:c81dc1cb885e 186
elt14lpo 0:c81dc1cb885e 187 //true if voltage value in microphone is above the current threshold value
elt14lpo 0:c81dc1cb885e 188 bool calibrateThreshold(double micValue, double currentThreshold)
elt14lpo 0:c81dc1cb885e 189 {
elt14lpo 0:c81dc1cb885e 190 if ( micValue > currentThreshold ) {
elt14lpo 0:c81dc1cb885e 191 return true;
elt14lpo 0:c81dc1cb885e 192 } else return false;
elt14lpo 0:c81dc1cb885e 193 }
elt14lpo 0:c81dc1cb885e 194
elt14lpo 0:c81dc1cb885e 195
elt14lpo 0:c81dc1cb885e 196 // main() runs in its own thread in the OS
elt14lpo 0:c81dc1cb885e 197 int main()
elt14lpo 0:c81dc1cb885e 198 {
elt14lpo 0:c81dc1cb885e 199 for(int i = 0; i < test; i++) {
elt14lpo 0:c81dc1cb885e 200 delaytest[i] = -420 + i*105;
elt14lpo 0:c81dc1cb885e 201 }
elt14lpo 0:c81dc1cb885e 202 t.start(); // start timer
elt14lpo 0:c81dc1cb885e 203
elt14lpo 0:c81dc1cb885e 204 //while (true) {
elt14lpo 0:c81dc1cb885e 205 led1 = !led1;
elt14lpo 0:c81dc1cb885e 206 wait(0.5);
elt14lpo 0:c81dc1cb885e 207
elt14lpo 0:c81dc1cb885e 208
elt14lpo 0:c81dc1cb885e 209 //STATE MACHINE
elt14lpo 0:c81dc1cb885e 210 STATE = IDLE;
elt14lpo 0:c81dc1cb885e 211 //int counter = 0;
elt14lpo 0:c81dc1cb885e 212 while (true) {
elt14lpo 0:c81dc1cb885e 213 switch (STATE) {
elt14lpo 0:c81dc1cb885e 214 case IDLE: //always start here
elt14lpo 0:c81dc1cb885e 215 DebugPrintState( std::cout << "Nucleo state is IDLE: " << std::endl; );
elt14lpo 0:c81dc1cb885e 216 Debug( wait(0.5); );
elt14lpo 0:c81dc1cb885e 217 if (!calibratedStatus) STATE = CALIBRATE;
elt14lpo 0:c81dc1cb885e 218 else STATE = TESTNEW;
elt14lpo 0:c81dc1cb885e 219 break;
elt14lpo 0:c81dc1cb885e 220
elt14lpo 0:c81dc1cb885e 221 case CALIBRATE:
elt14lpo 0:c81dc1cb885e 222 DebugPrintState( std::cout << "Nucleo state is CALIBRATE: " << std::endl; );
elt14lpo 0:c81dc1cb885e 223 Debug( wait(1); );
elt14lpo 0:c81dc1cb885e 224 //listen for X seconds to background noise, to set accurate threshold value
elt14lpo 0:c81dc1cb885e 225 // This should be done only once when rebooting Nucleo
elt14lpo 0:c81dc1cb885e 226 int startTime = t.read_us();
elt14lpo 0:c81dc1cb885e 227 int offsetTime = 3000; //microseconds
elt14lpo 0:c81dc1cb885e 228 int blinkTime = 500; //microseconds
elt14lpo 0:c81dc1cb885e 229 while (t.read_us() < (startTime + offsetTime) ) {
elt14lpo 0:c81dc1cb885e 230 double micValue_1 = getAudioValue(mic1);
elt14lpo 0:c81dc1cb885e 231 if ( calibrateThreshold(micValue_1, threshold_1) ) {
elt14lpo 0:c81dc1cb885e 232 threshold_1 = micValue_1; //threshold value updated
elt14lpo 0:c81dc1cb885e 233 }
elt14lpo 0:c81dc1cb885e 234 double micValue_2 = getAudioValue(mic2);
elt14lpo 0:c81dc1cb885e 235 if ( calibrateThreshold(micValue_2, threshold_2) ) {
elt14lpo 0:c81dc1cb885e 236 threshold_2 = micValue_2; //threshold value updated
elt14lpo 0:c81dc1cb885e 237 }
elt14lpo 0:c81dc1cb885e 238 //make LED blink every 500 ms
elt14lpo 0:c81dc1cb885e 239 if ( t.read_us() > (startTime + blinkTime) ) {
elt14lpo 0:c81dc1cb885e 240 led1 = !led1;
elt14lpo 0:c81dc1cb885e 241 blinkTime = blinkTime + 500;
elt14lpo 0:c81dc1cb885e 242 }
elt14lpo 0:c81dc1cb885e 243 }
elt14lpo 0:c81dc1cb885e 244 threshold_1 = threshold_2 + threshold_adjust;
elt14lpo 0:c81dc1cb885e 245 threshold_2 = threshold_2 + threshold_adjust;
elt14lpo 0:c81dc1cb885e 246
elt14lpo 0:c81dc1cb885e 247 //Calibrate temp and hum
elt14lpo 0:c81dc1cb885e 248 if(checkTemp){
elt14lpo 0:c81dc1cb885e 249 bool done = false;
elt14lpo 0:c81dc1cb885e 250 while(!done) {
elt14lpo 0:c81dc1cb885e 251 if(sensor.readData() == 0) {
elt14lpo 0:c81dc1cb885e 252 temp = sensor.ReadTemperature(CELCIUS);
elt14lpo 0:c81dc1cb885e 253 hum = sensor.ReadHumidity();
elt14lpo 0:c81dc1cb885e 254 DebugPrintState(std::cout << "Temp: " << temp << "Degrees Celcius" <<std::endl; );
elt14lpo 0:c81dc1cb885e 255 DebugPrintState(std::cout << "Hum: " << temp << "%" <<std::endl; );
elt14lpo 0:c81dc1cb885e 256 done = true;
elt14lpo 0:c81dc1cb885e 257 }
elt14lpo 0:c81dc1cb885e 258 }
elt14lpo 0:c81dc1cb885e 259 }
elt14lpo 0:c81dc1cb885e 260
elt14lpo 0:c81dc1cb885e 261 calibratedStatus = true;
elt14lpo 0:c81dc1cb885e 262 STATE = TESTNEW; //next state
elt14lpo 0:c81dc1cb885e 263 break;
elt14lpo 0:c81dc1cb885e 264
elt14lpo 0:c81dc1cb885e 265 case TESTNEW:
elt14lpo 0:c81dc1cb885e 266 DebugPrintState( std::cout << "Nucleo state is TESTNEW: " << std::endl; );
elt14lpo 0:c81dc1cb885e 267 int i = 0;
elt14lpo 0:c81dc1cb885e 268 bool quit = false;
elt14lpo 0:c81dc1cb885e 269 while(!quit) {
elt14lpo 0:c81dc1cb885e 270 channel_1[i] = getAudioValue(mic1);
elt14lpo 0:c81dc1cb885e 271 timestamps_1[i] = t.read_us();
elt14lpo 0:c81dc1cb885e 272 channel_2[i] = getAudioValue(mic2);
elt14lpo 0:c81dc1cb885e 273 timestamps_2[i] = t.read_us();
elt14lpo 0:c81dc1cb885e 274 if(overThreshold(channel_1[i], channel_2[i]) == true) {
elt14lpo 0:c81dc1cb885e 275 capture_1[0] = channel_1[i];
elt14lpo 0:c81dc1cb885e 276 capturestamps_1[0] = timestamps_1[i];
elt14lpo 0:c81dc1cb885e 277 capture_2[0] = channel_2[i];
elt14lpo 0:c81dc1cb885e 278 capturestamps_2[0] = timestamps_2[i];
elt14lpo 0:c81dc1cb885e 279 for(int i = 1; i < captureLength; i++) {
elt14lpo 0:c81dc1cb885e 280 capture_1[i] = getAudioValue(mic1);
elt14lpo 0:c81dc1cb885e 281 capturestamps_1[i] = t.read_us();
elt14lpo 0:c81dc1cb885e 282 capture_2[i] = getAudioValue(mic2);
elt14lpo 0:c81dc1cb885e 283 capturestamps_2[i] = t.read_us();
elt14lpo 0:c81dc1cb885e 284 }
elt14lpo 0:c81dc1cb885e 285 quit = true;
elt14lpo 0:c81dc1cb885e 286 }
elt14lpo 0:c81dc1cb885e 287 if(i < dataLength) {
elt14lpo 0:c81dc1cb885e 288 i++;
elt14lpo 0:c81dc1cb885e 289 } else {
elt14lpo 0:c81dc1cb885e 290 i = 0;
elt14lpo 0:c81dc1cb885e 291 }
elt14lpo 0:c81dc1cb885e 292 }
elt14lpo 0:c81dc1cb885e 293 STATE = CALC;
elt14lpo 0:c81dc1cb885e 294 break;
elt14lpo 0:c81dc1cb885e 295
elt14lpo 0:c81dc1cb885e 296
elt14lpo 0:c81dc1cb885e 297 case CALC:
elt14lpo 0:c81dc1cb885e 298 DebugPrintState( std::cout << "Nucleo state is CALC: " << std::endl; );
elt14lpo 0:c81dc1cb885e 299 //Debug( wait(0.5); );
elt14lpo 0:c81dc1cb885e 300 arm_correlate_f32(p1, captureLength, p2, captureLength, p_res);
elt14lpo 0:c81dc1cb885e 301
elt14lpo 0:c81dc1cb885e 302 int positionOfMaxVal_1 = FindPeak(1);
elt14lpo 0:c81dc1cb885e 303 int positionOfMaxVal_2 = FindPeak(2);
elt14lpo 0:c81dc1cb885e 304 //run functions
elt14lpo 0:c81dc1cb885e 305 double timedelay = FindTimeDelay(positionOfMaxVal_1, positionOfMaxVal_2); //microseceonds
elt14lpo 0:c81dc1cb885e 306 if(abs(timedelay) > micDist/calcSoundSpeed(temp, hum)){
elt14lpo 0:c81dc1cb885e 307 STATE = CALC_ERROR;
elt14lpo 0:c81dc1cb885e 308 break;
elt14lpo 0:c81dc1cb885e 309 }
elt14lpo 0:c81dc1cb885e 310 double speed = calcSoundSpeed(temp, hum); //meters per second
elt14lpo 0:c81dc1cb885e 311 double distance = calcDist(timedelay/1000000, speed); //input converted to meters
elt14lpo 0:c81dc1cb885e 312 double angle = calcAng((double)distance, micDist); //0,15m = 15cm = 150mm, double type cast because of asin function in angle calculation
elt14lpo 0:c81dc1cb885e 313 //go to state SEND if no calc_error
elt14lpo 0:c81dc1cb885e 314
elt14lpo 0:c81dc1cb885e 315 Debug(
elt14lpo 0:c81dc1cb885e 316 std::cout << "max position for channel 1: " << positionOfMaxVal_1+1 << std::endl;
elt14lpo 0:c81dc1cb885e 317 std::cout << "max position for channel 2: " << positionOfMaxVal_2+1 << std::endl;
elt14lpo 0:c81dc1cb885e 318 std::cout << "run FindPeak, delay is: " << timedelay << "microseconds" << std::endl;
elt14lpo 0:c81dc1cb885e 319 std::cout << "run calcDist, delta s is: " << distance << " millimeters" << std::endl;
elt14lpo 0:c81dc1cb885e 320 std::cout << "run calcAngle, angle is: " << angle << " radians" << std::endl;
elt14lpo 0:c81dc1cb885e 321 std::cout << "run calcAngle, angle is: " << angle*(180 / PI) << " degrees" << std::endl;
elt14lpo 0:c81dc1cb885e 322 std::cout << "run convertAngToCamNbr, coordinates: "<< convertAngToCamNbr(angle)<<std::endl; //return "panNumber tiltNumber";
elt14lpo 0:c81dc1cb885e 323 );
elt14lpo 0:c81dc1cb885e 324 if (angle > (3 * PI )/2 || angle < 0 ) { //vinkel larger than 270 eller minde än noll
elt14lpo 0:c81dc1cb885e 325 STATE = CALC_ERROR;
elt14lpo 0:c81dc1cb885e 326 } else {
elt14lpo 0:c81dc1cb885e 327 STATE = SEND;
elt14lpo 0:c81dc1cb885e 328 }
elt14lpo 0:c81dc1cb885e 329 break;
elt14lpo 0:c81dc1cb885e 330
elt14lpo 0:c81dc1cb885e 331 case CALC_ERROR:
elt14lpo 0:c81dc1cb885e 332 DebugPrintState( std::cout << "Nucleo state is CALC_ERROR: " << std::endl; );
elt14lpo 0:c81dc1cb885e 333 Debug( wait(0.5); );
elt14lpo 0:c81dc1cb885e 334 //error message
elt14lpo 0:c81dc1cb885e 335 std::cout << "Error. angle not within limits 0 -270 degrees" << std::endl;
elt14lpo 0:c81dc1cb885e 336 //nollställ vektorer, , stoppa klockan , osv
elt14lpo 0:c81dc1cb885e 337 STATE = TESTNEW;
elt14lpo 0:c81dc1cb885e 338 break;
elt14lpo 0:c81dc1cb885e 339
elt14lpo 0:c81dc1cb885e 340 case SEND:
elt14lpo 0:c81dc1cb885e 341 DebugPrintState( std::cout << "Nucleo state is SEND: " << std::endl; );
elt14lpo 0:c81dc1cb885e 342 Debug( wait(0.5); );
elt14lpo 0:c81dc1cb885e 343 // send coordinates to serial port to camera
elt14lpo 0:c81dc1cb885e 344 std::cout<<convertAngToCamNbr(angle)<<std::endl; //return "panNumber tiltNumber";
elt14lpo 0:c81dc1cb885e 345 Debug( wait(0.5); );
elt14lpo 0:c81dc1cb885e 346 STATE = IDLE;
elt14lpo 0:c81dc1cb885e 347 wait(5);
elt14lpo 0:c81dc1cb885e 348 break;
elt14lpo 0:c81dc1cb885e 349 }
elt14lpo 0:c81dc1cb885e 350 }
elt14lpo 0:c81dc1cb885e 351 }
elt14lpo 0:c81dc1cb885e 352