base ultimate + line following HYBRID
Dependencies: ESC Motor PS_PAD hadah mbed Ping
Fork of Ultimate_Hybrid by
Diff: sensor_goertzel.txt
- Revision:
- 6:7fce519e4976
diff -r 256f6eac0358 -r 7fce519e4976 sensor_goertzel.txt --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/sensor_goertzel.txt Thu Apr 28 14:59:21 2016 +0000 @@ -0,0 +1,104 @@ +/* last edited : 15/02/2016 + * commit : optimized algorithm, default goertzel formula from library + */ + +#include <Goertzel.h> +#include <EEPROMex.h> +#include <Statistic.h> + +// ===================== DEKLARASI VARIABEL ============================= + +// goertzel constanta +const float TARGET_FREQUENCY = 445; +const float SAMPLING_FREQUENCY = 8900; +const int N = 20; +Goertzel goertzel = Goertzel(TARGET_FREQUENCY, N, SAMPLING_FREQUENCY); + +//EEPROM addres +int addr=0; + +//Sensor Pins +int pwm = 3; +int sensor[6] = {A0,A5,A4,A1,A3,A2}; + +// output pins +//int output[6] = {0,1,2,3,4,5,13,6,12,7,11,8,10}; +int calibrateButton = 2; + +//sensor data +float THRESHOLD[6]; +float magnitude[6]; +char data; + +Statistic stat; + +//========================================= +//=========================== MAIN PROGRAM ================================== + +void setup() { + Serial.begin(115200); + + EEPROM.setMaxAllowedWrites(500); + + pinMode(pwm, OUTPUT); + pinMode(calibrateButton, INPUT_PULLUP); + +} + +void loop() { + // sensor_running(); + //Serial.write(data); + //read saved THRESHOLD data + EEPROM.readBlock(addr, THRESHOLD,6); + + // PWM + tone(pwm, TARGET_FREQUENCY); + data = 0; + + //Sensor logic + for(int i=0; i<6; i++) + { + goertzel.sample(sensor[i]); //Will take n samples + magnitude[i] = goertzel.detect(); //check them for target_freq + + if(magnitude[i] > THRESHOLD[i]) + { + data |= 1 << i; + //Serial.print("1"); + //digitalWrite(output[i],HIGH); + } + else + { + data |= 0 << i; + //Serial.print("0"); + //digitalWrite(output[i],LOW); + } + + //Serial.print(THRESHOLD[i]); + //Serial.print(magnitude[i]); + //Serial.print("\t"); + + + } + + //Serial.println(); + /* ============= KALIBRASI ============== */ + + if(digitalRead(calibrateButton)== LOW) + { + //Serial.write(0xFF); + for(int i=0; i<6; i++) + { + stat.clear(); + for (int n=0; n<50; n++) + { + stat.add(magnitude[i]); + } + THRESHOLD[i] = stat.minimum() - 20; + } + EEPROM.updateBlock(addr,THRESHOLD,6); + } + else{ + Serial.write(data); + } +} \ No newline at end of file