Delete proton_pack_state_machine.ino

Delete proton_pack_state_machine.ino

--- proton_pack_state_machine.ino
+++ proton_pack_state_machine.ino
@@ -1,605 +0,0 @@
-/* Proton pack lights sounds v2.0 (2021-10-13)
-
-This code drives two sets of two 74HC595N shift registers. One set controls the 'power cell' LEDs
-of the proton pack, the other set runs the 'bargraph' and two firing LEDs on the thrower/wand.
-My pack's cyclotron lights are controlled by a 4017 decade counter and 555 timer however one could
-use the remaining available pins of the arduino to control the cyclotron.
-Please note with this code that I'm not going for total screen accuracy, simply something that
-looks pretty close. However, the patterns should be fairly easy to understand and modify if you
-wish to produce something very authentic to that seen on screen (or even in the video game).
-Feel free to use the code in your own project, the only thing I ask is that you share any
-modifications, ideally also on gbfans.com forums.
-
-For an excellent overview of using shift registers with arduino please see this video:
-https://www.youtube.com/watch?v=Ys2fu4NINrA
-
- Martinux
-*/
-
-// ===============================================================================================
-// ADAFRUIT AUDIO FX BOARD
-// ===============================================================================================
-// UG to Ground
-// TX to #5
-// RX to #6
-// RST to #4
-// ACT to #3
-#include <SoftwareSerial.h>
-#include "Adafruit_Soundboard.h"
-
-#define SFX_TX 5
-#define SFX_RX 6
-#define SFX_RST 4  // Reset
-#define ACT 3
-SoftwareSerial ss = SoftwareSerial(SFX_TX, SFX_RX);
-Adafruit_Soundboard sfx = Adafruit_Soundboard(&ss, NULL, SFX_RST);
-
-// audio track names on soundboard
-char power_on_snd[] =     "T00     WAV";
-char firing_snd[] =       "T01     WAV";
-char stop_firing_snd[] =  "T02     WAV";
-char hum_long_snd[] =     "T03     WAV";
-char hum_snd[] =          "T03_SH~1WAV";
-char power_down_snd[] =   "T04     WAV";
-char clickTrack[] =       "T05     WAV";
-char charge_snd[] =       "T06     WAV";
-char overheat_snd[] =     "T07     WAV";
-char vent_snd[] =         "T08     WAV";
-char texTrack[] =         "T09     WAV";
-char choreTrack[] =       "T10     WAV";
-char toolsTrack[] =       "T11     WAV";
-char listenTrack[] =      "T12     WAV";
-char thatTrack[] =        "T13     WAV";
-char neutronizedTrack[] = "T14     WAV";
-char boxTrack[] =         "T15     WAV";
-char slowTheme[] =        "T16     OGG";
-
-int audio_playing;  // to keep track if a sound is playing.
-
-// ===============================================================================================
-// STATE MACHINE PHASE TRACKING
-// ===============================================================================================
-int current_pack_state = 0;
-int previous_pack_state = 0;
-int first_power_up = 1;
-
-// ===============================================================================================
-// TIMEKEEPING
-// ===============================================================================================
-unsigned long currentMillis = 0;
-unsigned long previousMillis_on = 0;
-unsigned long previousMillis_fire = 0;
-unsigned long off_interval = 30;
-unsigned long pwrcl_interval = 70;     // interval at which to cycle lights (milliseconds).
-unsigned long max_firing_interval = 60;
-unsigned long min_firing_interval = 40;
-unsigned long firing_interval = max_firing_interval;    // interval at which to cycle firing lights (milliseconds).
-unsigned long overheat_interval = 200;
-int max_wand_overheat_cycles = overheat_interval / min_firing_interval;
-int wand_overheat_cycles;
-
-// ===============================================================================================
-// BUTTONS
-// ===============================================================================================
-#define DEBOUNCE 5  // button debouncing.
-// Pins 14, 15 and 16 are Analog 0, 1 and 2 respectively.
-int power_button = 14;
-int firing_button = 15;
-int aux_button = 16;  // Currently held in reserve for use with Adafruit audio shield.
-int power_button_state;
-int firing_button_state;
-int aux_button_state;
-
-// ===============================================================================================
-// SHIFT REGISTERS
-// ===============================================================================================
-int clockPinPWR = 7;  // Power cell
-int dataPinPWR = 8;  // Power cell
-int latchPin = 9;
-int clockPinWand = 18;  // (18 is analog pin 4 on arduino)
-int dataPinWand = 19;   // (19 is analog pin 5 on arduino)
-
-// data array position trackers
-int power_cell_array_pos = 0;
-int wand_array_pos = 0;
-int wand_active_array_pos = 0;
-int overheat_array_pos = 0;
-// holders for information we're going to pass to shifting function
-byte dataPwrCell;
-byte dataWand;
-byte dataWandON;
-
-// ===============================================================================================
-// Populate light pattern arrays into flash memory to save RAM
-// ===============================================================================================
-// power cell LEDs 0 through 7 then LEDs 8 through 15 (two shift registers)
-const PROGMEM uint16_t PwrCell[] = {B10000000,B00000000,
-                                    B11000000,B00000000,
-                                    B11100000,B00000000,
-                                    B11110000,B00000000,
-                                    B11111000,B00000000,
-                                    B11111100,B00000000,
-                                    B11111110,B00000000,
-                                    B11111111,B00000000,
-                                    B11111111,B10000000,
-                                    B11111111,B11000000,
-                                    B11111111,B11100000,
-                                    B11111111,B11110000,
-                                    B11111111,B11111000,
-                                    B11111111,B11111100,
-                                    B11111111,B11111110
-                                    };
-
-//Proton Wand idle pattern - two shift registers
-const PROGMEM uint16_t Wand[] = {B01000000,B00000000,
-                                 B01100000,B00000000,
-                                 B01110000,B00000000,
-                                 B01111000,B00000000,
-                                 B01111100,B00000000,
-                                 B01111110,B00000000,
-                                 B01111111,B10000000,
-                                 B01111111,B11000000,
-                                 B01111111,B11100000,
-                                 B01111111,B11110000,
-                                 B01111111,B11111000,
-                                 B01111111,B11111100,
-                                 B01111111,B11111110,
-                                 B01111111,B11111100,
-                                 B01111111,B11111000,
-                                 B01111111,B11110000,
-                                 B01111111,B11100000,
-                                 B01111111,B11000000,
-                                 B01111111,B10000000,
-                                 B01111111,B00000000,
-                                 B01111110,B00000000,
-                                 B01111100,B00000000,
-                                 B01111000,B00000000,
-                                 B01110000,B00000000,
-                                 B01100000,B00000000,
-                                 B01000000,B00000000
-                                 };
-
-// Proton Wand firing pattern - two shift registers
-// 10000000 and 00000001 used for Wand barrel strobe
-const PROGMEM uint16_t WandON[] = {B11000000,B00000010,
-                                   B00100000,B00000101,
-                                   B10010000,B00001000,
-                                   B00001000,B00010001,
-                                   B10000100,B00100000,
-                                   B00000010,B01000001,
-                                   B10000001,B10000000,
-                                   B10000001,B10000000,
-                                   B10000001,B10000000,
-                                   B00000010,B01000001,
-                                   B10000100,B00100000,
-                                   B00001000,B00010001,
-                                   B10010000,B00001000,
-                                   B00100000,B00000101,
-                                   B11000000,B00000010
-                                   };
-
-// Overheat patterns
-const PROGMEM uint16_t OHWand1[] = {B00000000,
-                                    B01000000,
-                                    B01100000,
-                                    B01110000,
-                                    B01111000,
-                                    B01111100,
-                                    B01111110,
-                                    B01111111,
-                                    B01111111,
-                                    B01111111,
-                                    B01111111,
-                                    B01111111,
-                                    B01111111,
-                                    B01111111
-                                    };
-const PROGMEM uint16_t OHWand2[] = {B00000000,
-                                    B00000000,
-                                    B00000000,
-                                    B00000000,
-                                    B00000000,
-                                    B00000000,
-                                    B00000000,
-                                    B00000000,
-                                    B10000000,
-                                    B11000000,
-                                    B11100000,
-                                    B11110000,
-                                    B11111000,
-                                    B11111100
-                                    };
-const PROGMEM uint16_t OHPwrCell[] = {B00000000,B00000000,
-                                      B01111111,B11111110,
-                                      B00000000,B00000000,
-                                      B01111111,B11111110,
-                                      B00000000,B00000000,
-                                      B01111111,B11111110,
-                                      B00000000,B00000000,
-                                      B01111111,B11111110,
-                                      B00000000,B00000000,
-                                      B01111111,B11111110,
-                                      B00000000,B00000000,
-                                      B01111111,B11111110,
-                                      B00000000,B00000000,
-                                      B01111111,B11111110
-                                      };
-
-
-int freeRam(void) {
-    extern int  __bss_end;
-    extern int  *__brkval;
-    int free_memory;
-    if((int)__brkval == 0) {
-        free_memory = ((int)&free_memory) - ((int)&__bss_end);
-    }
-    else {
-        free_memory = ((int)&free_memory) - ((int)__brkval);
-    }
-    return free_memory;
-}
-
-
-void setup() {
-    // set up serial port
-    //Serial.begin(9600);
-    Serial.begin(115200);
-    while (!Serial);  // wait for serial port to connect. Needed for native USB
-
-    Serial.print(F("Free RAM: "));
-    Serial.println(freeRam());      // if this is under 150 bytes it may spell trouble!
-
-    // Make input & enable pull-up resistors on switch pins
-    pinMode(power_button, INPUT_PULLUP);
-    pinMode(firing_button, INPUT_PULLUP);
-    pinMode(aux_button, INPUT_PULLUP);
-
-    // Set pins for shift registers:
-    pinMode(latchPin, OUTPUT);      // shared between power cell and thrower
-    pinMode(clockPinPWR, OUTPUT);
-    pinMode(clockPinWand, OUTPUT);
-    pinMode(dataPinPWR, OUTPUT);
-    pinMode(dataPinWand, OUTPUT);
-    // Initialise Power cell and Wand lights
-    initialise_lights();
-
-    // softwareserial at 9600 baud
-    ss.begin(9600);
-    // can also do Serial1.begin(9600)
-
-    if (!sfx.reset()) {
-    Serial.println("Not found");
-        while (1);
-    }
-    Serial.println("SFX board found");
-    uint8_t files = sfx.listFiles();
-
-    Serial.println("File Listing");
-    Serial.println("========================");
-    Serial.println();
-    Serial.print("Found "); Serial.print(files); Serial.println(" Files");
-    for (uint8_t f = 0; f < files; f++) {
-        Serial.print(f);
-        Serial.print("\tname: "); Serial.print(sfx.fileName(f));
-        Serial.print("\tsize: "); Serial.println(sfx.fileSize(f));
-    }
-    Serial.println("========================");
-}
-
-
-void loop() {
-    audio_playing = digitalRead(ACT);  // False if audio playing.
-    pack_state();  // all the "magic" is done in this function
-    if (previous_pack_state != current_pack_state) {
-        Serial.print(F("STATE: "));
-        Serial.println(current_pack_state);
-    }
-}
-
-
-// ===============================================================================================
-// State machine
-// ===============================================================================================
-void pack_state() {
-    previous_pack_state = current_pack_state;
-    switch (current_pack_state) {
-        case 0:  // Pack default state & reset variables
-            //overheat_array_pos = 0;
-            firing_interval = max_firing_interval;
-            wand_overheat_cycles = max_wand_overheat_cycles;
-            current_pack_state = 1;  // next cycle move onto Start state
-            break;
-
-        case 1:  // Start
-            power_button_state = digitalRead(power_button);
-            if (power_button_state == LOW) {  // power switch pressed.
-                if (first_power_up) {
-                    playAudio(power_on_snd, audio_playing);
-                    Serial.println(F("First power on of pack."));
-                    first_power_up = 0;
-                }
-                else {
-                    current_pack_state = 2;
-                }
-            }
-            break;
-
-        case 2: // Powered on
-            power_button_state = digitalRead(power_button);
-            firing_button_state = digitalRead(firing_button);
-            // After playing power up play pack hum on loop:
-            if ( (power_button_state == LOW) && (audio_playing == 1) ) {
-                 playAudio(hum_snd, audio_playing);
-            }
-            if (power_button_state == HIGH) {
-                playAudio(power_down_snd, audio_playing);
-                first_power_up = 1;
-                current_pack_state = 3;  // Next cycle power down.
-            }
-            if (firing_button_state == LOW) {
-                playAudio(firing_snd, audio_playing);
-                current_pack_state = 4;  // Next cycle run firing routine.
-            }
-            powered_on();
-            break;
-
-        case 3: // Power down
-            powered_down();
-            break;
-
-        case 4: // Firing
-            power_button_state = digitalRead(power_button);
-            firing_button_state = digitalRead(firing_button);
-            if ((power_button_state == HIGH) && (firing_button_state == LOW)) {
-                current_pack_state = 3;
-            }
-            if (firing_button_state == HIGH) {
-                playAudio(stop_firing_snd, audio_playing);
-                current_pack_state = 5;
-            }
-            firing();
-            break;
-
-        case 5: // End firing
-            current_pack_state = 2;
-            break;
-
-        case 6: // Overheat
-            power_button_state = digitalRead(power_button);
-            firing_button_state = digitalRead(firing_button);
-            if (firing_button_state == HIGH) {
-                playAudio(stop_firing_snd, audio_playing);
-                overheat_array_pos = 0;
-                current_pack_state = 2;
-            }
-            overheat();
-            break;
-
-        case 7: // Overheat lockout (pack shuts down until vented)
-            firing_button_state = digitalRead(firing_button);
-            if (firing_button_state == HIGH) {
-                playAudio(vent_snd, audio_playing);
-                if (audio_playing == 1) {
-                     current_pack_state = 0;
-                }
-            }
-            break;
-
-    }
-}
-
-
-// ===============================================================================================
-// Pack states
-// ===============================================================================================
-void initialise_lights() {
-    digitalWrite(latchPin, 0);
-    shiftOut(dataPinPWR, clockPinPWR, MSBFIRST, B11000000);  // First 8 LEDs
-    shiftOut(dataPinPWR, clockPinPWR, MSBFIRST, B00000000);  // Second 8 LEDs
-    shiftOut(dataPinWand, clockPinWand, MSBFIRST, B01000000);
-    shiftOut(dataPinWand, clockPinWand, MSBFIRST, B00000000);
-    digitalWrite(latchPin, 1);
-}
-
-
-void powered_on() {
-    currentMillis = millis();
-    if (currentMillis - previousMillis_on > pwrcl_interval) {
-        previousMillis_on = currentMillis;
-        digitalWrite(latchPin, 0);  // Low to send data
-
-        // Load the light sequence from arrays then send data to shift registers:
-        dataPwrCell = pgm_read_word_near(PwrCell + power_cell_array_pos);
-        shiftOut(dataPinPWR, clockPinPWR, MSBFIRST, dataPwrCell);
-        power_cell_array_pos++;
-        dataPwrCell = pgm_read_word_near(PwrCell + power_cell_array_pos);
-        shiftOut(dataPinPWR, clockPinPWR, MSBFIRST, dataPwrCell);
-
-        dataWand = pgm_read_word_near(Wand + wand_array_pos);
-        shiftOut(dataPinWand, clockPinWand, MSBFIRST, dataWand);
-        wand_array_pos++;
-        dataWand = pgm_read_word_near(Wand + wand_array_pos);
-        shiftOut(dataPinWand, clockPinWand, MSBFIRST, dataWand);
-
-        digitalWrite(latchPin, 1);  // high to make shift register output data.
-
-        power_cell_array_pos++;
-        if (power_cell_array_pos > 29) {
-             power_cell_array_pos = 0;
-        }
-        wand_array_pos++;
-        if (wand_array_pos > 49) {
-            wand_array_pos = 0;
-        }
-    }
-}
-
-
-void powered_down() {
-    currentMillis = millis();
-    if (currentMillis - previousMillis_on > off_interval) {
-        previousMillis_on = currentMillis;
-        if (wand_array_pos != power_cell_array_pos) {
-            wand_array_pos = power_cell_array_pos;
-        }
-
-        digitalWrite(latchPin, 0);  // Low to send data
-        // Load the light sequence from arrays then send data to shift registers:
-        dataPwrCell = pgm_read_word_near(PwrCell + power_cell_array_pos);
-        shiftOut(dataPinPWR, clockPinPWR, MSBFIRST, dataPwrCell);
-        dataPwrCell = pgm_read_word_near(PwrCell + (power_cell_array_pos + 1));
-        shiftOut(dataPinPWR, clockPinPWR, MSBFIRST, dataPwrCell);
-
-        dataWand = pgm_read_word_near(Wand + wand_array_pos);
-        shiftOut(dataPinWand, clockPinWand, MSBFIRST, dataWand);
-        dataWand = pgm_read_word_near(Wand + (wand_array_pos + 1));
-        shiftOut(dataPinWand, clockPinWand, MSBFIRST, dataWand);
-        digitalWrite(latchPin, 1);  // high to make shift register show data.
-
-        power_cell_array_pos -= 2;
-        if (power_cell_array_pos < 0) {
-            power_cell_array_pos = 0;
-            current_pack_state = 0;
-        }
-        wand_array_pos -= 2;
-        if (wand_array_pos < 0) {
-            wand_array_pos = 0;
-            current_pack_state = 0;
-        }
-    }
-}
-
-
-void firing() {
-    currentMillis = millis();
-    if (currentMillis - previousMillis_on > firing_interval) {
-        previousMillis_on = currentMillis;
-        digitalWrite(latchPin, 0);  // Low to send data
-
-        dataPwrCell = pgm_read_word_near(PwrCell + power_cell_array_pos);
-        shiftOut(dataPinPWR, clockPinPWR, MSBFIRST, dataPwrCell);
-        power_cell_array_pos++;
-        dataPwrCell = pgm_read_word_near(PwrCell + power_cell_array_pos);
-        shiftOut(dataPinPWR, clockPinPWR, MSBFIRST, dataPwrCell);
-
-        dataWandON = pgm_read_word_near(WandON + wand_active_array_pos);
-        shiftOut(dataPinWand, clockPinWand, MSBFIRST, dataWandON);
-        wand_active_array_pos++;
-        dataWandON = pgm_read_word_near(WandON + wand_active_array_pos);
-        shiftOut(dataPinWand, clockPinWand, MSBFIRST, dataWandON);
-
-        digitalWrite(latchPin, 1);  // high to make shift register show data.
-
-        power_cell_array_pos++;
-        if (power_cell_array_pos > 28) {
-            firing_interval--;
-            Serial.print(F("Firing interval: "));
-            Serial.println(firing_interval);
-            power_cell_array_pos = 0;
-        }
-        wand_active_array_pos++;
-        if (wand_active_array_pos > 28) {
-            wand_active_array_pos = 0;
-        }
-        if (firing_interval == min_firing_interval) {
-            playAudio(overheat_snd, audio_playing);
-            wand_active_array_pos = 0;
-            current_pack_state = 6;
-        }
-    }
-}
-
-
-void overheat() {
-    currentMillis = millis();
-    if (currentMillis - previousMillis_fire > firing_interval) {
-        previousMillis_fire = currentMillis;
-        digitalWrite(latchPin, 0);  // Low to send data
-
-        dataWandON = pgm_read_word_near(WandON + wand_active_array_pos);
-        shiftOut(dataPinWand, clockPinWand, MSBFIRST, dataWandON);
-        wand_active_array_pos++;
-        dataWandON = pgm_read_word_near(WandON + wand_active_array_pos);
-        shiftOut(dataPinWand, clockPinWand, MSBFIRST, dataWandON);
-
-        digitalWrite(latchPin, 1);  // high to make shift register show data.
-
-        wand_active_array_pos++;
-        if (wand_active_array_pos > 28) {
-            wand_active_array_pos = 0;
-        }
-    }
-    if (currentMillis - previousMillis_on > overheat_interval) {
-        previousMillis_on = currentMillis;
-        /*
-        // 5x wand cycles for every powercell cycle
-        for (wand_overheat_cycles = 0; wand_overheat_cycles < max_wand_overheat_cycles; wand_overheat_cycles++) {
-            Serial.print("wand_overheat_cycles: ");
-            Serial.println(wand_overheat_cycles);
-            digitalWrite(latchPin, 0);  // Low to send data
-
-            dataWand = pgm_read_word_near(OHWand1 + wand_active_array_pos);
-            Serial.print(dataWand, BIN);
-            Serial.print(F("|"));
-            if (wand_overheat_cycles % 2 == 0) { // flip the strobe every other cycle
-                dataWand ^= 1 << 7;  // Red firing LED
-            }
-            Serial.print(dataWand, BIN);
-            shiftOut(dataPinWand, clockPinWand, MSBFIRST, dataWand);
-
-            Serial.print(F(" || "));
-
-            dataWand = pgm_read_word_near(OHWand2 + wand_active_array_pos);
-            Serial.print(dataWand, BIN);
-            Serial.print(F("|"));
-            if (wand_overheat_cycles % 2 == 1) { // flip the strobe every other cycle
-                dataWand ^= 1 << 0;  // Blue firing LED
-            }
-            Serial.println(dataWand, BIN);
-            shiftOut(dataPinWand, clockPinWand, MSBFIRST, dataWand);
-
-            digitalWrite(latchPin, 1);  // high to make shift register show data.
-        }
-        wand_active_array_pos++;
-        Serial.print(F("wand_active_array_pos: "));
-        Serial.println(wand_active_array_pos);
-        if (wand_active_array_pos > 28) {
-            wand_active_array_pos = 0;
-        }
-        */
-
-
-        digitalWrite(latchPin, 0);  // Low to send data
-
-        // Load the light sequence from arrays then send data to shift registers:
-        dataPwrCell = pgm_read_word_near(OHPwrCell + overheat_array_pos);
-        shiftOut(dataPinPWR, clockPinPWR, MSBFIRST, dataPwrCell);
-        overheat_array_pos++;
-        dataPwrCell = pgm_read_word_near(OHPwrCell + overheat_array_pos);
-        shiftOut(dataPinPWR, clockPinPWR, MSBFIRST, dataPwrCell);
-
-        digitalWrite(latchPin, 1);  // high to make shift register show data.
-
-        overheat_array_pos++;
-        if (overheat_array_pos > 26) {
-            Serial.println(F("Finished overheat cycle."));
-            overheat_array_pos = 0;
-            playAudio(stop_firing_snd, audio_playing);
-            initialise_lights();
-            current_pack_state = 7;
-        }
-    }
-}
-
-
-void playAudio( char* trackname, int audio_playing ) {
-  // stop track if one is going
-  if (audio_playing == 0) {
-    sfx.stop();
-  }
-
-  // now go play
-  if (sfx.playTrack(trackname)) {
-    sfx.unpause();
-  }
-}


