From a5b73c823a0d79e786ebc530ebc7c238b5d60373 Mon Sep 17 00:00:00 2001
From: Colin Murdoch <colin.murdoch@btinternet.com>
Date: Sat, 20 Jan 2024 18:09:03 +0000
Subject: [PATCH] Added SETFREQ command

Added SETFREQ command to EXRAIL
---
 DCCTimerAVR.cpp     |  2 +-
 EXRAIL2.cpp         | 37 +++++++++++++++++++++++++++++++++++++
 EXRAIL2.h           |  2 +-
 EXRAIL2MacroReset.h |  2 ++
 EXRAILMacros.h      |  1 +
 5 files changed, 42 insertions(+), 2 deletions(-)

diff --git a/DCCTimerAVR.cpp b/DCCTimerAVR.cpp
index e0fe941..5c361a0 100644
--- a/DCCTimerAVR.cpp
+++ b/DCCTimerAVR.cpp
@@ -157,7 +157,7 @@ void DCCTimer::DCCEXanalogWriteFrequencyInternal(uint8_t pin, uint32_t fbits) {
 
     TCCR2A = (TCCR2A & B11111100) | abits; // set WGM0 and WGM1
     TCCR2B = (TCCR2B & B11110000) | bbits; // set WGM2 and 3 bits of prescaler
-    //DIAG(F("Timer 2 A=%x B=%x"), TCCR2A, TCCR2B);
+    DIAG(F("Timer 2 A=%x B=%x"), TCCR2A, TCCR2B);
 
   } else { // not timer 9 or 10
     abits = B01;
diff --git a/EXRAIL2.cpp b/EXRAIL2.cpp
index cc3269b..de254a6 100644
--- a/EXRAIL2.cpp
+++ b/EXRAIL2.cpp
@@ -667,6 +667,43 @@ void RMFT2::loop2() {
       }
       break; 
 
+  case OPCODE_SETFREQ:
+      // Frequency is default 0, or 1, 2,3
+      //if (loco) DCC::setFn(loco,operand,true);
+      switch (operand) {
+        case 0:  // default - all F-s off
+            if (loco) {
+                DCC::setFn(loco,29,false);
+                DCC::setFn(loco,30,false);
+                DCC::setFn(loco,31,false);
+            }
+        break;
+        case 1:
+            //if (loco) DCC::setFn(loco,29,true);
+            if (loco) {
+              DCC::setFn(loco,30,true);
+              DCC::setFn(loco,31,false);
+            }
+        break;
+        case 2:
+            //if (loco) DCC::setFn(loco,30,true);
+            if (loco) {
+              DCC::setFn(loco,30,false);
+              DCC::setFn(loco,31,true);
+            }
+        break;
+        case 3:
+            //if (loco) DCC::setFn(loco,31,true);
+            if (loco) {
+              DCC::setFn(loco,30,true);
+              DCC::setFn(loco,31,true);
+            }
+        break;
+
+      }
+
+      break;
+
   case OPCODE_RESUME:
     pausingTask=NULL;
     driveLoco(speedo);
diff --git a/EXRAIL2.h b/EXRAIL2.h
index 30a2f45..63d20e7 100644
--- a/EXRAIL2.h
+++ b/EXRAIL2.h
@@ -51,7 +51,7 @@ enum OPCODE : byte {OPCODE_THROW,OPCODE_CLOSE,
              OPCODE_JOIN,OPCODE_UNJOIN,OPCODE_READ_LOCO1,OPCODE_READ_LOCO2,
 #endif
              OPCODE_POM,
-             OPCODE_START,OPCODE_SETLOCO,OPCODE_SENDLOCO,OPCODE_FORGET,
+             OPCODE_START,OPCODE_SETLOCO,OPCODE_SETFREQ,OPCODE_SENDLOCO,OPCODE_FORGET,
              OPCODE_PAUSE, OPCODE_RESUME,OPCODE_POWEROFF,OPCODE_POWERON,
              OPCODE_ONCLOSE, OPCODE_ONTHROW, OPCODE_SERVOTURNOUT, OPCODE_PINTURNOUT,
              OPCODE_PRINT,OPCODE_DCCACTIVATE,
diff --git a/EXRAIL2MacroReset.h b/EXRAIL2MacroReset.h
index 7811a0d..8ab854d 100644
--- a/EXRAIL2MacroReset.h
+++ b/EXRAIL2MacroReset.h
@@ -151,6 +151,7 @@
 #undef SET_TRACK
 #undef SET_POWER
 #undef SETLOCO 
+#undef SETFREQ
 #undef SIGNAL 
 #undef SIGNALH 
 #undef SPEED 
@@ -302,6 +303,7 @@
 #define SET_TRACK(track,mode)
 #define SET_POWER(track,onoff)
 #define SETLOCO(loco) 
+#define SETFREQ(loco,freq)
 #define SIGNAL(redpin,amberpin,greenpin) 
 #define SIGNALH(redpin,amberpin,greenpin) 
 #define SPEED(speed) 
diff --git a/EXRAILMacros.h b/EXRAILMacros.h
index f79693d..93ea911 100644
--- a/EXRAILMacros.h
+++ b/EXRAILMacros.h
@@ -493,6 +493,7 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
 #define SET_TRACK(track,mode)  OPCODE_SET_TRACK,V(TRACK_MODE_##mode  <<8 | TRACK_NUMBER_##track),
 #define SET_POWER(track,onoff) OPCODE_SET_POWER,V(TRACK_POWER_##onoff),OPCODE_PAD, V(TRACK_NUMBER_##track),
 #define SETLOCO(loco) OPCODE_SETLOCO,V(loco),
+#define SETFREQ(loco,freq) OPCODE_SETLOCO,V(loco), OPCODE_SETFREQ,V(freq),
 #define SIGNAL(redpin,amberpin,greenpin) 
 #define SIGNALH(redpin,amberpin,greenpin) 
 #define SPEED(speed) OPCODE_SPEED,V(speed),