Сенсорный мини выключатель cо стеклянной панелью на nRF52832

int8_t timer_status = 0;
boolean sens_flag1 = 0;
boolean sens_flag2 = 0;
boolean switch_a = 0;
boolean switch_b = 0;
uint16_t temp;
float vcc;
int battery;
int old_battery;
uint32_t oldmillis;
uint32_t newmillis;
uint32_t interrupt_time;
uint32_t SLEEP_TIME = 7000;
uint32_t SLEEP_TIME_W;
uint32_t SLEEP_TIME_W2;
int NrfRSSI;
uint16_t NrfRSSI2;
boolean wait_off;

//#define MY_DEBUG
#define MY_DISABLED_SERIAL
#define MY_RADIO_NRF5_ESB
#define MY_PASSIVE_NODE
#define MY_NODE_ID 120
#define MY_PARENT_NODE_ID 0
#define MY_PARENT_NODE_IS_STATIC
#define MY_TRANSPORT_UPLINK_CHECK_DISABLED
#define POWER_CHILD_ID 110
#define UP_POWER_SWITCH_ID 1
#define DOWN_POWER_SWITCH_ID 2
#define CHILD_ID_nRF52_RSSI_RX 3

#define BAT_COOF 0.0092957746478873
#define BAT_MIN 200
#define BAT_MAX 290

#include <MySensors.h>
MyMessage upMsg(UP_POWER_SWITCH_ID, V_STATUS);
MyMessage downMsg(DOWN_POWER_SWITCH_ID, V_STATUS);
MyMessage powerMsg(POWER_CHILD_ID, V_VAR1);
MyMessage msgRF52RssiReceiv(CHILD_ID_nRF52_RSSI_RX, V_VAR1);

void preHwInit() {
  pinMode(31, OUTPUT); //power management pin
  digitalWrite(31, HIGH);
  delay(3000);
  pinMode(3, INPUT); // on off mode button
  pinMode(25, OUTPUT); // sens1 led
  pinMode(26, OUTPUT); // sens1 led
  pinMode(27, OUTPUT); // sens1 led
  pinMode(6, OUTPUT); // sens21 led
  pinMode(7, OUTPUT); // sens2 led
  pinMode(8, OUTPUT); // sens2 led

  pinMode(28, OUTPUT); // bizzer

  pinMode(2, INPUT); // common interrupt for touch sensors
  pinMode(9, INPUT); // touch sensors1
  pinMode(10, INPUT); //touch sensors2

  pinMode(29, INPUT); // battery

  digitalWrite(28, LOW);
  digitalWrite(27, HIGH);
  digitalWrite(26, HIGH);
  digitalWrite(25, HIGH);
  digitalWrite(6, HIGH);
  digitalWrite(7, HIGH);
  digitalWrite(8, HIGH);
}

void before()
{
  NRF_POWER->DCDCEN = 1;
  analogReadResolution(12);
  disableNfc();
  turnOffAdc();
  digitalWrite(25, LOW);
  digitalWrite(6, LOW);
  wait(200);
  digitalWrite(25, HIGH);
  digitalWrite(6, HIGH);
  wait(100);
  playSound0();
  wait(100);
  digitalWrite(25, LOW);
  digitalWrite(6, LOW);
  wait(200);
  digitalWrite(25, HIGH);
  digitalWrite(6, HIGH);
  wait(3000);

  digitalWrite(27, LOW);
  digitalWrite(8, LOW);
  wait(200);
  digitalWrite(27, HIGH);
  digitalWrite(8, HIGH);
  wait(400);
  digitalWrite(6, LOW);
  digitalWrite(25, LOW);
  wait(200);
  digitalWrite(6, HIGH);
  digitalWrite(25, HIGH);
  wait(400);
  digitalWrite(26, LOW);
  digitalWrite(7, LOW);
  wait(200);
  digitalWrite(26, HIGH);
  digitalWrite(7, HIGH);
  wait(1000);
  digitalWrite(26, LOW);
  digitalWrite(7, LOW);
}

