KRAI 2016 / Mbed 2 deprecated Ultimate_Hybrid_LF

base ultimate + line following HYBRID

Dependencies:   ESC Motor PS_PAD hadah mbed Ping

Fork of Ultimate_Hybrid by KRAI 2016

sensor_goertzel.txt@6:7fce519e4976, 2016-04-28 (annotated)

Committer:
rizqicahyo
Date:
Thu Apr 28 14:59:21 2016 +0000
Revision:
6:7fce519e4976
tambah kodingan sensor arduino

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rizqicahyo 6:7fce519e4976 1 /* last edited : 15/02/2016
rizqicahyo 6:7fce519e4976 2 * commit : optimized algorithm, default goertzel formula from library
rizqicahyo 6:7fce519e4976 3 */
rizqicahyo 6:7fce519e4976 4
rizqicahyo 6:7fce519e4976 5 #include <Goertzel.h>
rizqicahyo 6:7fce519e4976 6 #include <EEPROMex.h>
rizqicahyo 6:7fce519e4976 7 #include <Statistic.h>
rizqicahyo 6:7fce519e4976 8
rizqicahyo 6:7fce519e4976 9 // ===================== DEKLARASI VARIABEL =============================
rizqicahyo 6:7fce519e4976 10
rizqicahyo 6:7fce519e4976 11 // goertzel constanta
rizqicahyo 6:7fce519e4976 12 const float TARGET_FREQUENCY = 445;
rizqicahyo 6:7fce519e4976 13 const float SAMPLING_FREQUENCY = 8900;
rizqicahyo 6:7fce519e4976 14 const int N = 20;
rizqicahyo 6:7fce519e4976 15 Goertzel goertzel = Goertzel(TARGET_FREQUENCY, N, SAMPLING_FREQUENCY);
rizqicahyo 6:7fce519e4976 16
rizqicahyo 6:7fce519e4976 17 //EEPROM addres
rizqicahyo 6:7fce519e4976 18 int addr=0;
rizqicahyo 6:7fce519e4976 19
rizqicahyo 6:7fce519e4976 20 //Sensor Pins
rizqicahyo 6:7fce519e4976 21 int pwm = 3;
rizqicahyo 6:7fce519e4976 22 int sensor[6] = {A0,A5,A4,A1,A3,A2};
rizqicahyo 6:7fce519e4976 23
rizqicahyo 6:7fce519e4976 24 // output pins
rizqicahyo 6:7fce519e4976 25 //int output[6] = {0,1,2,3,4,5,13,6,12,7,11,8,10};
rizqicahyo 6:7fce519e4976 26 int calibrateButton = 2;
rizqicahyo 6:7fce519e4976 27
rizqicahyo 6:7fce519e4976 28 //sensor data
rizqicahyo 6:7fce519e4976 29 float THRESHOLD[6];
rizqicahyo 6:7fce519e4976 30 float magnitude[6];
rizqicahyo 6:7fce519e4976 31 char data;
rizqicahyo 6:7fce519e4976 32
rizqicahyo 6:7fce519e4976 33 Statistic stat;
rizqicahyo 6:7fce519e4976 34
rizqicahyo 6:7fce519e4976 35 //=========================================
rizqicahyo 6:7fce519e4976 36 //=========================== MAIN PROGRAM ==================================
rizqicahyo 6:7fce519e4976 37
rizqicahyo 6:7fce519e4976 38 void setup() {
rizqicahyo 6:7fce519e4976 39 Serial.begin(115200);
rizqicahyo 6:7fce519e4976 40
rizqicahyo 6:7fce519e4976 41 EEPROM.setMaxAllowedWrites(500);
rizqicahyo 6:7fce519e4976 42
rizqicahyo 6:7fce519e4976 43 pinMode(pwm, OUTPUT);
rizqicahyo 6:7fce519e4976 44 pinMode(calibrateButton, INPUT_PULLUP);
rizqicahyo 6:7fce519e4976 45
rizqicahyo 6:7fce519e4976 46 }
rizqicahyo 6:7fce519e4976 47
rizqicahyo 6:7fce519e4976 48 void loop() {
rizqicahyo 6:7fce519e4976 49 // sensor_running();
rizqicahyo 6:7fce519e4976 50 //Serial.write(data);
rizqicahyo 6:7fce519e4976 51 //read saved THRESHOLD data
rizqicahyo 6:7fce519e4976 52 EEPROM.readBlock(addr, THRESHOLD,6);
rizqicahyo 6:7fce519e4976 53
rizqicahyo 6:7fce519e4976 54 // PWM
rizqicahyo 6:7fce519e4976 55 tone(pwm, TARGET_FREQUENCY);
rizqicahyo 6:7fce519e4976 56 data = 0;
rizqicahyo 6:7fce519e4976 57
rizqicahyo 6:7fce519e4976 58 //Sensor logic
rizqicahyo 6:7fce519e4976 59 for(int i=0; i<6; i++)
rizqicahyo 6:7fce519e4976 60 {
rizqicahyo 6:7fce519e4976 61 goertzel.sample(sensor[i]); //Will take n samples
rizqicahyo 6:7fce519e4976 62 magnitude[i] = goertzel.detect(); //check them for target_freq
rizqicahyo 6:7fce519e4976 63
rizqicahyo 6:7fce519e4976 64 if(magnitude[i] > THRESHOLD[i])
rizqicahyo 6:7fce519e4976 65 {
rizqicahyo 6:7fce519e4976 66 data |= 1 << i;
rizqicahyo 6:7fce519e4976 67 //Serial.print("1");
rizqicahyo 6:7fce519e4976 68 //digitalWrite(output[i],HIGH);
rizqicahyo 6:7fce519e4976 69 }
rizqicahyo 6:7fce519e4976 70 else
rizqicahyo 6:7fce519e4976 71 {
rizqicahyo 6:7fce519e4976 72 data |= 0 << i;
rizqicahyo 6:7fce519e4976 73 //Serial.print("0");
rizqicahyo 6:7fce519e4976 74 //digitalWrite(output[i],LOW);
rizqicahyo 6:7fce519e4976 75 }
rizqicahyo 6:7fce519e4976 76
rizqicahyo 6:7fce519e4976 77 //Serial.print(THRESHOLD[i]);
rizqicahyo 6:7fce519e4976 78 //Serial.print(magnitude[i]);
rizqicahyo 6:7fce519e4976 79 //Serial.print("\t");
rizqicahyo 6:7fce519e4976 80
rizqicahyo 6:7fce519e4976 81
rizqicahyo 6:7fce519e4976 82 }
rizqicahyo 6:7fce519e4976 83
rizqicahyo 6:7fce519e4976 84 //Serial.println();
rizqicahyo 6:7fce519e4976 85 /* ============= KALIBRASI ============== */
rizqicahyo 6:7fce519e4976 86
rizqicahyo 6:7fce519e4976 87 if(digitalRead(calibrateButton)== LOW)
rizqicahyo 6:7fce519e4976 88 {
rizqicahyo 6:7fce519e4976 89 //Serial.write(0xFF);
rizqicahyo 6:7fce519e4976 90 for(int i=0; i<6; i++)
rizqicahyo 6:7fce519e4976 91 {
rizqicahyo 6:7fce519e4976 92 stat.clear();
rizqicahyo 6:7fce519e4976 93 for (int n=0; n<50; n++)
rizqicahyo 6:7fce519e4976 94 {
rizqicahyo 6:7fce519e4976 95 stat.add(magnitude[i]);
rizqicahyo 6:7fce519e4976 96 }
rizqicahyo 6:7fce519e4976 97 THRESHOLD[i] = stat.minimum() - 20;
rizqicahyo 6:7fce519e4976 98 }
rizqicahyo 6:7fce519e4976 99 EEPROM.updateBlock(addr,THRESHOLD,6);
rizqicahyo 6:7fce519e4976 100 }
rizqicahyo 6:7fce519e4976 101 else{
rizqicahyo 6:7fce519e4976 102 Serial.write(data);
rizqicahyo 6:7fce519e4976 103 }
rizqicahyo 6:7fce519e4976 104 }