From 46d0304ce009f818eec533bc68ab4bf9d8959707 Mon Sep 17 00:00:00 2001
From: Harald Barth <haba@kth.se>
Date: Tue, 16 Mar 2021 22:51:33 +0100
Subject: [PATCH] 28 speed steps as global debug option

---
 DCC.cpp         | 40 +++++++++++++++++++++++-----------------
 DCC.h           |  4 ++++
 DCCEXParser.cpp | 13 +++++++++++++
 3 files changed, 40 insertions(+), 17 deletions(-)

diff --git a/DCC.cpp b/DCC.cpp
index d103653..b676331 100644
--- a/DCC.cpp
+++ b/DCC.cpp
@@ -46,6 +46,7 @@ const byte FN_GROUP_5=0x10;
 
 FSH* DCC::shieldName=NULL;
 byte DCC::joinRelay=UNUSED_PIN;
+byte DCC::globalSpeedsteps=128;
 
 void DCC::begin(const FSH * motorShieldName, MotorDriver * mainDriver, MotorDriver* progDriver,
                  byte joinRelayPin) {
@@ -81,28 +82,33 @@ void DCC::setThrottle2( uint16_t cab, byte speedCode)  {
     b[nB++] = highByte(cab) | 0xC0;    // convert train number into a two-byte address
   b[nB++] = lowByte(cab);
 
-#ifdef USE_28_STEP
+  if (globalSpeedsteps <= 28) {
 
-  uint8_t speed128 = speedCode & 0x7F;
-  uint8_t speed28;
-  uint8_t code28;
+    uint8_t speed128 = speedCode & 0x7F;
+    uint8_t speed28;
+    uint8_t code28;
 
-  if (speed128 == 0 || speed128 == 1) { // stop or emergency stop
+    if (speed128 == 0 || speed128 == 1) { // stop or emergency stop
       code28 = speed128;
-  } else {
+    } else {
       speed28= (speed128*10+36)/46;                 // convert 2-127 to 1-28
-      code28 = (speed28+3)/2 + 16*(!(speed28 & 1)); // convert 1-28 to DCC 28 step speed code
+/*
+      if (globalSpeedsteps <= 14)                   // Don't want to do 14 steps, to get F0 there is ugly
+        code28 = (speed28+3)/2 | (Value of F0);     // convert 1-28 to DCC 14 step speed code
+      else
+*/
+      code28 = (speed28+3)/2 | ( (speed28 & 1) ? 0 : 0b00010000 ); // convert 1-28 to DCC 28 step speed code
+    }
+    //        Construct command byte from:
+    //        command      speed    direction
+    b[nB++] = 0b01000000 | code28 | (0b00100000 * (speedCode & 0x80));
+
+  } else { // 128 speedsteps
+
+    b[nB++] = SET_SPEED;                      // 128-step speed control byte
+    b[nB++] = speedCode; // for encoding see setThrottle
+
   }
-  //        Construct command byte from:
-  //        command      speed    direction
-  b[nB++] = 0b01000000 | code28 | (0b00100000 * (speedCode & 0x80));
-
-#else
-
-  b[nB++] = SET_SPEED;                      // 128-step speed control byte
-  b[nB++] = speedCode; // for encoding see setThrottle
-
-#endif
 
   DCCWaveform::mainTrack.schedulePacket(b, nB, 0);
 }
diff --git a/DCC.h b/DCC.h
index 4a9c5e0..382b7dd 100644
--- a/DCC.h
+++ b/DCC.h
@@ -102,6 +102,9 @@ public:
   static void displayCabList(Print *stream);
 
   static FSH *getMotorShieldName();
+  static inline void setGlobalSpeedsteps(byte s) {
+    globalSpeedsteps = s;
+  };
 
 private:
   struct LOCO
@@ -119,6 +122,7 @@ private:
   static bool issueReminder(int reg);
   static int nextLoco;
   static FSH *shieldName;
+  static byte globalSpeedsteps;
 
   static LOCO speedTable[MAX_LOCOS];
   static byte cv1(byte opcode, int cv);
diff --git a/DCCEXParser.cpp b/DCCEXParser.cpp
index 2556cc2..aa7713d 100644
--- a/DCCEXParser.cpp
+++ b/DCCEXParser.cpp
@@ -51,6 +51,9 @@ const int HASH_KEYWORD_LIMIT = 27413;
 const int HASH_KEYWORD_ETHERNET = -30767;    
 const int HASH_KEYWORD_MAX = 16244;
 const int HASH_KEYWORD_MIN = 15978;
+const int HASH_KEYWORD_SPEED28 = -17064;
+const int HASH_KEYWORD_SPEED128 = 25816;
+
 
 int DCCEXParser::stashP[MAX_COMMAND_PARAMS];
 bool DCCEXParser::stashBusy;
@@ -766,6 +769,16 @@ bool DCCEXParser::parseD(Print *stream, int params, int p[])
 	    EEStore::dump(p[1]);
 	return true;
 
+    case HASH_KEYWORD_SPEED28:
+        DCC::setGlobalSpeedsteps(28);
+	StringFormatter::send(stream, F("28 Speedsteps"));
+        return true;
+
+    case HASH_KEYWORD_SPEED128:
+        DCC::setGlobalSpeedsteps(128);
+	StringFormatter::send(stream, F("128 Speedsteps"));
+        return true;
+
     default: // invalid/unknown
         break;
     }