Update E01_KY-040_RotaryEncoder.ino

Update E01_KY-040_RotaryEncoder.ino

--- RotaryEncoder/examples/E01_KY-040_RotaryEncoder/E01_KY-040_RotaryEncoder.ino
+++ RotaryEncoder/examples/E01_KY-040_RotaryEncoder/E01_KY-040_RotaryEncoder.ino
@@ -1,84 +1,84 @@
-//  Getting Started: E01 KY-040 RotaryEncoder Demo
-//
-//  Search string:
-//    "ky-040 rotary decoder encoder module for arduino"
-//
-//  Functions:
-//    Rotate CW:   count up         0..15, 0
-//    Rotate CCW:  count down       0, 15..0
-//    Push Button: reset counter to 0
-//
-//
-//  Wiring:
-//         KY-040                 Arduino AVR (UNO, Nano, etc.)
-//    +-------------+
-//    |             |--- CLK  >   o Pin 3
-//    |             |--- DAT  >   o Pin 2
-//    |   top view  |--- SW   >   o Pin A1
-//    |             |--- VCC  >   o 5.0V
-//    |             |--- GND  >   o GND
-//    +-------------+
-//
-//     No external RC- or R-parts are used. Debouncing will be supported by Rotary Encoder library 'RotaryEncoder.h'
-//
-//  Copyright (c) 2015 Thomas Walloschke (artkeller at gmx.de). All rights reserved.
-//
-//  Permission is hereby granted, free of charge, to any person obtaining a copy
-//  of this software and associated documentation files (the "Software"), to deal
-//  in the Software without restriction, including without limitation the rights
-//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-//  copies of the Software, and to permit persons to whom the Software is
-//  furnished to do so, subject to the following conditions:
-//
-//  The above copyright notice and this permission notice shall be included in
-//  all copies or substantial portions of the Software.
-//
-//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-//  THE SOFTWARE.
-
-// ***************************************************************
-
-#if ARDUINO >= 100
-#include "Arduino.h"
-#else
-#include "WProgram.h"
-#endif
-
-#include <RotaryEncoder.h>
-
-// Default Rotary Encoder Object
-RotaryEncoder rotaryEncoder = RotaryEncoder();  // CLK = pin 3, DAT = pin 2, SW = pin A1, CW = count up, CCW = count down
-
-// ***************************************************************
-
-void setup() {
-  Serial.begin(115200);
-  Serial.print(F("E01 KY-040 RotaryEncoder Demo"));
-  Serial.println(F(" (compiled " __DATE__ " " __TIME__ ")"));
-}
-
-// ***************************************************************
-
-// Main loop
-// All the magic is done in the library, this one prints stuff
-void loop() {                                                   // apply Rotary Encoder methods
-
-  // Get Rotary Encoder status
-  rotaryEncoder.startDebouncer();                               // has to be inside loop to enable debouncer
-  rotaryEncoder.updateClearButton();                            // enable Button to clear recent encoder position if pushed, set to 0
-
-  // Print changes
-  if (rotaryEncoder.hasChanged()) {                             // verify if encoder position has changed
-    volatile byte encoderPos = rotaryEncoder.encodedPos() % 16; // update counter for the dial (values 0..15)
-    Serial.print(F("Position: "));
-    Serial.println(encoderPos, DEC);
-  }
-}
-
-// ***************************************************************
+//  Getting Started: E01 KY-040 RotaryEncoder Demo
+//
+//  Web Search String:
+//    "ky-040 rotary decoder encoder module for arduino"
+//
+//  Functions:
+//    Rotate CW:   count up         0..15, 0
+//    Rotate CCW:  count down       0, 15..0
+//    Push Button: reset counter to 0
+//
+//
+//  Wiring:
+//         KY-040                 Arduino AVR (UNO, Nano, etc.)
+//    +-------------+
+//    |             |--- CLK  >   o Pin 3
+//    |             |--- DAT  >   o Pin 2
+//    |   top view  |--- SW   >   o Pin A1
+//    |             |--- VCC  >   o 5.0V
+//    |             |--- GND  >   o GND
+//    +-------------+
+//
+//     No external RC- or R-parts are used. Debouncing will be supported by Rotary Encoder library 'RotaryEncoder.h'
+//
+//  Copyright (c) 2015 Thomas Walloschke (artkeller at gmx.de). All rights reserved.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+//
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+//
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.

