Not working BluEOS

BluEOS.ino

#define DEBUG
#define BLUETOOTH
#define BT_HC06

#include <CmdMessenger.h>
#include <stdlib.h>
#ifdef BLUETOOTH
  #include <SoftReset.h>
#endif

#define pinSHUTTER      4
#define pinFOCUS        5
#define pinLED          13
#ifdef BLUETOOTH
  #define pinKey        6
  #define pinReset      2
  #define resetThresh   2000 //ms
  #define btBaud        9600
  #define defaultName   "BluEOS"
  #define defaultNameLen 6
  #define defaultPin    2015
  #define defaultBaud   4 //9600
  #define atCmdDelay    1100
#endif

enum {
  kHELP,
  kAPI,
  kTECH,
  kACK,
  kERR,
  kSHOOT,
  kFOCUS,
  kRELEASE,
  kTLAPSE,
  kTLSHOTSIG,
  kDELAY,
  kCANCEL,
  kDONE,
  kBOOTED,
#ifdef BLUETOOTH
  kBT_COMMIT,
  kBT_CHNAME,
  kBT_CHPIN,
  kBT_CHBAUD,
  kBT_VERSION,
  kBT_RESET,
#endif
};

#ifndef BLUETOOTH
  #define NUM_CMDs        14
  #define API_VERSION     "v1ser"
  #define DEV_TECH        "serial"
#else
  #define NUM_CMDs        20
  #define API_VERSION     "v1bt"
  #define DEV_TECH        "bluetooth"
#endif

#define LEDON_T         200
#define LEDOFF_T        1600

bool stop_delay = false;
bool led_state = false;
long prev_time = 0;

#ifdef BLUETOOTH
  bool bt_chname = false;
  bool bt_chpin = false;
  bool bt_chbaud = false;
  char* bt_newname;
  uint8_t bt_newnamelen;
  uint16_t bt_newpin;
  uint8_t bt_newbaud;
#endif

CmdMessenger cmdMessenger = CmdMessenger(Serial);

uint8_t stoppableDelay(uint32_t delayt, uint16_t stept=100) {
  uint32_t t1;
#ifdef DEBUG
  Serial.print(F("Sleeping for "));
  Serial.print(delayt);
  Serial.print(F("ms with a step of "));
  Serial.println(stept);
#endif
  for (uint32_t timeSpent=0; timeSpent < delayt;) {
    checkRoutines();
    t1 = millis();
    delay(stept);
    if (!stop_delay) {
      timeSpent += millis() - t1;
    } else {
      #ifdef DEBUG
        Serial.println(F("Delay interrupted"));
      #endif
      stop_delay = false;
      return 1;
      break;
    }
  }
  #ifdef DEBUG
    Serial.println(F("Delay terminated"));
  #endif
  return 0;
}

uint8_t pressButton(uint8_t pin, int32_t hold) {
  stop_delay = false;
  digitalWrite(pin, HIGH);
  #ifdef DEBUG
    Serial.println(F("Button pressed"));
  #endif
  switch (hold) {
    case -1:
      break;
    case 0:
      // Simulate human button press
      delay(100);
      digitalWrite(pin, LOW);
      #ifdef DEBUG
        Serial.println(F("Button released"));
      #endif
      break;
    default:
      uint8_t err = stoppableDelay(hold);
      digitalWrite(pin, LOW);
      #ifdef DEBUG
        Serial.println(F("Button released"));
      #endif
      return err;
}
  return 0;
}

uint8_t doTimeLapse(uint32_t hold, uint32_t del, uint16_t shots) {
  stop_delay = false;
  for (uint16_t i = 0; i < shots; i++) {
    #ifdef DEBUG
      Serial.print(F("Timelapse, shot "));
      Serial.print(i+1);
      Serial.print(F(" of "));
      Serial.println(shots);
    #endif
    sendAck(kACK, kSHOOT, F("Timelapse shutter down"));
    uint8_t err = pressButton(pinSHUTTER, hold);
    sendAck(kDONE, kSHOOT, F("Timelapse shutter up"));
    if (err == 1) {
      return err;
      break;
    }
    sendAck(kTLSHOTSIG, i+1, F("Timelapse shot taken"));
    if (i < shots-1) {
      err = stoppableDelay(del);
      if (err == 1) {
        return err;
        break;
      }
    }
  }
  return 0;
}

void toggleLED() {
  led_state = !led_state;
  prev_time = millis();
  digitalWrite(pinLED, (int)led_state);
}

void checkLED() {
  if (led_state && millis() - prev_time > LEDON_T) {
    toggleLED();
  } else if (!led_state && millis() - prev_time > LEDOFF_T) {
    toggleLED();
  }
}

void checkRoutines() {
  checkLED();
  cmdMessenger.feedinSerialData();
}

void onUnknownCommand() {
  sendAck(kERR, 255, F("Unknown command"));
}

void sendAck(byte ack, byte cmd, char* msg) {
  cmdMessenger.sendCmdStart(ack);
  cmdMessenger.sendCmdArg(cmd);
  cmdMessenger.sendCmdArg(msg);
  cmdMessenger.sendCmdEnd();
}

