1 files changed, 177 insertions, 0 deletions

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;
+};