1 files changed, 349 insertions, 0 deletions

diff --git a/duplo_train/src/main.cpp b/duplo_train/src/main.cpp
new file mode 100644
index 0000000..0922c6e
--- /dev/null
+++ b/duplo_train/src/main.cpp
@@ -0,0 +1,349 @@
+
+#include <Arduino.h>
+#include <Bounce2.h>
+#include <WiFi.h>
+
+#include <esp_wifi.h>
+#include <esp_bt.h>
+#include <soc/soc.h>
+#include <soc/rtc_cntl_reg.h>
+
+#include "myhub.h"
+
+const uint8_t BUTTON_BASE_1   = 25;
+const uint8_t BUTTON_BASE_2   = 22;
+const uint8_t BUTTON_BASE_3   = 26;
+const uint8_t BUTTON_BASE_4   = 18;
+const uint8_t BUTTON_BASE_5   = 33;
+const uint8_t BUTTON_BASE_6   = 5;
+const uint8_t BUTTON_BASE_7   = 19;
+const uint8_t BUTTON_BASE_8   = 23;
+const uint8_t BUTTON_HANDLE_1 = 17;
+const uint8_t BUTTON_HANDLE_2 = 16;
+const uint8_t POTI_THROTTLE   = 32;
+
+const int idle_shutdown = 10 * 60 * 1000;
+unsigned long last_scan_start = 0;
+
+Bounce2::Button button_base1   = Bounce2::Button();
+Bounce2::Button button_base2   = Bounce2::Button();
+Bounce2::Button button_base3   = Bounce2::Button();
+Bounce2::Button button_base4   = Bounce2::Button();
+Bounce2::Button button_base5   = Bounce2::Button();
+Bounce2::Button button_base6   = Bounce2::Button();
+Bounce2::Button button_base7   = Bounce2::Button();
+Bounce2::Button button_base8   = Bounce2::Button();
+Bounce2::Button button_handle1 = Bounce2::Button();
+Bounce2::Button button_handle2 = Bounce2::Button();
+
+MyHub myHub;
+
+void color_sensor_callback(void *hub, byte port_number,
+  DeviceType device_type, uint8_t *data)
+{
+  MyHub *myHub = (MyHub *)hub;
+
+  if (device_type == DeviceType::DUPLO_TRAIN_BASE_COLOR_SENSOR)
+  {
+    int color = myHub->parseColor(data);
+    Serial.print("Color: ");
+    Serial.println(color);
+  }
+}
+
+void speedometer_sensor_callback(void *hub, byte port_number,
+  DeviceType device_type, uint8_t *data)
+{
+  MyHub *myHub = (MyHub *)hub;
+
+  if (device_type == DeviceType::DUPLO_TRAIN_BASE_SPEEDOMETER)
+  {
+    int current_speed = myHub->parseSpeedometer(data);
+    Serial.print("speedometer="); Serial.println(current_speed);
+    if (current_speed == 0)
+    {
+      //TODO child has fun with "crashing" the train into things
+      // and the train reacts with the break sound
+      //myHub->stopMotor();
+      myHub->stopMotorWithBreak();
+    }
+    else if (myHub->isMotorStopped())
+    {
+      if (current_speed > 10)
+        myHub->setMotorSpeedLevel(1);
+      else if (current_speed < -10)
+        myHub->setMotorSpeedLevel(-1);
+    }
+  }
+}
+
+#ifdef WIFI_TOGGLE
+enum wifi_enable_state
+  : uint8_t
+{
+  Enable  = 0,
+  Disable = 1,
+  None    = 2,
+};
+
+hw_timer_t *timer = NULL;
+
+volatile wifi_enable_state wifi_enable = wifi_enable_state::None;
+void IRAM_ATTR on_timer()
+{
+  static volatile int cnt = 0;
+  cnt = (cnt < 15) ? cnt + 1 : 0;
+  if (cnt == 0)
+    wifi_enable = wifi_enable_state::Enable;
+  else if (cnt == 1)
+    wifi_enable = wifi_enable_state::Disable;
+}
+#endif
+
+void controller_deep_sleep()
+{
+  Serial.print("Controller deep sleep enabled");
+
+#ifdef WIFI_TOGGLE
+  WiFi.disconnect(true);
+  WiFi.mode(WIFI_OFF);
+  esp_wifi_stop();
+#endif
+
+  NimBLEDevice::getScan()->stop();
+  NimBLEDevice::deinit();
+  esp_bt_controller_disable();
+
+  // disable brownout detector
+  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
+  //adc_power_release();
+  esp_deep_sleep_start();
+}
+
+void setup()
+{
+  // disable brownout detector
+  // problems with the usb battery?
+  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
+
+  Serial.begin(115200);
+  pinMode(BUILTIN_LED, OUTPUT);
+
+  analogReadResolution(12); //12 bits
+  analogSetAttenuation(ADC_0db);  //For all pins
+  analogSetPinAttenuation(POTI_THROTTLE, ADC_11db); //11db attenuation
+
+  button_base1.attach(BUTTON_BASE_1, INPUT_PULLUP);
+  button_base2.attach(BUTTON_BASE_2, INPUT_PULLUP);
+  button_base3.attach(BUTTON_BASE_3, INPUT_PULLUP);
+  button_base4.attach(BUTTON_BASE_4, INPUT_PULLUP);
+  button_base5.attach(BUTTON_BASE_5, INPUT_PULLUP);
+  button_base6.attach(BUTTON_BASE_6, INPUT_PULLUP);
+  button_base7.attach(BUTTON_BASE_7, INPUT_PULLUP);
+  button_base8.attach(BUTTON_BASE_8, INPUT_PULLUP);
+  button_handle1.attach(BUTTON_HANDLE_1, INPUT_PULLUP);
+  button_handle2.attach(BUTTON_HANDLE_2, INPUT_PULLUP);
+
+  button_base1.setPressedState(LOW);
+  button_base2.setPressedState(LOW);
+  button_base3.setPressedState(LOW);
+  button_base4.setPressedState(LOW);
+  button_base5.setPressedState(LOW);
+  button_base6.setPressedState(LOW);
+  button_base7.setPressedState(LOW);
+  button_base8.setPressedState(LOW);
+  button_handle1.setPressedState(LOW);
+  button_handle2.setPressedState(LOW);
+
+  last_scan_start = 0;
+  myHub.init();
+
+  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_DEFAULT);
+  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL0);
+  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL1);
+  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL2);
+  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL3);
+  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL4);
+  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL5);
+  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL6);
+  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL7);
+  NimBLEDevice::setPower(ESP_PWR_LVL_P9, ESP_BLE_PWR_TYPE_CONN_HDL8);
+
+#ifdef WIFI_TOGGLE
+  timer = timerBegin(0, 80, true); // 1 tick = 1/(80MHZ) => x/80 = 1us
+  timerAttachInterrupt(timer, &on_timer, true);
+  timerAlarmWrite(timer, 1000000, true); // 1000000 * 1us = 1sec
+  timerAlarmEnable(timer);
+#endif
+}
+
+void loop()
+{
+  /*
+   * TODO
+   * - color sensor
+   * - throttle
+   */
+  if (!myHub.isConnecting() && !myHub.isConnected())
+  {
+    if (last_scan_start == 0)
+    {
+      delay(1000); // avoid fast-reconnect loops
+      last_scan_start = millis();
+    }
+    if (millis() - last_scan_start > idle_shutdown)
+      (void)controller_deep_sleep(); // never returns
+
+    if (!NimBLEDevice::getScan()->isScanning())
+    {
+      myHub.init();
+      Serial.println("BLE is scanning...");
+    }
+  }
+  else if (myHub.isConnecting())
+  {
+    Serial.println("Duplo Hub found");
+    myHub.connectHub();
+    if (myHub.isConnected())
+    {
+      last_scan_start = 0;
+
+      Serial.println("Connected to Duplo Hub");
+      myHub.onConnect();
+      delay(200);
+
+      // connect speed sensor and activate it for updates
+      myHub.activatePortDevice((byte)DuploTrainHubPort::SPEEDOMETER,
+        speedometer_sensor_callback);
+      delay(200);
+
+      // activate light
+      myHub.activateRgbLight();
+      delay(200);
+
+      // activate speaker
+      myHub.activateBaseSpeaker();
+      delay(200);
+    }
+    else
+    {
+      Serial.println("Failed to connect to Duplo Hub");
+      return;
+    }
+  }
+  else if (myHub.isConnected())
+  {
+    button_base1.update();
+    if (button_base1.pressed())
+    {
+      Serial.println("Button1 pressed");
+      myHub.playSound((byte)DuploTrainBaseSound::STEAM);
+    }
+
+    button_base2.update();
+    if (button_base2.pressed())
+    {
+      Serial.println("Button2 - brake pressed");
+      myHub.stopMotorWithBreak();
+    }
+
+    button_base3.update();
+    if (button_base3.pressed())
+    {
+      Serial.println("Button3 - light color pressed");
+      myHub.cycleLightColor();
+    }
+
+    button_base4.update();
+    if (button_base4.pressed())
+    {
+      Serial.println("Button4 - light toggle pressed");
+      myHub.toggleLight();
+    }
+
+    button_base5.update();
+    if (button_base5.pressed())
+    {
+      Serial.println("Button5 - refill pressed");
+      myHub.playSound((byte)DuploTrainBaseSound::WATER_REFILL);
+    }
+
+    button_base6.update();
+    if (button_base6.pressed())
+    {
+      Serial.println("Button6 - horn pressed");
+      myHub.playSound((byte)DuploTrainBaseSound::HORN);
+    }
+
+    button_base7.update();
+    if (button_base7.pressed())
+    {
+      Serial.println("Button7 - brake pressed");
+      myHub.stopMotorWithBreak();
+    }
+
+    button_base8.update();
+    if (button_base8.pressed())
+    {
+      Serial.println("Button8 pressed");
+      myHub.playSound((byte)DuploTrainBaseSound::STATION_DEPARTURE);
+    }
+
+    button_handle1.update();
+    if (button_handle1.pressed())
+    {
+      Serial.println("Button Handle1 - speed++");
+      myHub.incrMotorSpeed();
+    }
+
+    button_handle2.update();
+    if (button_handle2.pressed())
+    {
+      Serial.println("Button Handle2 - speed--");
+      myHub.decrMotorSpeed();
+    }
+
+    if (myHub.isMotorStopped() && millis() - myHub.millisLastCommand() > idle_shutdown)
+    {
+      myHub.shutDownHub();
+      delay(500);
+      myHub.shutDownHub();
+      delay(500);
+
+      (void)controller_deep_sleep(); // never returns
+    }
+
+#if 0
+    int potValue = analogRead(POTI_THROTTLE);
+    delay(50);
+    int potValue2 = analogRead(POTI_THROTTLE);
+    if (potValue == potValue2) {
+      //if (potValue <= )
+      Serial.print(" pot value "); Serial.println(potValue);
+    }
+    int speedValue = map(potValue, 0, 4095, -2, 2);
+    if (potValue == potValue2 && speed != speedValue)
+    {
+      Serial.print("old speed "); Serial.print(speed);
+      Serial.print(" new speed "); Serial.print(speedValue);
+      Serial.print("New speed "); Serial.println(speedValue * 35);
+      //myHub.setBasicMotorSpeed(motorPort, speedValue * 35);
+      speed = speedValue;
+    }
+    delay(50);
+#endif
+  }
+
+#ifdef WIFI_TOGGLE
+  if (wifi_enable != wifi_enable_state::None)
+  {
+    timerAlarmDisable(timer);
+    Serial.print("new mode ");
+    Serial.println((wifi_enable == wifi_enable_state::Enable) ? "WIFI_STA" : "WIFI_OFF");
+    WiFi.mode((wifi_enable == wifi_enable_state::Enable) ? WIFI_STA : WIFI_OFF);
+    digitalWrite(BUILTIN_LED, (wifi_enable == wifi_enable_state::Enable) ? HIGH : LOW);
+    wifi_enable = wifi_enable_state::None;
+    timerAlarmEnable(timer);
+  }
+#endif
+}