+// ***************************************************************
+
+#if ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+#include <RotaryEncoder.h>
+
+// Default Rotary Encoder Object
+RotaryEncoder rotaryEncoder = RotaryEncoder();  // CLK = pin 3, DAT = pin 2, SW = pin A1, CW = count up, CCW = count down
+
+// ***************************************************************
+
+void setup() {
+  Serial.begin(115200);
+  Serial.print(F("E01 KY-040 RotaryEncoder Demo"));
+  Serial.println(F(" (compiled " __DATE__ " " __TIME__ ")"));
+}
+
+// ***************************************************************
+
+// Main loop
+// All the magic is done in the library, this one prints stuff
+void loop() {                                                   // apply Rotary Encoder methods
+
+  // Get Rotary Encoder status
+  rotaryEncoder.startDebouncer();                               // has to be inside loop to enable debouncer
+  rotaryEncoder.updateClearButton();                            // enable Button to clear recent encoder position if pushed, set to 0
+
+  // Print changes
+  if (rotaryEncoder.hasChanged()) {                             // verify if encoder position has changed
+    volatile byte encoderPos = rotaryEncoder.encodedPos() % 16; // update counter for the dial (values 0..15)
+    Serial.print(F("Position: "));
+    Serial.println(encoderPos, DEC);
+  }
+}
+
+// ***************************************************************
+


Turn to angle

Turn to angle

