diff --git a/bacnet-stack/ports/pic18f6720/dlmstp.c b/bacnet-stack/ports/pic18f6720/dlmstp.c
index 7b51a93f..60c09807 100644
--- a/bacnet-stack/ports/pic18f6720/dlmstp.c
+++ b/bacnet-stack/ports/pic18f6720/dlmstp.c
@@ -169,8 +169,6 @@ void dlmstp_task(void)
     if (MSTP_Port.receive_state == MSTP_RECEIVE_STATE_IDLE) {
         while (MSTP_Master_Node_FSM(&MSTP_Port)) {
         };
-        /*MSTP_Master_Node_FSM(&MSTP_Port);
- */
     }
     /* see if there is a packet available, and a place
        to put the reply (if necessary) and process it */
diff --git a/bacnet-stack/ports/pic18f6720/hardware.h b/bacnet-stack/ports/pic18f6720/hardware.h
index 755aaa30..cabbc27c 100644
--- a/bacnet-stack/ports/pic18f6720/hardware.h
+++ b/bacnet-stack/ports/pic18f6720/hardware.h
@@ -252,6 +252,15 @@ enum INT_STATE { INT_DISABLED, INT_ENABLED, INT_RESTORE };
                                   BRA LOOP           \
                                 _endasm }
 #endif
+
+#define setup_timer4(mode, period, postscale) \
+  T4CON = (mode | (postscale - 1) << 3); \
+  PR4 = period
+
+#define setup_timer2(mode, period, postscale) \
+  T2CON = (mode | (postscale - 1) << 3); \
+  PR2 = period
+
 
 /* Global Vars */
 extern volatile LED_REGS LEDS;
diff --git a/bacnet-stack/ports/pic18f6720/isr.c b/bacnet-stack/ports/pic18f6720/isr.c
index ebd18e04..cf8286d9 100644
--- a/bacnet-stack/ports/pic18f6720/isr.c
+++ b/bacnet-stack/ports/pic18f6720/isr.c
@@ -29,7 +29,6 @@
 #include "dlmstp.h"
 
 /* from main.c */
-extern volatile uint8_t System_Seconds;
 extern volatile uint8_t Milliseconds;
 
 void InterruptHandlerHigh(void);
@@ -42,17 +41,23 @@ void Interrupt_USART_Tx(void);
 void Interrupt_CCP2(void);
 void INT0_Interrupt(void);
 
-#pragma code InterruptVectorHigh = 0x308
+#pragma code InterruptVectorHigh = 0x08
 void InterruptVectorHigh(void)
 {
-    _asm goto InterruptHandlerHigh      /* jump to interrupt routine */
- _endasm}
+    /* jump to interrupt routine */
+    _asm goto InterruptHandlerHigh
+    _endasm
+}
+
 #pragma code
-#pragma code InterruptVectorLow = 0x318
+#pragma code InterruptVectorLow = 0x18
 void InterruptVectorLow(void)
-{
-    _asm goto InterruptHandlerLow       /* jump to interrupt routine */
- _endasm}
+{
+    /* jump to interrupt routine */
+    _asm goto InterruptHandlerLow       
+    _endasm
+}
+
 #pragma code
 #pragma interrupt InterruptHandlerHigh
 void InterruptHandlerHigh(void)
@@ -79,7 +84,6 @@ void InterruptHandlerHigh(void)
     /* check for timer0 int */
     if ((INTCONbits.TMR0IF) && (INTCONbits.TMR0IE)) {
         INTCONbits.TMR0IF = 0;
-        System_Seconds++;
     }
 }
 
diff --git a/bacnet-stack/ports/pic18f6720/main.c b/bacnet-stack/ports/pic18f6720/main.c
index e9c53cc5..6a6a3e1f 100644
--- a/bacnet-stack/ports/pic18f6720/main.c
+++ b/bacnet-stack/ports/pic18f6720/main.c
@@ -90,9 +90,8 @@
 #endif /* USE_ICD */
 
 volatile uint8_t Milliseconds = 0;
-volatile uint8_t System_Seconds = 0;
 volatile uint8_t Zero_Cross_Timeout = 0;
-
+
 static void BACnet_Service_Handlers_Init(void)
 {
     /* we need to handle who-is to support dynamic device binding */
@@ -149,6 +148,104 @@ void Global_Int(enum INT_STATE state)
         break;
     }
 }
