From e933172ef3128bc8db7604fc5c35ed63396f83f3 Mon Sep 17 00:00:00 2001
From: Asbelos <asbelos@btinternet.com>
Date: Thu, 4 Jun 2020 20:30:11 +0100
Subject: [PATCH] CPU handling

Technique adapted from David Cutting's solution
---
 Hardware.cpp | 17 +++++++++++++----
 1 file changed, 13 insertions(+), 4 deletions(-)

diff --git a/Hardware.cpp b/Hardware.cpp
index bca0e66..6c5932c 100644
--- a/Hardware.cpp
+++ b/Hardware.cpp
@@ -6,6 +6,13 @@
 #include "Config.h"
 #include "DIAG.h"
 
+#if defined(ARDUINO_ARCH_AVR)
+    #include <DIO2.h>
+    #define WritePin digitalWrite2
+#else
+    #define WritePin digitalWrite
+#endif
+    
 void Hardware::init() {
   pinMode(MAIN_POWER_PIN, OUTPUT);
   pinMode(MAIN_SIGNAL_PIN, OUTPUT);
@@ -19,17 +26,17 @@ void Hardware::init() {
 }
 
 void Hardware::setPower(bool isMainTrack, bool on) {
-  digitalWrite(isMainTrack ? MAIN_POWER_PIN : PROG_POWER_PIN, on ? HIGH : LOW);
+  WritePin(isMainTrack ? MAIN_POWER_PIN : PROG_POWER_PIN, on ? HIGH : LOW);
 }
 void Hardware::setBrake(bool isMainTrack, bool on) {
-  digitalWrite(isMainTrack ? MAIN_BRAKE_PIN : PROG_BRAKE_PIN, on ? LOW:HIGH);
+  WritePin(isMainTrack ? MAIN_BRAKE_PIN : PROG_BRAKE_PIN, on ? LOW:HIGH);
 }
 
 void Hardware::setSignal(bool isMainTrack, bool high) {
   byte pin = isMainTrack ? MAIN_SIGNAL_PIN : PROG_SIGNAL_PIN;
   byte pin2 = isMainTrack ? MAIN_SIGNAL_PIN_ALT : PROG_SIGNAL_PIN_ALT;
-  digitalWrite(pin, high ? HIGH : LOW);
-  if (pin2) digitalWrite(pin2, high ? LOW : HIGH);
+  WritePin(pin, high ? HIGH : LOW);
+  if (pin2) WritePin(pin2, high ? LOW : HIGH);
 }
 
 int Hardware::getCurrentMilliamps(bool isMainTrack) {
@@ -46,12 +53,14 @@ void Hardware::setCallback(int duration, void (*isr)()) {
   Timer3.disablePwm(TIMER3_B_PIN);
   Timer3.attachInterrupt(isr);
 }
+
 void Hardware::setSingleCallback(int duration, void (*isr)()) {
   Timer1.initialize(duration);
   Timer1.disablePwm(TIMER1_A_PIN);
   Timer1.disablePwm(TIMER1_B_PIN);
   Timer1.attachInterrupt(isr);
 }
+
 void Hardware::resetSingleCallback(int duration) {
   if (duration==0) Timer1.stop();
   else Timer1.initialize(duration);