--- firefighter/firefighter.ino
+++ firefighter/firefighter.ino
@@ -1,640 +1,640 @@
-#include "Ultrasonic.h"
-#include "ColorSensor.h"
-#include "Motor.h"
-#include "FreqCount.h"
-#include <Servo.h>
-
-#include "I2Cdev.h"
-#include "MPU6050_6Axis_MotionApps20.h"
-//#include "MPU6050.h" // not necessary if using MotionApps include file
-
-#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
-    #include "Wire.h"
-#endif
-
-#define SOUND_LED 45
-#define FLAME_LED 53
-
-#define WALL_DISTANCE 7
-
-#define flame_sensor 30
-#define servo_pin 31
-#define interval 10
-
-#define LOW_START 3230
-#define HIGH_START 4370
-
-#define LEFT_MOTOR_1A 44
-#define LEFT_MOTOR_1B 3
-
-#define INTERRUPT_PIN 2
-
-// PID
-#define setpoint 0
-#define kP 0.05
-
-Servo extingusher_servo;
-
-Motor leftMotor1(LEFT_MOTOR_1A, LEFT_MOTOR_1B);
-Motor leftMotor2(4, 5);
-Motor rightMotor1(6, 7);
-Motor rightMotor2(8, 9);
-Motor strafeMotor(10, 11);
-Motor fan_motor(12, 13);
-
-ColorSensor colour_sensor(42, 46, 48, 50, 52);
-
-Ultrasonic frontUltrasonic(28, 29, true);
-Ultrasonic leftUltrasonic(26, 27, true);
-Ultrasonic rightUltrasonic(24, 25, true);
-Ultrasonic backUltrasonic(22, 23, true);
-
-MPU6050 mpu; // Default address 0x68
-
-// Drivetrain commands
-void driveToWhite();
-void driveThroughRoom();
-void hDrive(double move, double rotate, double strafe);
-void tankDrive(double leftSpeed, double rightSpeed, double strafeSpeed);
-void definedStartSearch();
-double getPercentError(double actual, double target);
-void turnToAngle(double targetAngle, double speed);
-void naviguessMaze(double swagSpeed);
-
-// Encoder commands
-void countPulseRight();
-int pulseRight;
-const byte encoderARight = 18;
-const byte encoderBRight = 16;
-byte encoderALastRight;
-bool directionRight;
-
-int pulseLeft;
-void countPulseLeft();
-const byte encoderALeft = 19;
-const byte encoderBLeft = 17;
-byte encoderALastLeft;
-bool directionLeft;
-
-double getAngle();
-double getDistanceLeft();
-double getDistanceRight();
-void resetEncoders();
-
-// Sound Sensor
-bool robotOn = false;
-unsigned long count;
-
-// Extinguisher
-void extinguish();
-
-int startSwitch = 14;
-
-// Gyro
-bool dmpReady = false;  // set true if DMP init was successful
-uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
-uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
-uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
-uint16_t fifoCount;     // count of all bytes currently in FIFO
-uint8_t fifoBuffer[64]; // FIFO storage buffer
-
-// orientation/motion vars
-Quaternion q;           // [w, x, y, z]         quaternion container
-float euler[3];         // [psi, theta, phi]    Euler angle container
-
-double gyroResetValX;
-double gyroResetValY;
-double gyroResetValZ;
-void resetGyro(char axis);
-double getAngle(char axis);
-
-int ri;
-
-bool pidEnabled = false;
-
-volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
-void dmpDataReady() {
-    mpuInterrupt = true;
-}
-
-void setup() {
-  // join I2C bus (I2Cdev library doesn't do this automatically)
-  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
-      Wire.begin();
-      Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
-  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
-      Fastwire::setup(400, true);
-  #endif
-
-  Serial.begin(9600);
-
-  // Start Switch
-  pinMode(startSwitch, INPUT);
-
-  // Status Lights
-  pinMode(SOUND_LED, OUTPUT);
-  pinMode(FLAME_LED, OUTPUT);
-
-  // Right Encoder
-  directionRight = true;//default -> Forward
-  pinMode(encoderBRight,INPUT);
-  attachInterrupt(5, countPulseRight, CHANGE);
-
-  // Left Encoder
-  directionLeft = true;//default -> Forward
-  pinMode(encoderBLeft,INPUT);
-  attachInterrupt(4, countPulseLeft, CHANGE);
-
-  // Extinguisher
-  pinMode(flame_sensor, INPUT);
-
-  ri = -1;
-
-  extingusher_servo.attach(servo_pin);
-  extingusher_servo.write(90);
-
-  mpu.initialize();
-  pinMode(INTERRUPT_PIN, INPUT);
-
-  devStatus = mpu.dmpInitialize();
-
-  // accel offsets
-  mpu.setXAccelOffset(-4265);
-  mpu.setYAccelOffset(-283);
-  mpu.setZAccelOffset(1675); // 1688 factory default for my test chip
-  // supply your own gyro offsets here, scaled for min sensitivity
-  mpu.setXGyroOffset(-51);
-  mpu.setYGyroOffset(40);
-  mpu.setZGyroOffset(27);
-
-  // make sure it worked (returns 0 if so)
-  if (devStatus == 0) {
-      // turn on the DMP, now that it's ready
-      mpu.setDMPEnabled(true);
-
-      // enable Arduino interrupt detection
-      attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
-      mpuIntStatus = mpu.getIntStatus();
-
-      // set our DMP Ready flag so the main loop() function knows it's okay to use it
-      Serial.println(F("DMP ready! Waiting for first interrupt..."));
-      dmpReady = true;
-
-      // get expected DMP packet size for later comparison
-      packetSize = mpu.dmpGetFIFOPacketSize();
-  }
-
-  resetGyro('z');
-
-  FreqCount.begin(1000);
-}
-
-bool firstStart = false;
-bool foundFlame = false;
-
-void loop() {
-  // put your main code here, to run repeatedly:
-  if (FreqCount.available()) {
-    count = FreqCount.read();
-	  //Serial.println(count);
-  } else {
-    count = 0;
-  }
-//  Serial.println(getAngle('z'));
-  if ((count > 3230 && count < 4370) || digitalRead(startSwitch)) {
-    digitalWrite(SOUND_LED, HIGH);
-    firstStart = true;
-    robotOn = !robotOn;
-    resetGyro('z');
-  }
-
-//  Serial.println(getAngle('z'));
-  if(robotOn){
-    if (firstStart) {
-      FreqCount.end();
-      ri = colour_sensor.getColor('r');
-    }
-
-  	while(!foundFlame && digitalRead(flame_sensor)) {
-  		// naviguessMaze(0.3);
-  		// followWall(1.0);
-       ultrasonicCheck();
-      // drivetrainCheck();
-
-//      if (!pidEnabled) {
-//        pidEnabled = true;
-//        resetGyro('z');
-//      }
-//      hDrive(0.5, getPidOutput(), 0.0);   // Correctional driving
-//      Serial.println(getPidOutput());
-
-  	}
-
-  	foundFlame = true;
-  	digitalWrite(FLAME_LED, foundFlame);
-
-  	fan_motor.set(1.0);
-
-  	hDrive(0.0, 0.0, 0.0);
-  	delay(100);
-
-  	while (frontUltrasonic.getDistance() > 0.5) {
-  		hDrive(0.3, 0.0, 0.0);
-  		extingusher_servo.write(110);
-  		delay(600);
-  		extingusher_servo.write(70);
-  		delay(600);
-  		hDrive(0.0, 0.0, 0.0);
-  		delay(200);
-  		digitalWrite(FLAME_LED, !digitalRead(flame_sensor));
-  	}
-
-	  hDrive(0.0, 0.0, 0.0);
-  }
-  else{
-    digitalWrite(SOUND_LED, LOW);
-    firstStart = false;
-    hDrive(0.0, 0.0, 0.0);
-  }
-
-//  delay(100);
-}
-
-void hDrive(double move, double rotate, double strafe) {
-  double leftSpeed, rightSpeed;
-
-  move = constrain(move, -1.0, 1.0);
-  rotate = constrain(rotate, -1.0, 1.0);
-  strafe = constrain(strafe, -1.0, 1.0);
-
-  if (move > 0.0) {
-    if (rotate > 0.0) {
-      leftSpeed =  move - rotate;
-      rightSpeed = max(move, rotate);
-    } else {
-      leftSpeed = max(move, -rotate);
-      rightSpeed = move + rotate;
-    }
-  } else {
-    if (rotate > 0.0) {
-      leftSpeed = -max(-move, rotate);
-      rightSpeed = move + rotate;
-    } else {
-      leftSpeed = move - rotate;
-      rightSpeed = -max(-move, -rotate);
-    }
-  }
-
-  leftMotor1.set(constrain(leftSpeed, -1.0, 1.0));
-  leftMotor2.set(constrain(leftSpeed, -1.0, 1.0));
-  rightMotor1.set(-constrain(rightSpeed, -1.0, 1.0));
-  rightMotor2.set(-constrain(rightSpeed, -1.0, 1.0));
-  strafeMotor.set(strafe);
-}
-
-void drivetrainCheck() {
-  hDrive(0.3, 0.0, 0.0); delay(300); hDrive(-0.3, 0.0, 0.0); delay(300);
-  hDrive(0.0, 0.3, 0.0); delay(300); hDrive(0.0, -0.3, 0.0); delay(300);
-  hDrive(0.0, 0.0, 0.3); delay(300); hDrive(0.0, 0.0, -0.3); delay(300);
-}
-
-void ultrasonicCheck() {
-	Serial.println("====================================");
-	Serial.print("F: ");
-	Serial.println(frontUltrasonic.getDistance());
-	Serial.print("L: ");
-	Serial.println(leftUltrasonic.getDistance());
-	Serial.print("R: ");
-	Serial.println(rightUltrasonic.getDistance());
-	Serial.print("B: ");
-	Serial.println(backUltrasonic.getDistance());
-	delay(300);
-}
-
-void driveToWhite() {
-  int ri = colour_sensor.getColor('r');
-  int r = ri;
-//  int g = colour_sensor.getColor('g');
-//  int b = colour_sensor.getColor('b');
-//  String colour = String(ri) + " " + String(g) + " " + String(b);
-//  Serial.println(colour);
-  hDrive(.3,0,0);
-  Serial.print("Initial: ");
-  Serial.println(ri);
-
-  while(r > ri - 70) {
-    r = colour_sensor.getColor('r');
-//        String colour = String(r);
-//        Serial.println(colour);
-  }
-
-  Serial.print("Final: ");
-  Serial.println(r);
-
-//      Serial.println("Done");
-  hDrive(0.0, 0.0, 0.0);
-}
-
-void driveThroughRoom() {
-  if (frontUltrasonic.getDistance() < WALL_DISTANCE) {
-    if (leftUltrasonic.getDistance() < WALL_DISTANCE) {
-      hDrive(0.0, 0.0, 1.0);
-    } else {
-      hDrive(0.0, 0.0, -1.0);
-    }
-  } else {
-    if (leftUltrasonic.getDistance() < WALL_DISTANCE) {
-      hDrive(0.0, 0.0, 1.0);
-    } else {
-      hDrive(0.0, 0.0, -1.0);
-    }
-  }
-}
-
-void definedStartSearch() {
-  // Start A
-  if (rightUltrasonic.getDistance() < WALL_DISTANCE) {
-    hDrive(0.0, 0.0, -1.0);
-    delay(500);
-    while (frontUltrasonic.getDistance() < WALL_DISTANCE) { }
-    hDrive(0.0, 0.0, 0.0);
-  // Start B
-  } else {
-    hDrive(1.0, 0.0, 0.0);
-    while (frontUltrasonic.getDistance() > WALL_DISTANCE) { }
-    hDrive(0.0, 0.0, 0.0);
-  }
-}
-
-void countPulseRight() {
-  int Lstate = digitalRead(encoderARight);
-  if((encoderALastRight == LOW) && Lstate==HIGH)
-  {
-    int val = digitalRead(encoderBRight);
-    if(val == LOW && directionRight)
-    {
-      directionRight = false; //Reverse
-    } else if(val == HIGH && !directionRight) {
-      directionRight = true;
-    }
-  }
-  encoderALastRight = Lstate;
-
-  if(!directionRight)  pulseRight--;
-  else  pulseRight++;
-}
-
-void countPulseLeft() {
-  int Lstate = digitalRead(encoderALeft);
-  if((encoderALastLeft == LOW) && Lstate==HIGH)
-  {
-    int val = digitalRead(encoderBLeft);
-    if(val == LOW && directionLeft)
-    {
-      directionLeft = false; //Reverse
-    } else if(val == HIGH && !directionLeft) {
-      directionLeft = true;
-    }
-  }
-  encoderALastLeft = Lstate;
-
-  if(!directionLeft)  pulseLeft++;
-  else  pulseLeft--;
-}
-
-double getAngle() {
-  return (180 / PI) * (getDistanceLeft() - getDistanceRight()) / 11.5;
-}
-
-double getDistanceLeft() {
-	return (pulseLeft * 12.5) / 1356;
-}
-
-double getDistanceRight() {
-	return (pulseRight * 12.5) / 1356;
-}
-
-void resetEncoders() {
-	pulseLeft = pulseRight = 0;
-}
-
-double getPercentError(double actual, double target) {
-  return abs(actual - target) / target;
-}
-
-void turnToAngle(double targetAngle, double speed) {
-  resetEncoders();
-
-  hDrive(0.0, speed, 0.0);
-
-  while (abs(getAngle()) < abs(targetAngle)) {
-    Serial.println(getAngle());
-  }
-
-  hDrive(0.0, 0.0, 0.0);
-}
-
-double getPidOutput() {
-  return (setpoint - getAngle('x')) * kP;
-}
-
-void extinguish() {
-  for(int i = 40; i <= 160; i += interval){
-    extingusher_servo.write(i);
-    delay(15*interval);//give time for servo to move
-
-    if(!digitalRead(flame_sensor))
-      digitalWrite(FLAME_LED, HIGH);
-
-    //turn on fan
-    fan_motor.set(1);
-    while(!digitalRead(flame_sensor)){
-      //while there is a fire
-      delay(1000);
-    }
-    //turn off fan
-    fan_motor.set(0);
-    digitalWrite(FLAME_LED, LOW);
-  }
-
-  for(int i = 140; i >=20; i -= interval){
-    extingusher_servo.write(i);
-    delay(15*interval);//give time for servo to move
-
-    if(!digitalRead(flame_sensor))
-      digitalWrite(FLAME_LED, HIGH);
-
-    //turn on fan
-    fan_motor.set(1);
-    while(!digitalRead(flame_sensor)){
-      //while there is a fire
-      delay(1000);
-    }
-    //turn off fan
-    fan_motor.set(0);
-    digitalWrite(FLAME_LED, LOW);
-  }
-}
-
-bool direction = true;
-int leftTurns = 0;
-int rightTurns = 0;
-
-void naviguessMaze(double swagSpeed) {
-	bool frontTriggered = frontUltrasonic.getDistance() < 2;
-	bool leftTriggered = leftUltrasonic.getDistance() < 3;
-	bool rightTriggered = rightUltrasonic.getDistance() < 3;
-	bool backTriggered = backUltrasonic.getDistance() < 2;
-
-	if (leftTriggered && frontTriggered) {
-		hDrive(-swagSpeed, 0.0, 0.0);
-		delay(100);
-		// hDrive(0.0, swagSpeed , 0.0);
-
-		if (rightTurns == 4) {
-			turnToAngle(-90, swagSpeed);
-			rightTurns = 0;
-		} else {
-			turnToAngle(90, -swagSpeed);
-			rightTurns++;
-		}
-	} else if (rightTriggered && frontTriggered) {
-		hDrive(-swagSpeed, 0.0, 0.0);
-		delay(100);
-		// hDrive(0.0, -swagSpeed , 0.0);
-
-		if (leftTurns == 4) {
-			turnToAngle(-90, -swagSpeed);
-			leftTurns = 0;
-		} else {
-			turnToAngle(-90, swagSpeed);
-			leftTurns++;
-		}
-	} else if (leftTriggered && !frontTriggered) {
-		// hDrive(swagSpeed - 0.2, -swagSpeed, 0.0);
-		turnToAngle(2, -swagSpeed);
-	} else if (rightTriggered && !frontTriggered) {
-		//hDrive(swagSpeed - 0.2, swagSpeed , 0.0);
-		turnToAngle(-2, swagSpeed);
-	} else if (frontTriggered && !rightTriggered & !leftTriggered){
-		hDrive(-swagSpeed, 0.0, 0.0);
-		delay(50);
-		if (rightTurns == 4) {
-			turnToAngle(-90, swagSpeed);
-			rightTurns = 0;
-		} else {
-			turnToAngle(90, -swagSpeed);
-			rightTurns++;
-		}
-		// turnToAngle(90, -swagSpeed);
-		// rightTurns++;
-	} else if (backTriggered) {
-		hDrive(swagSpeed, swagSpeed, 0.0);
-	} else {
-		hDrive(swagSpeed, 0.0, 0.0);
-	}
-
-	if (direction) {
-		strafeMotor.set(0.3);
-	} else {
-		strafeMotor.set(-0.3);
-	}
-
-	direction = !direction;
-
-	delay(50);
-}
-
-void tankDrive(double leftSpeed, double rightSpeed, double strafeSpeed) {
-	leftMotor1.set(constrain(leftSpeed, -1.0, 1.0));
-	leftMotor2.set(constrain(leftSpeed, -1.0, 1.0));
-	rightMotor1.set(-constrain(rightSpeed, -1.0, 1.0));
-	rightMotor2.set(-constrain(rightSpeed, -1.0, 1.0));
-	strafeMotor.set(strafeSpeed);
-}
-
-void followWall(double targetDistance) {
-	if (rightUltrasonic.getDistance() < targetDistance) {
-		tankDrive(0.0, 0.3, 0.0);
-	} else if (rightUltrasonic.getDistance() > targetDistance) {
-		tankDrive(0.3, 0.0, 0.0);
-	} else {
-		tankDrive(0.3, 0.4, 0.0);
-	}
-}
-
-double getAngle(char axis) {
-  // if programming failed, don't try to do anything
-    if (!dmpReady) return;
-
-    // wait for MPU interrupt or extra packet(s) available
-    while (!mpuInterrupt && fifoCount < packetSize) {
-        // other program behavior stuff here
-        // .
-        // .
-        // .
-        // if you are really paranoid you can frequently test in between other
-        // stuff to see if mpuInterrupt is true, and if so, "break;" from the
-        // while() loop to immediately process the MPU data
-        // .
-        // .
-        // .
-    }
-
-    // reset interrupt flag and get INT_STATUS byte
-    mpuInterrupt = false;
-    mpuIntStatus = mpu.getIntStatus();
-
-    // get current FIFO count
-    fifoCount = mpu.getFIFOCount();
-
-    // check for overflow (this should never happen unless our code is too inefficient)
-    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
-        // reset so we can continue cleanly
-        mpu.resetFIFO();
-        Serial.println(F("FIFO overflow!"));
-
-    // otherwise, check for DMP data ready interrupt (this should happen frequently)
-    } else if (mpuIntStatus & 0x02) {
-        // wait for correct available data length, should be a VERY short wait
-        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
-
-        // read a packet from FIFO
-        mpu.getFIFOBytes(fifoBuffer, packetSize);
-
-        // track FIFO count here in case there is > 1 packet available
-        // (this lets us immediately read more without waiting for an interrupt)
-        fifoCount -= packetSize;
-
-        // display Euler angles in degrees
-        mpu.dmpGetQuaternion(&q, fifoBuffer);
-        mpu.dmpGetEuler(euler, &q);
-
-        switch (axis) {
-          case 'x':
-            return (euler[2] * 180/M_PI) - gyroResetValX;
-            break;
-          case 'y':
-            return (euler[1] * 180/M_PI) - gyroResetValY;
-            break;
-          case 'z':
-            return (euler[0] * 180/M_PI) - gyroResetValZ;
-            break;
-        }
-    }
-}
-
-void resetGyro(char axis) {
-  switch (axis) {
-    case 'x':
-      gyroResetValX += getAngle('x');
-      break;
-    case 'y':
-      gyroResetValY += getAngle('y');
-      break;
-    case 'z':
-      gyroResetValZ += getAngle('z');
-      break;
-  }
-}
-
+#include "Ultrasonic.h"
+#include "ColorSensor.h"
+#include "Motor.h"
+#include "FreqCount.h"
+#include <Servo.h>
+
+#include "I2Cdev.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+//#include "MPU6050.h" // not necessary if using MotionApps include file
+
+#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
+    #include "Wire.h"
+#endif
+
+#define SOUND_LED 45
+#define FLAME_LED 53
+
+#define WALL_DISTANCE 7
+
+#define flame_sensor 30
+#define servo_pin 31
+#define interval 10
+
+#define LOW_START 3230
+#define HIGH_START 4370
+
+#define LEFT_MOTOR_1A 44
+#define LEFT_MOTOR_1B 3
+
+#define INTERRUPT_PIN 2
+
+// PID
+#define setpoint 0
+#define kP 0.05
+
+Servo extingusher_servo;
+
+Motor leftMotor1(LEFT_MOTOR_1A, LEFT_MOTOR_1B);
+Motor leftMotor2(4, 5);
+Motor rightMotor1(6, 7);
+Motor rightMotor2(8, 9);
+Motor strafeMotor(10, 11);
+Motor fan_motor(12, 13);
+
+ColorSensor colour_sensor(42, 46, 48, 50, 52);
+
+Ultrasonic frontUltrasonic(28, 29, true);
+Ultrasonic leftUltrasonic(26, 27, true);
+Ultrasonic rightUltrasonic(24, 25, true);
+Ultrasonic backUltrasonic(22, 23, true);
+
+MPU6050 mpu; // Default address 0x68
+
+// Drivetrain commands
+void driveToWhite();
+void driveThroughRoom();
+void hDrive(double move, double rotate, double strafe);
+void tankDrive(double leftSpeed, double rightSpeed, double strafeSpeed);
+void definedStartSearch();
+double getPercentError(double actual, double target);
+void turnToAngle(double targetAngle, double speed);
+void naviguessMaze(double swagSpeed);
+
+// Encoder commands
+void countPulseRight();
+int pulseRight;
+const byte encoderARight = 18;
+const byte encoderBRight = 16;
+byte encoderALastRight;
+bool directionRight;
+
+int pulseLeft;
+void countPulseLeft();
+const byte encoderALeft = 19;
+const byte encoderBLeft = 17;
+byte encoderALastLeft;
+bool directionLeft;
+
+double getAngle();
+double getDistanceLeft();
+double getDistanceRight();
+void resetEncoders();
+
+// Sound Sensor
+bool robotOn = false;
+unsigned long count;
+
+// Extinguisher
+void extinguish();
+
+int startSwitch = 14;
+
+// Gyro
+bool dmpReady = false;  // set true if DMP init was successful
+uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
+uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
+uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount;     // count of all bytes currently in FIFO
+uint8_t fifoBuffer[64]; // FIFO storage buffer
+
+// orientation/motion vars
+Quaternion q;           // [w, x, y, z]         quaternion container
+float euler[3];         // [psi, theta, phi]    Euler angle container
+
+double gyroResetValX;
+double gyroResetValY;
+double gyroResetValZ;
+void resetGyro(char axis);
+double getAngle(char axis);
+
+int ri;
+
+bool pidEnabled = false;
+
+volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
+void dmpDataReady() {
+    mpuInterrupt = true;
+}
+
+void setup() {
+  // join I2C bus (I2Cdev library doesn't do this automatically)
+  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
+      Wire.begin();
+      Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
+  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
+      Fastwire::setup(400, true);
+  #endif
+
+  Serial.begin(9600);
+
+  // Start Switch
+  pinMode(startSwitch, INPUT);
+
+  // Status Lights
+  pinMode(SOUND_LED, OUTPUT);
+  pinMode(FLAME_LED, OUTPUT);
+
+  // Right Encoder
+  directionRight = true;//default -> Forward
+  pinMode(encoderBRight,INPUT);
+  attachInterrupt(5, countPulseRight, CHANGE);
+
+  // Left Encoder
+  directionLeft = true;//default -> Forward
+  pinMode(encoderBLeft,INPUT);
+  attachInterrupt(4, countPulseLeft, CHANGE);
+
+  // Extinguisher
+  pinMode(flame_sensor, INPUT);
+
+  ri = -1;
+
+  extingusher_servo.attach(servo_pin);
+  extingusher_servo.write(90);
+
+  mpu.initialize();
+  pinMode(INTERRUPT_PIN, INPUT);
+
+  devStatus = mpu.dmpInitialize();
+
+  // accel offsets
+  mpu.setXAccelOffset(-4265);
+  mpu.setYAccelOffset(-283);
+  mpu.setZAccelOffset(1675); // 1688 factory default for my test chip
+  // supply your own gyro offsets here, scaled for min sensitivity
+  mpu.setXGyroOffset(-51);
+  mpu.setYGyroOffset(40);
+  mpu.setZGyroOffset(27);
+
+  // make sure it worked (returns 0 if so)
+  if (devStatus == 0) {
+      // turn on the DMP, now that it's ready
+      mpu.setDMPEnabled(true);
+
+      // enable Arduino interrupt detection
+      attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
+      mpuIntStatus = mpu.getIntStatus();
+
+      // set our DMP Ready flag so the main loop() function knows it's okay to use it
+      Serial.println(F("DMP ready! Waiting for first interrupt..."));
+      dmpReady = true;
+
+      // get expected DMP packet size for later comparison
+      packetSize = mpu.dmpGetFIFOPacketSize();
+  }
+
+  resetGyro('z');
+
+  FreqCount.begin(1000);
+}
+
+bool firstStart = false;
+bool foundFlame = false;
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  if (FreqCount.available()) {
+    count = FreqCount.read();
+	  //Serial.println(count);
+  } else {
+    count = 0;
+  }
+//  Serial.println(getAngle('z'));
+  if ((count > 3230 && count < 4370) || digitalRead(startSwitch)) {
+    digitalWrite(SOUND_LED, HIGH);
+    firstStart = true;
+    robotOn = !robotOn;
+    resetGyro('z');
+  }
+
+//  Serial.println(getAngle('z'));
+  if(robotOn){
+    if (firstStart) {
+      FreqCount.end();
+      ri = colour_sensor.getColor('r');
+    }
+
+  	while(!foundFlame && digitalRead(flame_sensor)) {
+  		// naviguessMaze(0.3);
+  		// followWall(1.0);
+       ultrasonicCheck();
+      // drivetrainCheck();
+
+//      if (!pidEnabled) {
+//        pidEnabled = true;
+//        resetGyro('z');
+//      }
+//      hDrive(0.5, getPidOutput(), 0.0);   // Correctional driving
+//      Serial.println(getPidOutput());
+
+  	}
+
+  	foundFlame = true;
+  	digitalWrite(FLAME_LED, foundFlame);
+
+  	fan_motor.set(1.0);
+
+  	hDrive(0.0, 0.0, 0.0);
+  	delay(100);
+
+  	while (frontUltrasonic.getDistance() > 0.5) {
+  		hDrive(0.3, 0.0, 0.0);
+  		extingusher_servo.write(110);
+  		delay(600);
+  		extingusher_servo.write(70);
+  		delay(600);
+  		hDrive(0.0, 0.0, 0.0);
+  		delay(200);
+  		digitalWrite(FLAME_LED, !digitalRead(flame_sensor));
+  	}
+
+	  hDrive(0.0, 0.0, 0.0);
+  }
+  else{
+    digitalWrite(SOUND_LED, LOW);
+    firstStart = false;
+    hDrive(0.0, 0.0, 0.0);
+  }
+
+//  delay(100);
+}
+
+void hDrive(double move, double rotate, double strafe) {
+  double leftSpeed, rightSpeed;
+
+  move = constrain(move, -1.0, 1.0);
+  rotate = constrain(rotate, -1.0, 1.0);
+  strafe = constrain(strafe, -1.0, 1.0);
+
+  if (move > 0.0) {
+    if (rotate > 0.0) {
+      leftSpeed =  move - rotate;
+      rightSpeed = max(move, rotate);
+    } else {
+      leftSpeed = max(move, -rotate);
+      rightSpeed = move + rotate;
+    }
+  } else {
+    if (rotate > 0.0) {
+      leftSpeed = -max(-move, rotate);
+      rightSpeed = move + rotate;
+    } else {
+      leftSpeed = move - rotate;
+      rightSpeed = -max(-move, -rotate);
+    }
+  }
+
+  leftMotor1.set(constrain(leftSpeed, -1.0, 1.0));
+  leftMotor2.set(constrain(leftSpeed, -1.0, 1.0));
+  rightMotor1.set(-constrain(rightSpeed, -1.0, 1.0));
+  rightMotor2.set(-constrain(rightSpeed, -1.0, 1.0));
+  strafeMotor.set(strafe);
+}
+
+void drivetrainCheck() {
+  hDrive(0.3, 0.0, 0.0); delay(300); hDrive(-0.3, 0.0, 0.0); delay(300);
+  hDrive(0.0, 0.3, 0.0); delay(300); hDrive(0.0, -0.3, 0.0); delay(300);
+  hDrive(0.0, 0.0, 0.3); delay(300); hDrive(0.0, 0.0, -0.3); delay(300);
+}
+
+void ultrasonicCheck() {
+	Serial.println("====================================");
+	Serial.print("F: ");
+	Serial.println(frontUltrasonic.getDistance());
+	Serial.print("L: ");
+	Serial.println(leftUltrasonic.getDistance());
+	Serial.print("R: ");
+	Serial.println(rightUltrasonic.getDistance());
+	Serial.print("B: ");
+	Serial.println(backUltrasonic.getDistance());
+	delay(300);
+}
+
+void driveToWhite() {
+  int ri = colour_sensor.getColor('r');
+  int r = ri;
+//  int g = colour_sensor.getColor('g');
+//  int b = colour_sensor.getColor('b');
+//  String colour = String(ri) + " " + String(g) + " " + String(b);
+//  Serial.println(colour);
+  hDrive(.3,0,0);
+  Serial.print("Initial: ");
+  Serial.println(ri);
+
+  while(r > ri - 70) {
+    r = colour_sensor.getColor('r');
+//        String colour = String(r);
+//        Serial.println(colour);
+  }
+
+  Serial.print("Final: ");
+  Serial.println(r);
+
+//      Serial.println("Done");
+  hDrive(0.0, 0.0, 0.0);
+}
+
+void driveThroughRoom() {
+  if (frontUltrasonic.getDistance() < WALL_DISTANCE) {
+    if (leftUltrasonic.getDistance() < WALL_DISTANCE) {
+      hDrive(0.0, 0.0, 1.0);
+    } else {
+      hDrive(0.0, 0.0, -1.0);
+    }
+  } else {
+    if (leftUltrasonic.getDistance() < WALL_DISTANCE) {
+      hDrive(0.0, 0.0, 1.0);
+    } else {
+      hDrive(0.0, 0.0, -1.0);
+    }
+  }
+}
+
+void definedStartSearch() {
+  // Start A
+  if (rightUltrasonic.getDistance() < WALL_DISTANCE) {
+    hDrive(0.0, 0.0, -1.0);
+    delay(500);
+    while (frontUltrasonic.getDistance() < WALL_DISTANCE) { }
+    hDrive(0.0, 0.0, 0.0);
+  // Start B
+  } else {
+    hDrive(1.0, 0.0, 0.0);
+    while (frontUltrasonic.getDistance() > WALL_DISTANCE) { }
+    hDrive(0.0, 0.0, 0.0);
+  }
+}
+
+void countPulseRight() {
+  int Lstate = digitalRead(encoderARight);
+  if((encoderALastRight == LOW) && Lstate==HIGH)
+  {
+    int val = digitalRead(encoderBRight);
+    if(val == LOW && directionRight)
+    {
+      directionRight = false; //Reverse
+    } else if(val == HIGH && !directionRight) {
+      directionRight = true;
+    }
+  }
+  encoderALastRight = Lstate;
+
+  if(!directionRight)  pulseRight--;
+  else  pulseRight++;
+}
+
+void countPulseLeft() {
+  int Lstate = digitalRead(encoderALeft);
+  if((encoderALastLeft == LOW) && Lstate==HIGH)
+  {
+    int val = digitalRead(encoderBLeft);
+    if(val == LOW && directionLeft)
+    {
+      directionLeft = false; //Reverse
+    } else if(val == HIGH && !directionLeft) {
+      directionLeft = true;
+    }
+  }
+  encoderALastLeft = Lstate;
+
+  if(!directionLeft)  pulseLeft++;
+  else  pulseLeft--;
+}
+
+double getAngle() {
+  return (180 / PI) * (getDistanceLeft() - getDistanceRight()) / 11.5;
+}
+
+double getDistanceLeft() {
+	return (pulseLeft * 12.5) / 1356;
+}
+
+double getDistanceRight() {
+	return (pulseRight * 12.5) / 1356;
+}
+
+void resetEncoders() {
+	pulseLeft = pulseRight = 0;
+}
+
+double getPercentError(double actual, double target) {
+  return abs(actual - target) / target;
+}
+
+void turnToAngle(double targetAngle, double speed) {
+  resetEncoders();
+
+  hDrive(0.0, speed, 0.0);
+
+  while (abs(getAngle('z')) < abs(targetAngle)) {
+    Serial.println(getAngle('z'));
+  }
+
+  hDrive(0.0, 0.0, 0.0);
+}
+
+double getPidOutput() {
+  return (setpoint - getAngle('x')) * kP;
+}
+
+void extinguish() {
+  for(int i = 40; i <= 160; i += interval){
+    extingusher_servo.write(i);
+    delay(15*interval);//give time for servo to move
+
+    if(!digitalRead(flame_sensor))
+      digitalWrite(FLAME_LED, HIGH);
+
+    //turn on fan
+    fan_motor.set(1);
+    while(!digitalRead(flame_sensor)){
+      //while there is a fire
+      delay(1000);
+    }
+    //turn off fan
+    fan_motor.set(0);
+    digitalWrite(FLAME_LED, LOW);
+  }
+
+  for(int i = 140; i >=20; i -= interval){
+    extingusher_servo.write(i);
+    delay(15*interval);//give time for servo to move
+
+    if(!digitalRead(flame_sensor))
+      digitalWrite(FLAME_LED, HIGH);
+
+    //turn on fan
+    fan_motor.set(1);
+    while(!digitalRead(flame_sensor)){
+      //while there is a fire
+      delay(1000);
+    }
+    //turn off fan
+    fan_motor.set(0);
+    digitalWrite(FLAME_LED, LOW);
+  }
+}
+
+bool direction = true;
+int leftTurns = 0;
+int rightTurns = 0;
+
+void naviguessMaze(double swagSpeed) {
+	bool frontTriggered = frontUltrasonic.getDistance() < 2;
+	bool leftTriggered = leftUltrasonic.getDistance() < 3;
+	bool rightTriggered = rightUltrasonic.getDistance() < 3;
+	bool backTriggered = backUltrasonic.getDistance() < 2;
+
+	if (leftTriggered && frontTriggered) {
+		hDrive(-swagSpeed, 0.0, 0.0);
+		delay(100);
+		// hDrive(0.0, swagSpeed , 0.0);
+
+		if (rightTurns == 4) {
+			turnToAngle(-90, swagSpeed);
+			rightTurns = 0;
+		} else {
+			turnToAngle(90, -swagSpeed);
+			rightTurns++;
+		}
+	} else if (rightTriggered && frontTriggered) {
+		hDrive(-swagSpeed, 0.0, 0.0);
+		delay(100);
+		// hDrive(0.0, -swagSpeed , 0.0);
+
+		if (leftTurns == 4) {
+			turnToAngle(-90, -swagSpeed);
+			leftTurns = 0;
+		} else {
+			turnToAngle(-90, swagSpeed);
+			leftTurns++;
+		}
+	} else if (leftTriggered && !frontTriggered) {
+		// hDrive(swagSpeed - 0.2, -swagSpeed, 0.0);
+		turnToAngle(2, -swagSpeed);
+	} else if (rightTriggered && !frontTriggered) {
+		//hDrive(swagSpeed - 0.2, swagSpeed , 0.0);
+		turnToAngle(-2, swagSpeed);
+	} else if (frontTriggered && !rightTriggered & !leftTriggered){
+		hDrive(-swagSpeed, 0.0, 0.0);
+		delay(50);
+		if (rightTurns == 4) {
+			turnToAngle(-90, swagSpeed);
+			rightTurns = 0;
+		} else {
+			turnToAngle(90, -swagSpeed);
+			rightTurns++;
+		}
+		// turnToAngle(90, -swagSpeed);
+		// rightTurns++;
+	} else if (backTriggered) {
+		hDrive(swagSpeed, swagSpeed, 0.0);
+	} else {
+		hDrive(swagSpeed, 0.0, 0.0);
+	}
+
+	if (direction) {
+		strafeMotor.set(0.3);
+	} else {
+		strafeMotor.set(-0.3);
+	}
+
+	direction = !direction;
+
+	delay(50);
+}
+
+void tankDrive(double leftSpeed, double rightSpeed, double strafeSpeed) {
+	leftMotor1.set(constrain(leftSpeed, -1.0, 1.0));
+	leftMotor2.set(constrain(leftSpeed, -1.0, 1.0));
+	rightMotor1.set(-constrain(rightSpeed, -1.0, 1.0));
+	rightMotor2.set(-constrain(rightSpeed, -1.0, 1.0));
+	strafeMotor.set(strafeSpeed);
+}
+
+void followWall(double targetDistance) {
+	if (rightUltrasonic.getDistance() < targetDistance) {
+		tankDrive(0.0, 0.3, 0.0);
+	} else if (rightUltrasonic.getDistance() > targetDistance) {
+		tankDrive(0.3, 0.0, 0.0);
+	} else {
+		tankDrive(0.3, 0.4, 0.0);
+	}
+}
+
+double getAngle(char axis) {
+  // if programming failed, don't try to do anything
+    if (!dmpReady) return;
+
+    // wait for MPU interrupt or extra packet(s) available
+    while (!mpuInterrupt && fifoCount < packetSize) {
+        // other program behavior stuff here
+        // .
+        // .
+        // .
+        // if you are really paranoid you can frequently test in between other
+        // stuff to see if mpuInterrupt is true, and if so, "break;" from the
+        // while() loop to immediately process the MPU data
+        // .
+        // .
+        // .
+    }
+
+    // reset interrupt flag and get INT_STATUS byte
+    mpuInterrupt = false;
+    mpuIntStatus = mpu.getIntStatus();
+
+    // get current FIFO count
+    fifoCount = mpu.getFIFOCount();
+
+    // check for overflow (this should never happen unless our code is too inefficient)
+    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
+        // reset so we can continue cleanly
+        mpu.resetFIFO();
+        Serial.println(F("FIFO overflow!"));
+
+    // otherwise, check for DMP data ready interrupt (this should happen frequently)
+    } else if (mpuIntStatus & 0x02) {
+        // wait for correct available data length, should be a VERY short wait
+        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
+
+        // read a packet from FIFO
+        mpu.getFIFOBytes(fifoBuffer, packetSize);
+
+        // track FIFO count here in case there is > 1 packet available
+        // (this lets us immediately read more without waiting for an interrupt)
+        fifoCount -= packetSize;
+
+        // display Euler angles in degrees
+        mpu.dmpGetQuaternion(&q, fifoBuffer);
+        mpu.dmpGetEuler(euler, &q);
+
+        switch (axis) {
+          case 'x':
+            return (euler[2] * 180/M_PI) - gyroResetValX;
+            break;
+          case 'y':
+            return (euler[1] * 180/M_PI) - gyroResetValY;
+            break;
+          case 'z':
+            return (euler[0] * 180/M_PI) - gyroResetValZ;
+            break;
+        }
+    }
+}
+
+void resetGyro(char axis) {
+  switch (axis) {
+    case 'x':
+      gyroResetValX += getAngle('x');
+      break;
+    case 'y':
+      gyroResetValY += getAngle('y');
+      break;
+    case 'z':
+      gyroResetValZ += getAngle('z');
+      break;
+  }
+}
+