void setup()
{
  digitalWrite(26, HIGH);
  digitalWrite(7, HIGH);
  wait(50);
  playSound();
  wait(2000);
  readBatLev();
  wait(200);
  SLEEP_TIME_W = SLEEP_TIME;
}

void presentation()
{
  sendSketchInfo("EFEKTA ON|OFF NODE 2CH", "1.0");
  wait(100);
  present(POWER_CHILD_ID, S_CUSTOM, "BATTERY DATA");
  wait(100);
  present(UP_POWER_SWITCH_ID, S_BINARY, "UP SWITCH");
  wait(100);
  present(DOWN_POWER_SWITCH_ID, S_BINARY, "DOWN SWITCH");
}

void loop()
{
  if (sens_flag1 == 0 && sens_flag2 == 0) {
    if (switch_a == 0 && switch_b == 0) {
      timer_status = sleep(digitalPinToInterrupt(2), RISING, digitalPinToInterrupt(3), RISING, 3600000, false);
      wait_off = 1;
    } else {
      //oldmillis = millis();
      timer_status = sleep(digitalPinToInterrupt(2), RISING, digitalPinToInterrupt(3), RISING, SLEEP_TIME_W, false);
      wait_off = 0;
    }
  }
  if (timer_status == 3) {
    wait(100);
    digitalWrite(27, LOW);
    digitalWrite(8, LOW);
    wait(2000);
    digitalWrite(27, HIGH);
    digitalWrite(8, HIGH);
    wait(100);
    digitalWrite(31, LOW);
  }

  if (timer_status == 2) {

    if (digitalRead(9) == HIGH && sens_flag1 == 0 && switch_b == 0) {
      sens_flag1 = 1;
      if (switch_a == 0) {
        oldmillis = millis();
        SLEEP_TIME_W = SLEEP_TIME;
        switch_a = 1;
        digitalWrite(6, LOW);
        wait(10);
        playSound1();
        wait(20);
        playSound2();
        wait(50);
        send(upMsg.set(switch_a));
        wait(200);
      } else {
        switch_a = 0;
        digitalWrite(6, HIGH);
        wait(10);
        playSound2();
        wait(20);
        playSound1();
        wait(50);
        send(upMsg.set(switch_a));
        wait(200);
      }
    }
    if (digitalRead(10) == HIGH && sens_flag2 == 0 && switch_a == 0) {
      sens_flag2 = 1;
      if (switch_b == 0) {
        oldmillis = millis();
        SLEEP_TIME_W = SLEEP_TIME;
        switch_b = 1;
        digitalWrite(25, LOW);
        wait(10);
        playSound1();
        wait(20);
        playSound2();
        wait(50);
        send(downMsg.set(switch_b));
        wait(200);
      } else {
        switch_b = 0;
        digitalWrite(25, HIGH);
        wait(10);
        playSound2();
        wait(20);
        playSound1();
        wait(50);
        send(downMsg.set(switch_b));
        wait(200);
      }
    }

    if (digitalRead(9) == LOW && sens_flag1 == 1) {
      sens_flag1 = 0;
    }

    if (digitalRead(10) == LOW && sens_flag2 == 1) {
      sens_flag2 = 0;
    }
    if (switch_a == 1 || switch_b == 1) {
      if (wait_off == 0) {
        newmillis = millis();
        wait(10);
        SLEEP_TIME_W2 = SLEEP_TIME_W;
        wait(10);
        interrupt_time = newmillis - oldmillis;
        wait(10);
        SLEEP_TIME_W = SLEEP_TIME_W2 - interrupt_time;
        wait(10);
        Serial.print("WAS IN A SLEEP: ");
        Serial.print(newmillis - oldmillis);
        Serial.println(" MILLISECONDS");

        if (SLEEP_TIME_W < 1000) {
          if (switch_a == 1) {
            switch_a = 0;
            digitalWrite(6, HIGH);
            wait(10);
            playSound2();
            wait(20);
            playSound1();
            wait(50);
            send(upMsg.set(switch_a));
            wait(200);
          }
          if (switch_b == 1) {
            switch_b = 0;
            digitalWrite(25, HIGH);
            wait(10);
            playSound2();
            wait(20);
            playSound1();
            wait(50);
            send(downMsg.set(switch_b));
            wait(200);
          }
          SLEEP_TIME_W = SLEEP_TIME;
          wait(50);
        }
        Serial.println(SLEEP_TIME);
        Serial.println(SLEEP_TIME_W);
        Serial.println(SLEEP_TIME_W2);
        Serial.print("GO TO SLEEP FOR: ");
        Serial.print(SLEEP_TIME_W);
        Serial.println(" MILLISECONDS");
      }
      oldmillis = millis();
    }
  }

  if (timer_status == -1) {
    if (switch_a == 1 || switch_b == 1) {
      if (switch_a == 1) {
        switch_a = 0;
        digitalWrite(6, HIGH);
        wait(10);
        playSound2();
        wait(20);
        playSound1();
        wait(50);
        send(upMsg.set(switch_a));
        wait(200);
      }
      if (switch_b == 1) {
        switch_b = 0;
        digitalWrite(25, HIGH);
        wait(10);
        playSound2();
        wait(20);
        playSound1();
        wait(50);
        send(downMsg.set(switch_b));
        wait(200);
      }
    } else {
      readBatLev();
    }
  }
}