void sendAck(byte ack, byte cmd, const __FlashStringHelper* msg) {
  cmdMessenger.sendCmdStart(ack);
  cmdMessenger.sendCmdArg(cmd);
  cmdMessenger.sendCmdArg(msg);
  cmdMessenger.sendCmdEnd();
}

void onShoot() {
  int32_t hold = cmdMessenger.readInt32Arg();
  sendAck(kACK, kSHOOT, F("Shoot command received"));
  pressButton(pinSHUTTER, hold);
  if (hold > -1) {
    sendAck(kDONE, kSHOOT, F("Shoot command terminated"));
  }
}

void onFocus() {
  int32_t hold = cmdMessenger.readInt32Arg();
  sendAck(kACK, kFOCUS, F("Focus command received"));
  pressButton(pinFOCUS, hold);
  if (hold > -1) {
    sendAck(kDONE, kFOCUS, F("Focus command terminated"));
  }
}

void onRelease() {
  sendAck(kACK, kRELEASE, F("Release buttons command received"));
  digitalWrite(pinSHUTTER, LOW);
  digitalWrite(pinFOCUS, LOW);
  sendAck(kDONE, kRELEASE, F("Buttons released"));
}

void onTimeLapse() {
  uint32_t hold = cmdMessenger.readInt32Arg();
  uint32_t del = cmdMessenger.readInt32Arg();
  uint16_t shots = cmdMessenger.readInt16Arg();
  sendAck(kACK, kTLAPSE, F("Time lapse command received"));
  uint8_t err = doTimeLapse(hold, del, shots);
  if (err) {
    sendAck(kERR, kTLAPSE, F("Time lapse interrupted by user"));
  } else {
    sendAck(kDONE, kTLAPSE, F("Time lapse done"));
  }
}

void onAPIVersion() {
  sendAck(kACK, kAPI, API_VERSION);
}

void onDeviceTechnology() {
  sendAck(kACK, kTECH, DEV_TECH);
}

void onCancel() {
  sendAck(kACK, kCANCEL, F("Operations will be canceled ASAP"));
  stop_delay = true;
  sendAck(kDONE, kCANCEL, F("Operations should have been canceled"));
}

void onDelay() {
  uint32_t del = cmdMessenger.readInt32Arg();
  sendAck(kACK, kDELAY, F("Delay started"));
  uint8_t err = stoppableDelay(del);
  if (err) {
    sendAck(kERR, kDELAY, F("Delay interrupted by user"));
  } else {
    sendAck(kDONE, kDELAY, F("Delay ended"));
  }
}

void onHelp() {
  #ifdef DEBUG
    Serial.print(F("kHELP: "));
    Serial.println((byte) kHELP);
    Serial.print(F("kAPI: "));
    Serial.println((byte) kAPI);
    Serial.print(F("kTECH: "));
    Serial.println((byte) kTECH);
    Serial.print(F("kSHOOT: "));
    Serial.println((byte) kSHOOT);
    Serial.print(F("kFOCUS: "));
    Serial.println((byte) kFOCUS);
    Serial.print(F("kRELEASE: "));
    Serial.println((byte) kRELEASE);
    Serial.print(F("kTLAPSE: "));
    Serial.println((byte) kTLAPSE);
    Serial.print(F("kTLSHOTSIG: "));
    Serial.println((byte) kTLSHOTSIG);
    Serial.print(F("kDELAY: "));
    Serial.println((byte) kDELAY);
    Serial.print(F("kACK: "));
    Serial.println((byte) kACK);
    Serial.print(F("kERR: "));
    Serial.println((byte) kERR);
    Serial.print(F("kCANCEL: "));
    Serial.println((byte) kCANCEL);
    Serial.print(F("kDONE: "));
    Serial.println((byte) kDONE);
    Serial.print(F("kBOOTED: "));
    Serial.println((byte) kBOOTED);
    #ifdef BLUETOOTH
      Serial.print(F("kBT_COMMIT: "));
      Serial.println((byte) kBT_COMMIT);
      Serial.print(F("kBT_CHNAME: "));
      Serial.println((byte) kBT_CHNAME);
      Serial.print(F("kBT_CHPIN: "));
      Serial.println((byte) kBT_CHPIN);
      Serial.print(F("kBT_CHBAUD: "));
      Serial.println((byte) kBT_CHBAUD);
      Serial.print(F("kBT_VERSION: "));
      Serial.println((byte) kBT_VERSION);
      Serial.print(F("kBT_RESET: "));
      Serial.println((byte) kBT_RESET);
    #endif
  #else
    sendAck(kERR, kHELP, F("Not active"));
  #endif
}