use 250Hz instead of 125Hz

use 250Hz instead of 125Hz

--- DanceFeather/DanceFeather.ino
+++ DanceFeather/DanceFeather.ino
@@ -16,9 +16,10 @@
 //#define FFT_SIZE 512
 //#define HANN_SIZE 60

-#define ACC_SIZE 128
+#define LOW_PASS_SIZE 64 // 250Hz sample freq, this means signals above 125Hz are filtered
+#define ACC_SIZE 2 // reduce sample rate before doing fft for performance reasons
 #define FFT_SIZE 256
-#define HANN_SIZE 30
+#define HANN_SIZE 40
 #define FS (16000.0 / ACC_SIZE)
 #define F2 3.5f
 #define END_INDEX ((uint16_t) (F2 / (FS / FFT_SIZE)))
@@ -47,6 +48,8 @@

 // accumulate incoming values to create low pass filter
 extern PDMClass PDM;
+float lowPass = 0;
+uint16_t numInLowPass = 0;
 float acc = 0;
 uint16_t numInAcc = 0;

@@ -413,21 +416,24 @@
     uint32_t start = micros();
     for (int i = 0; i < samplesRead; i++) {
       float newVal = tempBuffer[i];
-      //acc += newVal * newVal;
-      acc += newVal;
-      numInAcc++;
+      lowPass += newVal;
+      if (++numInLowPass >= LOW_PASS_SIZE) {
+        acc += lowPass * lowPass;
+        numInAcc++;
+        numInLowPass = 0;
+        lowPass = 0;
+      }
+
       if (numInAcc >= ACC_SIZE) {
-          float value = acc / ACC_SIZE;
+          addToFFT(acc);
           acc = 0;
           numInAcc = 0;
-          addToFFT(value);
-          // addToFFT(value);
       }

       //samples--;
     }
     // clear the read count
