diff --git a/DCC.cpp b/DCC.cpp index 5a9a2fd..0185ce9 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) { shieldName=(FSH *)motorShieldName; @@ -82,8 +83,34 @@ void DCC::setThrottle2( uint16_t cab, byte speedCode) { if (cab > 127) b[nB++] = highByte(cab) | 0xC0; // convert train number into a two-byte address b[nB++] = lowByte(cab); - b[nB++] = SET_SPEED; // 128-step speed control byte - b[nB++] = speedCode; // for encoding see setThrottle + + if (globalSpeedsteps <= 28) { + + uint8_t speed128 = speedCode & 0x7F; + uint8_t speed28; + uint8_t code28; + + if (speed128 == 0 || speed128 == 1) { // stop or emergency stop + code28 = speed128; + } else { + speed28= (speed128*10+36)/46; // convert 2-127 to 1-28 +/* + 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 | ((speedCode & 0x80) ? 0b00100000 : 0); + + } else { // 128 speedsteps + + b[nB++] = SET_SPEED; // 128-step speed control byte + b[nB++] = speedCode; // for encoding see setThrottle + + } DCCWaveform::mainTrack.schedulePacket(b, nB, 0); } diff --git a/DCC.h b/DCC.h index c4a6ae3..0f5c489 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 e344a58..cfb7219 100644 --- a/DCCEXParser.cpp +++ b/DCCEXParser.cpp @@ -54,6 +54,8 @@ const int16_t HASH_KEYWORD_MAX = 16244; const int16_t HASH_KEYWORD_MIN = 15978; const int16_t HASH_KEYWORD_LCN = 15137; const int16_t HASH_KEYWORD_RESET = 26133; +const int16_t HASH_KEYWORD_SPEED28 = -17064; +const int16_t HASH_KEYWORD_SPEED128 = 25816; int16_t DCCEXParser::stashP[MAX_COMMAND_PARAMS]; bool DCCEXParser::stashBusy; @@ -790,6 +792,16 @@ bool DCCEXParser::parseD(Print *stream, int16_t params, int16_t 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; }