+
+void Hardware_Initialize(void)
+{
+  /* PORTA.0 Input - Photocell PORTA.1 Output - LED Row6 PORTA.2 Output
+   * - LED Row5 PORTA.3 Output - LED Row4 PORTA.4 Input - Square Wave
+   * input from RTC PORTA.5 Output - LCD RW */
+  TRISA = 0xD1;
+
+  /* PORTB.0 Input - Zero Cross PORTB.1 Input - USB RXF# PORTB.2 Input
+   * USB TXE# PORTB.3 Output - Keypad Row Enable (74HC373 Output Control)
+   * PORTB.4 Output Keypad Row Gate (74HC373 Gate) PORTB.5 Output Switch
+   * Input Latch & Keypad Column Gate (74HC373 Gate) PORTB.6 Input - ICD
+   * connection PORTB.7 Input - ICD connection */
+  TRISB = 0xC7;
+
+  /* PORTC.0 Output - Pilot Latch PORTC.1 Output - Pilot Output Enable
+   * (low) PORTC.2 I/O - Piezo PORTC.3 Input - I2C clock PORTC.4 Input
+   * I2C data PORTC.5 Output RS232 enable (low) PORTC.6 Output - RS232 Tx
+   * PORTC.7 Input - RS232 Rx */
+  TRISC = 0x9C;
+
+  /* PORTD.0 I/O - Data bus PORTD.1 I/O - Data bus PORTD.2 I/O - Data
+   * bus PORTD.3 I/O - Data bus PORTD.4 I/O - Data bus PORTD.5 I/O - Data
+   * bus PORTD.6 I/O - Data bus PORTD.7 I/O - Data bus */
+  TRISD = 0xFF;
+
+  /* PORTE.0 Input - USB RD PORTE.1 Input - USB WR PORTE.2 Output - LCD
+   * RS PORTE.3 Output - 485 transmit enable PORTE.4 Output - Relay data
+   * latch PORTE.5 Output Switch Input Clock PORTE.6 Output - Switch
+   * Input High/Low PORTE.7 Input Switch Input Data */
+  TRISE = 0x83;
+
+  /* PORTF.0 Output - LED Row2 PORTF.1 Output - LED Row1 PORTF.2 Output
+   * - LED Col5 PORTF.3 Output - LED Col4 PORTF.4 Output - LED Col3
+   * PORTF.5 Output - LED Col2 PORTF.6 Output - LED Col1 PORTF.7 Output
+   * LED Col0 */
+  TRISF = 0x00;
+
+  /* PORTG.0 Output - 485 receive enable PORTG.1 Output - 485 Tx PORTG.2
+   * Input 485 Rx PORTG.3 Output - LCD E PORTG.4 Output - LED Row0 */
+  TRISG = 0xE6;
+
+  /* The initial state of the keypad enables and latches */
+  KEYPAD_ROW_ENABLE = 1;
+  KEYPAD_ROW_LATCH = 0;
+  KEYPAD_COL_LATCH = 1;
+
+  RELAY_LATCH = 0;
+
+  /* Setup to read the switch inputs */
+  SWITCH_COM = 1;
+
+  /* Enable the RS232 transmitter */
+  RS232_ENABLE = 0;
+
+  /* Turn all leds off. These are the hardware pins */
+  LED_ROW1 = 1;
+  LED_ROW2 = 1;
+  LED_ROW3 = 1;
+  LED_ROW4 = 1;
+  LED_ROW5 = 1;
+  LED_ROW6 = 1;
+  LEDPORT = 0x03;
+
+  /* The initial values for the signals to the LCD */
+  LCD_E = 1;
+  LCD_RW = 1;
+  LCD_RS = 1;
+
+  /* The following gives us a PWM frequency of 1.990KHz with a 50% duty
+   * cycle It also serves to multiplex the LEDs. */
+  PIEZO_OFF();
+  CCPR1L = 0x4E;
+  CCP1CON = 0x2F;
+  setup_timer2(6, 156, 2);
+  PIE1bits.TMR2IE = 1;
+
+  /* We will use Timer4 as our system tick timer. Our system tick is set
+   * to 1ms. Hold off on enabling the int. */
+  setup_timer4(5, 250, 5);
+
+  /* Setup our interrupt priorities */
+  RCONbits.IPEN = 1;
+  IPR1 = 0;
+  IPR2 = 0;
+  IPR3 = 0;
+
+  /* Setup TMR0 to be high priority */
+  INTCON2 = 0xFC;
+  INTCON3 = 0;
+
+  /* USART 1 high priority */
+  IPR1bits.RC1IP = 1;
+  IPR1bits.TX1IP = 1;
+
+  /* Finally enable our ints */
+  Global_Int(INT_ENABLED);
+}
 
 void Initialize_Variables(void)
 {
@@ -160,48 +257,28 @@ void Initialize_Variables(void)
     dlmstp_init();
     /* Start our time from now */
     Milliseconds = 0;
-    System_Seconds = 0;
-}
-
-void Verify_Ints(void)
-{
-    /* Make sure the Abus data and clock lines are inputs. Also make sure
-     * that global interrupts are enabled. */
-    if (!TRISCbits.TRISC3)
-        TRISCbits.TRISC3 = 1;
-    if (!TRISCbits.TRISC4)
-        TRISCbits.TRISC4 = 1;
-    if (!INTCONbits.GIE)
-        INTCONbits.GIE = 1;
-    if (!INTCONbits.PEIE)
-        INTCONbits.PEIE = 1;
-    /* if (!INTCONbits.INT0E) £
-     * INTCONbits.INT0E=1; */
-    if (!PIE1bits.TMR2IE)
-        PIE1bits.TMR2IE = 1;
 }
 
 void MainTasks(void)
-{
+{
+    static uint16_t millisecond_counter = 0;
     /* Handle our millisecond counters */
-    while (Milliseconds) {
+    while (Milliseconds) {
+        millisecond_counter++;
         --Milliseconds;
     }
     /* Handle our seconds counters */
-    while (System_Seconds) {
+    if (millisecond_counter > 1000) {
+        millisecond_counter -= 1000;
         dcc_timer_seconds(1);
-        System_Seconds--;
     }
 }
-
+
 void main(void)
-{
-    /* Note that before main is called, the SCL line is £
-     * toggled 256 times to clear any I2C devices that £
-     * may be holding the data line low £
-     * Reset POR bit */
+{
     RCONbits.NOT_POR = 1;
     RCONbits.NOT_RI = 1;
+    Hardware_Initialize();
     Initialize_Variables();
     /* Handle anything that needs to be done on powerup */
     /* Greet the BACnet world! */
@@ -209,8 +286,9 @@ void main(void)
     /* Main loop */
     while (TRUE) {
         RESTART_WDT();
-        Verify_Ints();
         dlmstp_task();
         MainTasks();
+        Global_Int(INT_ENABLED);
+        ENABLE_TIMER4_INT();
     }
 }