-    uint32_t duration = micros() - start;
+    //uint32_t duration = micros() - start;
     //Serial.print("micros: "); Serial.println(duration);
     //Serial.print("samples: "); Serial.println(samplesRead);

@@ -441,6 +447,10 @@
 }

 void onPDMdata() {
+  if (samplesRead) {
+    Serial.println("PDM data did not get processed");
+  }
+
   // query the number of bytes available
   int bytesAvailable = PDM.available();


round rpm value

round rpm value

--- main/main.ino
+++ main/main.ino
@@ -162,8 +162,9 @@
         engine_map.currentValue = ((buf[0] * 256) + buf[1]) / 10.0;

         engine_rpm.previousValue = engine_rpm.currentValue;
-        engine_rpm.currentValue = buf[2] * 256 + buf[3];
-        engine_rpm.currentValue = 4938;
+        //engine_rpm.currentValue = buf[2] * 256 + buf[3];
+        //round rpm to nearest 10
+        engine_rpm.currentValue = round((buf[2] * 256 + buf[3]) / 10) * 10;

         engine_clt.previousValue = engine_clt.currentValue;
         engine_clt.currentValue = (buf[4] * 256 + buf[5]) / 10.0;


Removed FlyCam 1 Eco code

Removed FlyCam 1 Eco code

We removed the FlyCam 1 Eco from the Rover and thus from the code

--- Arduino/Rover3Multifunction/Rover3Multifunction.ino
+++ Arduino/Rover3Multifunction/Rover3Multifunction.ino
@@ -1,4 +1,4 @@
-// Rover v3 Multifunction: GPS, Tilt-compensated compass, ultrasonic range finder, FlyCam, temperature sensor, speaker, etc
+// Rover v3 Multifunction: GPS, Tilt-compensated compass, ultrasonic range finder, temperature sensor, speaker, etc

 // HMC6343 Compass: uses SDA/SCL on pins 20 and 21
 #include <Wire.h>
@@ -9,9 +9,6 @@
 // Temperature & Humiditiy Sensor
 #include <DHT22.h>

-// FlyCam
-#include <Servo.h>
-
 // XBEE: uses Serial2 port on pins 16 and 17
 #include <XBee.h>

@@ -23,15 +20,12 @@

 // Temperature sensor
 #define DHT22_PIN            42
-
-// FlyCam
-#define FLYCAM_PWM_PIN       44

 // Ultrasonic Range Finder (forward-facing)
 #define MAXSONAR_ACTIVE_PIN  43
 #define MAXSONAR_PWM_PIN     45

-// Infrared range sensor (rear-racing)
+// Infrared range sensor (rear-facing)
 #define IR_RANGE_PIN         15

 float ir_sensor_value = 0;
@@ -169,10 +163,6 @@

   // Tell XBee to start Serial
   xbee.begin(9600);
-
-  // FlyCam
-  myservo.attach(FLYCAM_PWM_PIN);  // attaches the servo on pin 9 to the servo object
-  myservo.write(0);              // tell servo to go to position in variable 'pos'

   // Motors

@@ -668,15 +658,6 @@

 }

-// FlyCam
-
-void FlyCamButton() {
-  Serial.println("Button");
-  myservo.write(180);              // tell servo to go to position in variable 'pos'
-  delay(250);
-  myservo.write(0);              // tell servo to go to position in variable 'pos'
-}
-
 // ***** MOTORS *******

 void testMotors()


Fully Functional

Fully Functional

--- WebToLEDMatrix.ino
+++ WebToLEDMatrix.ino
@@ -8,27 +8,37 @@
 #include <HttpClient.h>
 #include <Ethernet.h>
 #include <EthernetClient.h>
-#include "LedControl.h"
+#include <LedControl.h>
 #include <avr/pgmspace.h>

-//LedControl lc=LedControl(12,11,10,1);
+
+#define FULL_SCREEN 8
+//Pins for the Led Matrix (DATA IN,CLK,LOAD)
 LedControl lc=LedControl(2,4,3,1);

-// https://lyle.troxell.com/beMineMessage.txt
+//The URL to load the Message From
 const char kHostname[] = "lyle.github.io";
-//const char kPath[] = "/dev/beMineMessage.txt";
 const char kPath[] = "/dev/by-mine";
-
 const int kPort = 80;

+//Ethernet, we do need a network and IP Address
 byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };

 // Number of milliseconds to wait without receiving any data before we give up
 const int kNetworkTimeout =20L*1000L; //20 seconds before giving up
 // Number of milliseconds to wait if no data is available before trying again
 const int kNetworkDelay = 1000; //Between Requests to the server
-
-byte matrixVals[] = { 0,0,0,0,0,0,0,0 };
+byte  networkIcon[FULL_SCREEN] = {
+  B10000000,
+  B10011100,
+  B10110110,
+  B11100011,
+  B11100011,
+  B10110110,
+  B10011100,
+  B10000000};
+
+byte matrixVals[FULL_SCREEN] = { 0,0,0,0,0,0,0,0 };

 // time between web requests
@@ -51,21 +61,38 @@
   Serial.begin(9600);

   Serial.println("We Are Booting Up");
-  bool attemptNetwork = true;
-  while (attemptNetwork)
+  bool weHaveNetwork = false;
+  while (!weHaveNetwork)
   {
+    displayOnLED(networkIcon);
     Serial.println("About to get IP address via DHCP");
     int d = Ethernet.begin(mac);
     Serial.print("Ethernet Status:");
     Serial.println(d);
+
     if (d==1) {
-      attemptNetwork = false;
+      weHaveNetwork = true;
     } else {
-      delay(1500);
+      delay(800);
+      for (int i=0; i < FULL_SCREEN; ++i ){
+        shiftMatrix(networkIcon[i]);
+        delay(100);
+      }
+      delay(800);
     }
   }
+  lc.clearDisplay(0);
   Serial.print("My IP address: ");
   Serial.println(Ethernet.localIP());
