add duplo train remote

4 files changed, 557 insertions, 0 deletions

diff --git a/duplo_train/.gitignore b/duplo_train/.gitignore
new file mode 100644
index 0000000..89cc49c
--- /dev/null
+++ b/duplo_train/.gitignore
@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch
diff --git a/duplo_train/platformio.ini b/duplo_train/platformio.ini
new file mode 100644
index 0000000..31a8788
--- /dev/null
+++ b/duplo_train/platformio.ini
@@ -0,0 +1,26 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env]
+platform = espressif32
+board = wemos_d1_mini32
+framework = arduino
+lib_deps =
+  NimBLE-Arduino
+  Legoino
+  Bounce2
+monitor_speed = 115200
+;build_flags = -DCORE_DEBUG_LEVEL=ARDUHAL_LOG_LEVEL_DEBUG
+
+[env:wifi_toggle]
+build_flags = -D WIFI_TOGGLE
+
+[platformio]
+lib_dir=/home/manuel/coding/Arduino/libraries
\ No newline at end of file
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
+}
diff --git a/duplo_train/src/myhub.h b/duplo_train/src/myhub.h
new file mode 100644
index 0000000..2a3e690
--- /dev/null
+++ b/duplo_train/src/myhub.h
@@ -0,0 +1,177 @@
+#pragma once
+
+#include "Lpf2Hub.h"
+
+class MyHub
+  : public Lpf2Hub
+{
+public:
+  void activateBaseSpeaker()
+  {
+    byte setSoundMode[8] = { 0x41, (byte)DuploTrainHubPort::SPEAKER, 0x01, 0x01, 0x00, 0x00, 0x00, 0x01 };
+    Lpf2Hub::WriteValue(setSoundMode, 8);
+    m_last_cmd = millis();
+  }
+
+  void playSound(byte sound)
+  {
+    byte playSound[6] = { 0x81, (byte)DuploTrainHubPort::SPEAKER, 0x11, 0x51, 0x01, sound };
+    Lpf2Hub::WriteValue(playSound, 6);
+    m_last_cmd = millis();
+  }
+
+  void activateRgbLight()
+  {
+    byte port = getPortForDeviceType((byte)DeviceType::HUB_LED);
+    byte setColorMode[8] = { 0x41, port, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00 };
+    Lpf2Hub::WriteValue(setColorMode, 8);
+    m_last_cmd = millis();
+  }
+
+  void setLedColor(Color color)
+  {
+    byte port = getPortForDeviceType((byte)DeviceType::HUB_LED);
+    byte setColor[6] = { 0x81, port, 0x11, 0x51, 0x00, color };
+    Lpf2Hub::WriteValue(setColor, 6);
+    m_last_cmd = millis();
+  }
+
+  void setMotorSpeedLevel(int speed_level)
+  {
+    if (speed_level > 3)
+      speed_level = 3;
+    else if (speed_level < -3)
+      speed_level = -3;
+    Serial.print("new speed="); Serial.println(speed_level);
+
+    int speed_pct = 0;
+    switch(speed_level)
+    {
+      case 3:
+        speed_pct = 70;
+        break;
+      case 2:
+        speed_pct = 50;
+        break;
+      case 1:
+        speed_pct = 30;
+        break;
+      case -1:
+        speed_pct = -30;
+        break;
+      case -2:
+        speed_pct = -50;
+        break;
+      case -3:
+        speed_pct = -70;
+        break;
+      default:
+        speed_pct = 0;
+        break;
+    }
+
+    m_speed_level = speed_level;
+    setBasicMotorSpeed(speed_pct);
+    if (abs(m_speed_level) == 1)
+    {
+      delay(200);
+      setBasicMotorSpeed(speed_pct);
+    }
+  }
+
+  void incrMotorSpeed()
+  {
+    setMotorSpeedLevel(m_speed_level + 1);
+  }
+
+  void decrMotorSpeed()
+  {
+    setMotorSpeedLevel(m_speed_level - 1);
+  }
+
+  void stopMotor()
+  {
+    stopBasicMotor();
+    m_speed_level = 0;
+  }
+
+  void stopMotorWithBreak()
+  {
+    if (abs(m_speed_level) == 3)
+    {
+      playSound((byte)DuploTrainBaseSound::BRAKE);
+      delay(200);
+    }
+    return stopMotor();
+  }
+
+  int isMotorStopped()
+  {
+    // make sure last motor command is at least 1 seconds ago
+    // otherwise issueing a stop motor command might also falsely trigger this
+    return (m_speed_level == 0 && millis() - m_last_motor_cmd > 1000);
+  }
+
+  int currentSpeedLevel()
+  {
+    return m_speed_level;
+  }
+
+  void toggleLight()
+  {
+    if (m_color == Color::BLACK)
+    {
+      setLedColor(m_prev_color);
+      m_color = m_prev_color;
+      m_prev_color = Color::BLACK;
+    }
+    else
+    {
+      setLedColor(Color::BLACK);
+      m_prev_color = m_color;
+      m_color = Color::BLACK;
+    }
+  }
+
+  void cycleLightColor()
+  {
+    m_color = (Color)(int(m_color) + 1);
+    if (m_color >= Color::NUM_COLORS)
+      m_color = Color::PINK;
+    setLedColor(m_color);
+  }
+
+  void onConnect()
+  {
+    m_speed_level = 0;
+    m_last_cmd = m_last_motor_cmd = millis();
+    m_color = m_prev_color = Color::BLUE;
+  }
+
+  unsigned long millisLastCommand()
+  {
+    return m_last_cmd;
+  }
+
+private:
+  void stopBasicMotor()
+  {
+    Lpf2Hub::stopBasicMotor((byte)DuploTrainHubPort::MOTOR);
+    m_last_cmd = millis();
+    m_last_motor_cmd = m_last_cmd;
+  }
+
+  void setBasicMotorSpeed(int speed = 0)
+  {
+    Lpf2Hub::setBasicMotorSpeed((byte)DuploTrainHubPort::MOTOR, speed);
+    m_last_cmd = millis();
+    m_last_motor_cmd = m_last_cmd;
+  }
+
+private:
+  int m_speed_level = 0;
+  unsigned long m_last_motor_cmd = 0;
+  unsigned long m_last_cmd;
+  Color m_color = Color::BLACK;
+  Color m_prev_color = Color::BLACK;
+};