#ifdef BT_HC06
void btCommit() {
  Serial.println(bt_chname);
  Serial.println(bt_chpin);
  Serial.println(bt_chbaud);
  if (bt_chname) {
    digitalWrite(pinKey, HIGH);
    Serial.print(F("AT+NAME"));
    Serial.print(bt_newname);
    delay(atCmdDelay);
    digitalWrite(pinKey, LOW);
    delay(100);
  }
  if (bt_chpin) {
    digitalWrite(pinKey, HIGH);
    Serial.print(F("AT+PIN"));
    Serial.print(bt_newpin);
    delay(atCmdDelay);
    digitalWrite(pinKey, LOW);
    delay(100);
  }
  if (bt_chbaud) {
    digitalWrite(pinKey, HIGH);
    Serial.print(F("AT+BAUD"));
    Serial.print(bt_newbaud, HEX);
    delay(atCmdDelay);
    digitalWrite(pinKey, LOW);
    delay(100);
  }
}

void btReset() {
    bt_newname = defaultName;
    bt_newnamelen = defaultNameLen;
    bt_newpin = defaultPin;
    bt_newbaud = defaultBaud;
    bt_chname = true;
    bt_chpin = true;
    bt_chbaud = true;
}
//end ifdef BT_HC06
#endif 

#ifdef BLUETOOTH
void mcuReset() {
    for (int i = 0; i < 3; i++) {
      digitalWrite(pinLED, HIGH);
      delay(100);
      digitalWrite(pinLED, LOW);
      delay(100);
    }
    soft_restart();
}

void onBtChName() {
  sendAck(kACK, kBT_CHNAME, F("Storing new name."));
  uint8_t len = cmdMessenger.readInt16Arg();
  cmdMessenger.copyStringArg(bt_newname, len);
  bt_chname = true;
  sendAck(kDONE, kBT_CHNAME, F("New name stored. Please commit."));
}

void onBtChPin() {
  sendAck(kACK, kBT_CHPIN, F("Storing new pin."));
  bt_newpin = cmdMessenger.readInt16Arg();
  bt_chpin = true;
  sendAck(kDONE, kBT_CHPIN, F("New pin stored. Please commit."));
}

void onBtChBaud() {
  sendAck(kACK, kBT_CHBAUD, F("Storing new baud."));
  bt_newbaud = cmdMessenger.readInt16Arg();
  bt_chbaud = true;
  sendAck(kDONE, kBT_CHBAUD, F("New baud stored. Please commit."));
}

void onBtCommit() {
  sendAck(kACK, kBT_COMMIT, F("Committing changes."));
  sendAck(kDONE, kBT_COMMIT, F("After LED blinks 3x, please power cycle."));
  digitalWrite(pinLED, HIGH);
  btCommit();
  mcuReset();
}

void onBtReset() {
  sendAck(kACK, kBT_RESET, F("Resetting Bluetooth settings."));
  sendAck(kDONE, kBT_RESET, F("After LED blinks 3x, please power cycle."));
  digitalWrite(pinLED, HIGH);
  btReset();
  btCommit();
  mcuReset();
}

void resetISR() {
  sendAck(kACK, kBT_RESET, F("Reset button pressed."));
  delay(resetThresh);
  sendAck(kDONE, kBT_RESET, F("Resetting. After LED blinks 3x, please power cycle."));
  if (digitalRead(pinReset) == LOW) {
    sendAck(kERR, kBT_RESET, F("Reset canceled."));
    digitalWrite(pinLED, HIGH);
    btReset();
    btCommit();
    mcuReset();
  }
}
//end ifdef BLUETOOTH
#endif

void setup() {
  Serial.begin(115200);
  pinMode(pinSHUTTER, OUTPUT);
  pinMode(pinFOCUS, OUTPUT);
  pinMode(pinLED, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  toggleLED();

  #ifdef BLUETOOTH
    pinMode(pinKey, OUTPUT);
    pinMode(pinReset, INPUT_PULLUP);
    attachInterrupt(0, resetISR, FALLING);
  #endif

  #ifdef DEBUG
    cmdMessenger.printLfCr();
  #endif

  cmdMessenger.attach(onUnknownCommand);
  cmdMessenger.attach(kHELP, onHelp);
  cmdMessenger.attach(kSHOOT, onShoot);
  cmdMessenger.attach(kFOCUS, onFocus);
  cmdMessenger.attach(kRELEASE, onRelease);
  cmdMessenger.attach(kTLAPSE, onTimeLapse);
  cmdMessenger.attach(kDELAY, onDelay);
  cmdMessenger.attach(kAPI, onAPIVersion);
  cmdMessenger.attach(kTECH, onDeviceTechnology);
  cmdMessenger.attach(kCANCEL, onCancel);


  #ifdef BLUETOOTH
  cmdMessenger.attach(kBT_COMMIT, onBtCommit);
  cmdMessenger.attach(kBT_CHNAME, onBtChName);
  cmdMessenger.attach(kBT_CHPIN, onBtChPin);
  cmdMessenger.attach(kBT_CHBAUD, onBtChBaud);
  //cmdMessenger.attach(kBT_VERSION, on);
  cmdMessenger.attach(kBT_RESET, onBtReset);
  #endif

  cmdMessenger.sendCmd(kBOOTED, F("Microcontroller booted"));
}

void loop() {
  checkRoutines();
}