+}
+void displayOnLED(byte message[FULL_SCREEN]) {
+  int messageLength = sizeof(message);
+  Serial.print("messageLength=");
+  Serial.println(messageLength);
+  messageLength = messageLength > 8?8:messageLength;
+  for (int j=0; j < FULL_SCREEN; ++j ){
+    lc.setRow(0,j,message[j]);
+  }
 }

 void shiftMatrix(unsigned char movingLine) {
@@ -97,27 +124,24 @@
   HttpClient http(client);

   err = http.get(kHostname, kPort, kPath);
-  if (err == 0){
+  if (err == 0) {
     Serial.println("startedRequest ok");
     err = http.responseStatusCode();
     if (err == 200) {
       Serial.println("Response Code: " + err);
       err = http.skipResponseHeaders();
       if (err >= 0){
-        int bodyLen = http.contentLength();
         Serial.print("Content length is: ");
-        Serial.println(bodyLen);
+        Serial.println(http.contentLength());
         unsigned long timeoutStart = millis();

-        byte d[1];
-        byte modifyInt;
-        while ( (http.connected() || http.available()) &&
-               ((millis() - timeoutStart) < kNetworkTimeout) ){
+        while ((http.connected() || http.available()) &&
+              ((millis() - timeoutStart) < kNetworkTimeout)) {
+
             if (http.available()){
               c = http.read();
               shiftMatrix(c);
               delay(500);
-              bodyLen--;
               // We read something, reset the timeout counter
               timeoutStart = millis();
             } else {


Toggle hi/lo temp display modes on app

Toggle hi/lo temp display modes on app

--- hvacMonitor.ino
+++ hvacMonitor.ino
@@ -188,6 +188,35 @@
   }

   Blynk.virtualWrite(67, String("returntemp=") + tempRA + "&runstatus=" + runStatus + "&supplytemp=" + tempSA);
+
+  // The following changes the color of Time Until Filter Change in app... placed here because the timing.
+  if (filterChangeHours - (currentFilterSec / 3600) > 24 )
+  {
+    Blynk.setProperty(V10, "color", "#23C48E");   // Green
+  }
+  else if (filterChangeHours - (currentFilterSec / 3600) < 24 &&  filterChangeHours - (currentFilterSec / 3600) > 8)
+  {
+    Blynk.setProperty(V10, "color", "#ED9D00");   // Yellow
+  }
+  else if (filterChangeHours - (currentFilterSec / 3600) < 8)
+  {
+    Blynk.setProperty(V10, "color", "#D3435C");   // Red
+  }
+}
+
+BLYNK_WRITE(V19)              // App button to display all terminal commands available
+{
+  int pinData = param.asInt();
+
+  if (pinData == 0)
+  {
+    Blynk.setProperty(V16, "label", "Current Status:                    Hi/Lo temps above are last 24h");
+  }
+
+  if (pinData == 1)
+  {
+    Blynk.setProperty(V16, "label", "Current Status:                    Hi/Lo temps above are since midnight");
+  }
 }

 BLYNK_WRITE(V18)              // App button to display all terminal commands available
@@ -472,6 +501,7 @@
     else if (todaysDate != day())
     {
       yesterdayRuntime = (todaysAccumRuntimeSec / 60);  // Moves today's runtime to yesterday for the app display.
+      Blynk.virtualWrite(112, yesterdayRuntime);        // Record yesterday's runtime to vPin.
       todaysAccumRuntimeSec = 0;                        // Reset today's sec timer.
       Blynk.virtualWrite(111, todaysAccumRuntimeSec);
       hvacTodaysStartCount = 0;                         // Reset how many times unit has started today.
@@ -509,3 +539,59 @@
     Blynk.virtualWrite(100, minute());
   }
 }
+
+int todayTrends[96];  // 0 represents the first 15 minutes of the day, and so on. Will have 96 elements.
+int trackTrendIndex;
+int yesterdayTrends[96];
+bool trackTrendResetFlag;
+bool trackTrendMinuteFlag;
+
+void trackTrends()
+{
+  if (hour() == 0 && trackTrendResetFlag  == 0)   // At midnight...
+  {
+    int i;
+    for (i = 0; i < 97; i++) {                    // ...transfer todayTrends to yesterdayTrends.
+      yesterdayTrends[i] = todayTrends[i];
+    }
+
+    trackTrendIndex = 0;                          // Reset array index to 0.
+    trackTrendResetFlag = 1;
+  }
+  else if (hour() == 1)                           // Resets flag for the next midnight "reset"
+  {
+    trackTrendResetFlag = 0;
+  }
+
+  if (minute() == 0 && hour() == 0)
+  {
+    // Do nothing... this would record the previous day runtime (23:45 - 00:00) and we don't want that here.
+  }
+  else if ( (minute() == 0 || minute() == 15 || minute() == 30 || minute() == 45) && trackTrendMinuteFlag == 0)
+  {
+    todayTrends[trackTrendIndex] = (todaysAccumRuntimeSec / 60);
+
+    if (todayTrends[trackTrendIndex] = yesterdayTrends[trackTrendIndex])
+    {
+      // "Trending same runtime as yesterday"
+    }
+    else if (todayTrends[trackTrendIndex] > yesterdayTrends[trackTrendIndex])
+    {
+      // "Trending higher runtime today"
+    }
+    else if (todayTrends[trackTrendIndex] < yesterdayTrends[trackTrendIndex])
+    {
+      // "Trending lower runtime today"
+    }
+
+    ++trackTrendIndex;
+    trackTrendMinuteFlag = 1;
+  }
+
+
+  if (minute() == 1 || minute() == 16 || minute() == 31 || minute() == 46)
+  {
+    trackTrendMinuteFlag = 0;
+  }
+
+}


Modification du code C

Modification du code C

--- arduino/CommunicationArduino/CommunicationArduino.ino
+++ arduino/CommunicationArduino/CommunicationArduino.ino
@@ -1,14 +1,30 @@
-#include <Wire.h>
 #include <LiquidCrystal_I2C.h>

-LiquidCrystal_I2C lcd(0x27,20,4 );
+#include <Wire.h>
+
+// Set the LCD address to 0x27 for a 16 chars and 2 line display
+
+#include <LiquidCrystal_I2C.h>
+
+LiquidCrystal_I2C lcd(0x27,20,4);  // set the LCD address to 0x27 for a 16 chars and 2 line display
+#include <Servo.h>
+
+Servo myservo;  // create servo object to control a servo
+// twelve servo objects can be created on most boards
+
+int pos = 180;
+int LOW_POSITION =  180;
+int HIGH_POSITION =  0;

 void setup()
 {
   Serial.begin(9600);
   lcd.init(); // initialize the lcd
   lcd.backlight();
-  lcd.print("Setup Arduino"); // Print a message to the LCD.
+  lcd.print((char) 0x5C);
+ // lcd.print("Setup Arduino"); // Print a message to the LCD.
+  myservo.attach(9);
+  myservo.write(LOW_POSITION);

 }

@@ -16,9 +32,31 @@
 {
   if (Serial.available())  {
     delay(100);
-    lcd.clear();
-    lcd.autoscroll();
+    lcd.setCursor(0,0);
     lcd.print(Serial.readString());
     Serial.write("DONE");
+    /*for (pos = 20; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
+      // in steps of 1 degree
+      myservo.write(pos);              // tell servo to go to position in variable 'pos'
+      delay(40);                       // waits 15ms for the servo to reach the position
+    }
+    for (pos = 180; pos >= 20; pos -= 1) { // goes from 180 degrees to 0 degrees
+      myservo.write(pos);              // tell servo to go to position in variable 'pos'
+      delay(40);                       // waits 15ms for the servo to reach the position
+    }
+    myservo.write(LOW_POSITION);
+    delay(120);*/
+
+    for (pos = 180; pos >= 20; pos -= 1) { // goes from 180 degrees to 0 degrees
+      myservo.write(pos);              // tell servo to go to position in variable 'pos'
+      delay(15);                       // waits 15ms for the servo to reach the position
+    }
+  delay(1000);
+    for (pos = 20; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
+      // in steps of 1 degree
+      myservo.write(pos);              // tell servo to go to position in variable 'pos'
+      delay(20);                       // waits 15ms for the servo to reach the position
+    }
+
   }
 }


made example more clear by removing some defines and put flash function around a few strings that didnt have it

--- examples/skynetim_fakefirmata/skynetim_fakefirmata.ino
+++ examples/skynetim_fakefirmata/skynetim_fakefirmata.ino
@@ -32,10 +32,6 @@
 #include "Ethernet.h"
 #include "SPI.h"
 #include "SkynetClient.h"
-
-#define RED "red"
-#define BLUE "blue"
-#define GREEN "green"

 //for uno avoid pins 10 11 12 13 plus if you're using sd card 4, and additionally pin 7 if wifi
 //for mega avoid pins 50 51 52 53 plus if you're using sd card 4 and additionally pin 47 if wifi
@@ -103,35 +99,35 @@
   msg = aJson.getObjectItem(data, MESSAGE);
   Serial.println(aJson.print(msg));

-  color = aJson.getObjectItem(msg, RED);
+  color = aJson.getObjectItem(msg, "red");
   if (color != NULL) {
     if(strcmp(color->valuestring, ON) == 0){
-      Serial.println("red on");
+      Serial.println(F("red on"));
       digitalWrite(REDLED, HIGH);
     }else{
-      Serial.println("red off");
+      Serial.println(F("red off"));
       digitalWrite(REDLED, LOW);
     }
   }

-  color = aJson.getObjectItem(msg, BLUE);
+  color = aJson.getObjectItem(msg, "blue");
   if (color != NULL) {
     if(strcmp(color->valuestring, ON) == 0){
-      Serial.println("blue on");
+      Serial.println(F("blue on"));
       digitalWrite(BLUELED, HIGH);
     }else{
-      Serial.println("blue off");
+      Serial.println(F("blue off"));
       digitalWrite(BLUELED, LOW);
     }
   }

-  color = aJson.getObjectItem(msg, GREEN);
+  color = aJson.getObjectItem(msg, "green");
   if (color != NULL) {
     if(strcmp(color->valuestring, ON) == 0){
-      Serial.println("green off");
+      Serial.println(F("green off"));
       digitalWrite(GREENLED, HIGH);
     }else{
-      Serial.println("green off");
+      Serial.println(F("green off"));
       digitalWrite(GREENLED, LOW);
     }
   }


comments and fixes

comments and fixes

--- Arduino/hardware/REV10/REV10MessagePathTest/REV10SecureBHR/REV10SecureBHR.ino
+++ Arduino/hardware/REV10/REV10MessagePathTest/REV10SecureBHR/REV10SecureBHR.ino
@@ -14,20 +14,28 @@
 under the Licence.

 Author(s) / Copyright (s): Damon Hart-Davis 2013--2017
-                           Deniz Erbilgin 2015--2016
+                           Deniz Erbilgin 2015--2017
 */

-/*
-  Test of minimum relay code path.
-
-  DHD20130417: hardware setup on bare board.
-    * 1MHz CPU clock (from 8MHz internal RC clock with /8 prescaler) ATmega328P running at 1.8V--5V (typically 2V--3.3V).
-    * Fuse set for BOD-managed additional clock settle time, ie as fast a restart from sleep as possible.
-    * All unused pins unconnected and nominally floating (though driven low as output where possible).
-    * 32768Hz xtal between pins XTAL1 and XTAL2, async timer 2, for accurate timekeeping and low-power sleep.
-    * All unused system modules turned off.
-
-  Basic AVR power consumption ticking an (empty) control loop at ~0.5Hz should be ~1uA.
+/**
+ *  DE20170901
+ * @brief   To test alternative methods for relay send over the SIM900,
+ *          def ONE of the following flags at a time, in OTSIM900Link_OTSIM900Link.h.
+ *
+ *          OTSIM900LINK_SPLIT_SEND_TEST seems to drops less frames
+ *          (hard to tell as both REV10s under test were relaying the same REV7s).
+ *
+// IF DEFINED:  Splits the send routine into two steps, rather than polling for a prompt.
+//              Should be avoided if possible, but may be necessary e.g. with the REV10 as
+//              the blocking poll may overrun a sub-cycle, triggering a WDT reset.
+//              This behaviour depends on the fact that the V0p2 cycle takes long enough
+//              between polls for the SIM900 to be ready to receive a packet, but not
+//              long enough to time out the send routine. // XXX
+#undef OTSIM900LINK_SPLIT_SEND_TEST
+// IF DEFINED:  Flush until a fixed point in the sub-cycle. Note that this requires
+//              OTV0P2BASE::getSubCycleTime or equivalent to be passed in as a template param.
+//              May perform poorer/send junk in some circumstances (needs more testing).
+#undef OTSIM900LINK_SUBCYCLE_SEND_TIMEOUT_TEST
  */

 // GLOBAL flags that alter system build and behaviour.
@@ -82,7 +90,7 @@
 static constexpr int8_t RFM23B_IRQ_PIN = PIN_RFM_NIRQ;
 static constexpr bool RFM23B_allowRX = true;
 OTRFM23BLink::OTRFM23BLink<OTV0P2BASE::V0p2_PIN_SPI_nSS, RFM23B_IRQ_PIN, RFM23B_RX_QUEUE_SIZE, RFM23B_allowRX> PrimaryRadio;
-OTSIM900Link::OTSIM900Link<8, 5, RADIO_POWER_PIN, OTV0P2BASE::getSecondsLT> SecondaryRadio; // (REGULATOR_POWERUP, RADIO_POWER_PIN);
+OTSIM900Link::OTSIM900Link<8, 5, RADIO_POWER_PIN, OTV0P2BASE::getSecondsLT> SecondaryRadio;

 /////// SENSORS


fix to new encoder interrupt; change backoff distance with new bigger interrupt steps (but also let it get a bit longer)

--- arduino/motor_driver/motor_driver.ino
+++ arduino/motor_driver/motor_driver.ino
@@ -52,7 +52,7 @@

 // ENCODER SETUP ---------------------------
 // encoder on 2 and 3   // +5 is brown, ground is blue
-#define ENCODER_PORT PORTD
+#define ENCODER_PORT PIND
 const int encoder0PinA = 2;  // white   PORTD BIT 2
 const byte encoder0PinAMask = 0x04;
 const int encoder0PinB = 3;  // black   PORTD BIT 3
@@ -89,7 +89,7 @@
 float homingSpeed = 3.0;
 bool homed = false;

-const int BACKOFF_STEPS = 600; // how many encoder steps to reverse out of endstop (so Zero is this far from the switch)
+const int BACKOFF_STEPS = 200; // how many encoder steps to reverse out of endstop (so Zero is this far from the switch)

 // MOTOR TIMING/POS
@@ -398,10 +398,10 @@
     then you're going anti-clockwise.
   */
   if (ENCODER_PORT & encoder0PinBMask) {
+    encoder0Pos --;
+  }
+  else {
     encoder0Pos ++;
-  }
-  else {
-    encoder0Pos --;
   }
 }


Updated the flashing logic to not use delay so that the program is more responsive.

--- arduino/trafficLight/trafficLight.ino
+++ arduino/trafficLight/trafficLight.ino
@@ -2,10 +2,15 @@
 const int YELLOW_PIN = 3;
 const int GREEN_PIN = 2;

+const int VOID_PIN= -1;
+
 const int pins[] = { RED_PIN, YELLOW_PIN, GREEN_PIN };
 const int pinCount = sizeof(pins) / sizeof(int);

-int flashingPin = -1;
+int flashingPin = VOID_PIN;
+boolean flashingHigh;
+const unsigned long flashDuration = 750;
+unsigned long flashStartTime;

 void setup() {
     Serial.begin(9600);
@@ -18,12 +23,13 @@

 void loop() {

-    if (flashingPin != -1) {
-        // TODO use millis() to compare time instead of delays so that it's more responsive
-        digitalWrite(flashingPin, HIGH);
-        delay(750);
-        digitalWrite(flashingPin, LOW);
-        delay(750);
+    if (flashingPin != VOID_PIN) {
+        if ((millis() - flashStartTime) >= flashDuration) {
+            flashingHigh = !flashingHigh;
+            flashStartTime = millis();
+
+            digitalWrite(flashingPin, flashingHigh);
+        }
     }

     if (Serial.available()) {
@@ -34,37 +40,41 @@

         if (command == "light red") {
             lightPinExclusive(RED_PIN);
-            flashingPin = -1;
         }
         else if (command == "light yellow") {
             lightPinExclusive(YELLOW_PIN);
-            flashingPin = -1;
         }
         else if (command == "light green") {
             lightPinExclusive(GREEN_PIN);
-            flashingPin = -1;
         }
         else if (command == "flash red") {
-            lightPinExclusive(-1);
-            flashingPin = RED_PIN;
+            flashPinExclusive(RED_PIN);
         }
         else if (command == "flash yellow") {
-            lightPinExclusive(-1);
-            flashingPin = YELLOW_PIN;
+            flashPinExclusive(YELLOW_PIN);
         }
         else if (command == "flash green") {
-            lightPinExclusive(-1);
-            flashingPin = GREEN_PIN;
+            flashPinExclusive(GREEN_PIN);
         }
         else if (command == "lights off") {
-            lightPinExclusive(-1);
-            flashingPin = -1;
+            lightPinExclusive(VOID_PIN);
         }
     }
 }

 void lightPinExclusive(int pin) {
+    flashingPin = VOID_PIN;
+
     for (int i = 0; i < pinCount; i++) {
         digitalWrite(pins[i], pins[i] == pin ? HIGH : LOW);
     }
 }
+
+void flashPinExclusive(int pin) {
+    lightPinExclusive(pin);
+
+    flashingPin = pin;
+    flashingHigh = true;
+    flashStartTime = millis();
+}
+


calibration speed changed for L

calibration speed changed for L

--- mech/mech.ino
+++ mech/mech.ino
@@ -56,15 +56,15 @@
   pinMode(optInpin3, INPUT);            //  Defines Optical command PIN "C2"

   stepper1.setMaxSpeed(2000);           //  Set maximum roration speed for "L" Motor 1
-  stepper1.setSpeed(500);               //  Set maximum calibration speed for "L" Motor 1
+  stepper1.setSpeed(1500);               //  Set maximum calibration speed for "L" Motor 1
   stepper1.setAcceleration(2000);       //  Set maximum acceleration for "L" Motor 1

   stepper2.setMaxSpeed(2000);           //  Set maximum roration speed for "C" Motor 2
-  stepper2.setSpeed(500);               //  Set maximum calibration speed for "C" Motor 2
+  stepper2.setSpeed(1000);               //  Set maximum calibration speed for "C" Motor 2
   stepper2.setAcceleration(2000);       //  Set maximum acceleration for "C" Motor 2

   stepper3.setMaxSpeed(2000);           //  Set maximum roration speed for "C" Motor 3
-  stepper3.setSpeed(500);               //  Set maximum calibration speed for "C" Motor 3
+  stepper3.setSpeed(1000);               //  Set maximum calibration speed for "C" Motor 3
   stepper3.setAcceleration(2000);       //  Set maximum acceleration for "C" Motor 3

   stepper1.setCurrentPosition(0);       // Set "Zero" position "L" Motor 1
@@ -78,7 +78,7 @@
 // Calibration process for "L" Motor 1
 void calibrate1(boolean run) {
   long oldPosition = stepper1.currentPosition();
-  stepper1.setSpeed(-600);
+  stepper1.setSpeed(-1500);
   while (digitalRead(optInpin1) == LOW) {
     stepper1.runSpeed();
     busLoop();


Delete Loopback.ino

Delete Loopback.ino

--- examples/Loopback/Loopback.ino
+++ examples/Loopback/Loopback.ino
@@ -1,26 +0,0 @@
-#include <SPI.h>
-#include <SoftSPI.h>
-
-// Create a new SPI port with:
-// Pin 2 = MOSI,
-// Pin 3 = MISO,
-// Pin 4 = SCK
-SoftSPI mySPI(2, 3, 4);
-
-void setup() {
-	mySPI.begin();
-	Serial.begin(9600);
-}
-
-void loop() {
-	static uint8_t v = 0;
-
-	Serial.print("Sending value: ");
-	Serial.print(v, HEX);
-	uint8_t in = mySPI.transfer(v);
-	Serial.print(" Got value: ");
-	Serial.print(in, HEX);
-	Serial.println(v == in ? " PASS" : " FAIL");
-	delay(1000);
-	v++;
-}


check return code from write

check return code from write

--- examples/WriteTag/WriteTag.ino
+++ examples/WriteTag/WriteTag.ino
@@ -5,7 +5,7 @@
 NfcAdapter nfc = NfcAdapter();

 void setup() {
-      Serial.begin(115200);
+      Serial.begin(9600);
       Serial.println("NDEF Writer");
       nfc.begin();
 }
@@ -15,8 +15,13 @@
     if (nfc.tagPresent()) {
         NdefMessage message = NdefMessage();
         message.addUriRecord("http://arduino.cc");
-        nfc.write(message);
-        Serial.println("Try reading this tag with your phone.");
+
+        bool success = nfc.write(message);
+        if (success) {
+          Serial.println("Success. Try reading this tag with your phone.");
+        } else {
+          Serial.println("Write failed.");
+        }
     }
     delay(5000);
 }


done for today

done for today

--- projects/hsv_interaction/hsv_server/hsv_server.ino
+++ projects/hsv_interaction/hsv_server/hsv_server.ino
@@ -63,17 +63,13 @@

 /** Run as app client */
-Adafruit_NeoPixel * strip0 = NULL;
-HSV               * pixels = NULL;
-HsvMode           hsvMode = HSV_MODE_MONOCHROMATIC;
-
-HSV               nextHSV;
-float             nextHValue;
-
+Adafruit_NeoPixel * strip0      = NULL;
+HSV               * pixels      = NULL;
+HsvMode           hsvMode       = HSV_MODE_MONOCHROMATIC;
+float             nextHValue    = 0; // [0,1) => [0, 360] deg

 WifiState         wifiState     = WIFI_INIT;
 unsigned long     i2cHeartbeat  = 0;
-

 bool              isHost = false;
 int               numPixels = 0;
@@ -159,7 +155,7 @@
       case HSV_MODE_CHASE :
       case HSV_MODE_DECAY_CHASE :
         if (millis() > lastChange + 15000){
-          nextHValue  = 360.0f * HSVClockRandom().h;
+          nextHValue  = HSVClockRandom().h;
           lastChange  = millis();
         }
         break;
@@ -205,8 +201,7 @@
       if (byteBuffer[0] == 2){
         /** This is arduino <> arduino data */
         hsvMode = ((byteBuffer[1] % HSV_COUNT) + HSV_COUNT) % HSV_COUNT;
-        nextHSV = HSV(data/360.0f, 1.0f, 1.0f, 1.0f);
-        getNextHsvTime = millis();
+        nextHValue = data / 360.0f;
       }
     }
   }
@@ -221,7 +216,7 @@
   memset(byteBuffer, 0, I2C_BUF_SIZE);

   if (isHost){
-    int hval = (int)nextHValue;
+    int hval = (int)(nextHValue * 360);
     byteBuffer[0] = 1; // data available
     byteBuffer[1] = (byte)hsvMode;
     byteBuffer[2] = hval >> 8;
@@ -261,28 +256,32 @@
   bool dirty = false;

   if (nextNesRead < millis()){
+    static const float bigStep = 30.0f / 360.0f;
+    static const float smallStep = 0.01f / 360.0f;
+    static const int wait = 300;
+
     if ((data & 0x01) == 0){
-      nextHValue = fmod(nextHValue - 30.f + 360, 360.0f);
-      nextNesRead = 300 + millis();
+      nextHValue = fmod(nextHValue - bigStep + 1.0f, 1.0f);
+      nextNesRead = wait + millis();
       dirty = true;
     }else if((data & 0x02) == 0){
-      nextHValue = fmod(nextHValue + 30.f, 360.0f);
-      nextNesRead = 300 + millis();
+      nextHValue = fmod(nextHValue + bigStep, 1.0f);
+      nextNesRead = wait + millis();
       dirty = true;
     }else if((data & 0x04) == 0){
       hsvMode = (hsvMode + 1) % 16;
-      nextNesRead = 300 + millis();
+      nextNesRead = wait + millis();
       Serial.println(hsvMode);
       dirty = true;
     }else if((data & 0x08) == 0){
-      nextHValue = 360.0f * HSVClockRandom().h;
-      nextNesRead = 300 + millis();
+      nextHValue = HSVClockRandom().h;
+      nextNesRead = wait + millis();
       dirty = true;
     }else if((data & 0x40) == 0){
-      nextHValue = fmod(nextHValue - 0.01f + 360, 360.0f);
+      nextHValue = fmod(nextHValue - smallStep + 1.0f, 1.0f);
       dirty = true;
     }else if((data & 0x80) == 0){
-      nextHValue = fmod(nextHValue + 0.01f, 360.0f);
+      nextHValue = fmod(nextHValue + smallStep, 1.0f);
       dirty = true;
     }
   }
@@ -320,6 +319,7 @@

 /** WS2812: Set monochromatic colors */
 void setMonoChromatic(uint8_t wait){
+  HSV nextHSV(nextHValue, 1, 1, 1);
   for (int i=0; i<numPixels; i++){
     strip0->setPixelColor(i, nextHSV.ToRgb().ToUInt32());
   }
@@ -339,7 +339,7 @@
   if ( rising && offset >=  limit) rising = false;
   if (!rising && offset <= -limit) rising = true;

-  HSV hsv(nextHSV.h + offset, 1.0f, 1.0f, 1.0f);
+  HSV hsv(nextHValue + offset, 1.0f, 1.0f, 1.0f);
   for (int i=0; i<numPixels; i++){
     strip0->setPixelColor(i, hsv.ToRgb().ToUInt32());
   }
@@ -374,8 +374,8 @@

   /** reset ? */
   if (abs(pixels[targetPixel].h - targetHSV.h) < 0.01f){
+    targetHSV   = HSV(nextHValue, 1, 1, 1);
     targetPixel = random(numPixels);
-    targetHSV   = nextHSV;
   }

   strip0->show();
@@ -410,9 +410,8 @@

   /** reset ? */
   if (abs(pixels[targetPixel].h - targetHSV.h) < 0.01f){
+    targetHSV   = HSV(nextHValue, 1, 1, 1);
     targetPixel = random(numPixels);
-      targetHSV   = nextHSV;
-      targetPixel = random(numPixels);
   }

   strip0->show();


Verbessert_delaytime

--- Projekt-Code/Projekt-Code.ino
+++ Projekt-Code/Projekt-Code.ino
@@ -43,7 +43,7 @@
       delay(300);
       d = ultras()*10/58;
       digitalWrite(13,HIGH);
-      delay(300);
+      delay(500);
       digitalWrite(13,LOW);
       lcd.print("Abstand:");
       lcd.setCursor(3,1);


RTC Code added

RTC Code added

--- Firmware/i2c_ultrasonic1/i2c_ultrasonic1.ino
+++ Firmware/i2c_ultrasonic1/i2c_ultrasonic1.ino
@@ -1,6 +1,8 @@
 #include <Wire.h>
 #include "MMA7660.h"
+#include "DS1307.h"
 MMA7660 acc;
+DS1307 clock;

 #define SLAVE_ADDRESS 0x04
 int number = 0;
@@ -10,9 +12,10 @@
 int index=0;
 int flag=0;
 int i;
-byte val=0,b[4];
+byte val=0,b[9];
 int aRead=0;
-byte accFlag=0;
+byte accFlag=0,clkFlag=0;
+int8_t accv[3];
 void setup() {
     //pinMode(13, OUTPUT);
     //Serial.begin(9600);         // start serial for output
@@ -74,6 +77,7 @@
       //Serial.println(b[1]);
       //Serial.println(b[2]);
     }
+    //Accelerometer x,y,z, read
     if(cmd[0]==20)
     {
       if(accFlag==0)
@@ -81,8 +85,35 @@
         acc.init();
         accFlag=1;
       }
-      acc.getXYZ(&b[1],&b[2],&b[3]);
+      acc.getXYZ(&accv[0],&accv[1],&accv[2]);
+      b[1]=accv[0];
+      b[2]=accv[1];
+      b[3]=accv[2];
     }
+    //RTC tine read
+    if(cmd[0]=30)
+    {
+      if(clkFlag==0)
+      {
+        clock.begin();
+        //Set time the first time
+        //clock.fillByYMD(2013,1,19);
+        //clock.fillByHMS(15,28,30);//15:28 30"
+	//clock.fillDayOfWeek(SAT);//Saturday
+	//clock.setTime();//write time to the RTC chip
+        clkFlag=1;
+      }
+      clock.getTime();
+      b[1]=clock.hour;
+      b[2]=clock.minute;
+      b[3]=clock.second;
+      b[4]=clock.month;
+      b[5]=clock.dayOfMonth;
+      b[6]=clock.year;
+      b[7]=clock.dayOfMonth;
+      b[8]=clock.dayOfWeek;
+    }
+    //
   }
 }

@@ -112,5 +143,7 @@
   }
   if(cmd[0]==20)
     Wire.write(b, 4);
+  if(cmd[0]==30)
+    Wire.write(b, 9);
 }


add sensor id to rfdata frame

add sensor id to rfdata frame

--- Atom_Node.ino
+++ Atom_Node.ino
@@ -44,6 +44,7 @@
 #define __printlnIno(x)
 #endif

+
 unsigned char __GdtaUart[50];                   // uart data buff
 unsigned char __GdtaUartLen        = 0;         // uart data len
 unsigned char __GstringComplete    = 0;         // if get data


DOC: Improve documentation for debounce option

--- sketches/midi_organ_debounce/midi_organ_debounce.ino
+++ sketches/midi_organ_debounce/midi_organ_debounce.ino
@@ -4,17 +4,21 @@
 #include <midi_Message.h>
 #include <MIDI.h>
 /*
- Test serial-MIDI commands from a 61-key keyboard.
+ OrganDuino: serial-MIDI commands from multiple 61-key organ manuals and pedalboard.

  The circuit:
  * Up to five OrganDuino 2.0 PCB shields populated with diodes
  * 61-key organ manuals soldered in with buss soldered to a Drive Pin thru-hole
  * Solder jumpers on back for each manual properly set
- * 32-key AGO pedalboard soldered in to first
+ * 32-key AGO pedalboard soldered in to first 32 numbered keys on a separate OrganDuino 2.0 PCB
+
+ This version implements a simple debounce routine exclusively for the pedalboard,
+ but with tweaking could be used for other manual(s) if needed.

- created 2016-2019 by @JDWarner and @mscross
+ created 2016-2020 by @JDWarner and @mscross
 */

+// For convenience/conciseness
 #define keyON 127
 #define keyOFF 0
 #define no false
@@ -52,7 +56,7 @@
 // define how large some things are (matrix size)
 const int numKeys = 61;    // full organ manual, also length of keyPins and midiKeyNotes
 const int numPedals = 32;  // full AGO 32-key pedalboard
-const int numManuals = 5;  // number of manuals
+const int numManuals = 5;  // number of manuals (up to 5)

 const unsigned char midiKeyNotes[numKeys] = {36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, };
 const unsigned char midiChannels[numManuals] = {1, 2, 3, 4, 5, };  // Five manuals in first 5 MIDI channels


Don't print verbose information to USB if USB is being used to control stage.

--- OpenStage/OS_moveToTarget.ino
+++ OpenStage/OS_moveToTarget.ino
@@ -42,7 +42,7 @@

     stepsToTake[ii]=round(intendedMove[ii]/oneStep[ii]); //total number of steps we have to take

-    if (verbose){
+    if (verbose && !controlViaUSB){
       Serial.print(ii+1);
       Serial.print(". Targ: ");
       Serial.print(target[ii]);
@@ -79,7 +79,7 @@

   //Now run the steppers until they get there
-  if (verbose){
+  if (verbose && !controlViaUSB){
     Serial.println("Moving");
   }


Changed name of program to be consistent


Version auf 1.0 geändert

Version auf 1.0 geändert

--- BrewDuino.ino
+++ BrewDuino.ino
@@ -1,6 +1,6 @@
 #define CREATOR "Herbstmensch"
 #define PROG_NAME "BrewDuino"
-#define PROG_VERSION "v0.9"
+#define PROG_VERSION "v1.0"
 #define COPYRIGHT "(c) 2015 - "
 #define COPYRIGHT2 "Tim Herbst"
 #include <LCD5110_Basic.h>