void disableNfc() {
  NRF_NFCT->TASKS_DISABLE = 1;
  NRF_NVMC->CONFIG = 1;
  NRF_UICR->NFCPINS = 0;
  NRF_NVMC->CONFIG = 0;
}

void turnOffAdc() {
  if (NRF_SAADC->ENABLE) {
    NRF_SAADC->TASKS_STOP = 1;
    while (NRF_SAADC->EVENTS_STOPPED) {}
    NRF_SAADC->ENABLE = 0;
    while (NRF_SAADC->ENABLE) {}
  }
}

void myTone(uint32_t j, uint32_t k) {
  j = 500000 / j;
  k += millis();
  while (k > millis()) {
    digitalWrite(28, HIGH); delayMicroseconds(j);
    digitalWrite(28, LOW ); delayMicroseconds(j);
  }
}

void playSound0() {
  myTone(1300, 50);
  wait(20);
  myTone(1300, 50);
  wait(50);
}

void playSound() {
  myTone(700, 30); 
  wait(10);
  myTone(700, 30);
  wait(10);
  myTone(700, 30);
  wait(50);
}

void playSound1() {
  myTone(200, 10);
  wait(10);
  myTone(400, 5);
  wait(30);
}

void playSound2() {
  myTone(400, 10);
  wait(10);
  myTone(200, 5);
  wait(30);
}

void readBatLev() {
  temp = analogRead(29);
  vcc = temp * 0.0033 * 100;
  battery = map((int)vcc, BAT_MIN, BAT_MAX, 0, 100);
  if (battery < 0) {
    battery = 0;
  }
  if (battery > 100) {
    battery = 100;
  }
  sendBatteryLevel(battery, 1);
  wait(2000, C_INTERNAL, I_BATTERY_LEVEL);
  send(powerMsg.set(temp));
  wait(200);
  NrfRSSI = transportGetReceivingRSSI();
  NrfRSSI2 = map(NrfRSSI, -85, -40, 0, 100);
  if (NrfRSSI2 < 0) {
    NrfRSSI2 = 0;
  }
  if (NrfRSSI2 > 100) {
    NrfRSSI2 = 100;
  }
  send(msgRF52RssiReceiv.set(NrfRSSI2));
  wait(200);
}