PWM fixes, slow down fan update

include LPC1768 syntax for M42

couple more pin_t changes

consistency

change M42 to R, P format

Revert "change M42 to R, P format"

This reverts commit 01f12f579e.

Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.cpp

@@ -29,38 +29,49 @@
 /**
  * This is a hybrid system.
  *
- * The PWM1 module is used to directly control the Servo 0, 1 & 3 pins.  This keeps
+ * The PWM1 module is used to directly control the Servo 0, 1 & 3 pins and D9 & D10 pins.  This keeps
  * the pulse width jitter to under a microsecond.
  *
  * For all other pins the PWM1 module is used to generate interrupts.  The ISR
  * routine does the actual setting/clearing of pins.  The upside is that any pin can
  * have a PWM channel assigned to it.  The downside is that there is more pulse width
  * jitter. The jitter depends on what else is happening in the system and what ISRs
- * prempt the PWM ISR.  Writing to the SD card can add 20 microseconds to the pulse
- * width.
+ * pre-empt the PWM ISR.
  */
 
 /**
- * The data structures are setup to minimize the computation done by the ISR which
- * minimizes ISR execution time.  Execution times are 2.2 - 3.7 microseconds.
+ * The data structures are set up to minimize the computation done by the ISR which
+ * minimizes ISR execution time.  Execution times are 2-4µs except when updating to
+ * a new value when they are 19µs.
  *
- * Two tables are used.  active_table is used by the ISR.  Changes to the table are
- * are done by copying the active_table into the work_table, updating the work_table
- * and then swapping the two tables.  Swapping is done by manipulating pointers.
+ * Two tables are used.  One table contains the data used by the ISR to update/control
+ * the PWM pins.  The other is used as an aid when rebuilding the ISR table.
  *
- * Immediately after the swap the ISR uses the work_table until the start of the
- * next 20mS cycle. During this transition the "work_table" is actually the table
- * that was being used before the swap.  The "active_table" contains the data that
- * will start being used at the start of the next 20mS period.  This keeps the pins
- * well behaved during the transition.
+ * The LPC1768_PWM_attach_pin routine disables the ISR and then adds the new info to
+ * ISR table.  It can update the table directly because none of its changes affect
+ * what the ISR does.
  *
- * The ISR's priority is set to the maximum otherwise other ISRs can cause considerable
- * jitter in the PWM high time.
+ * LPC1768_PWM_detach_pin routine disables the ISR, disables the pin immediately if
+ * it's a directly controlled pin and updates the helper table.  It then flags the
+ * ISR that the ISR table needs to be rebuilt.
+ *
+ * LPC1768_PWM_write routine disables the ISR and updates the helper table.  It then
+ * flags the ISR that the ISR table needs to be rebuilt.
+ *
+ * The ISR's priority is set to less than the stepper ISR otherwise it could cause jitter
+ * in the step pulses.
  *
  * See the end of this file for details on the hardware/firmware interaction
  */
 
 #ifdef TARGET_LPC1768
+
+#include "../../inc/MarlinConfig.h"
+
+// #include <math.h>
+// #include <stdio.h>
+// #include <stdlib.h>
+
 #include <lpc17xx_pinsel.h>
 #include "LPC1768_PWM.h"
 #include "arduino.h"
@@ -68,48 +79,68 @@
 #define NUM_PWMS 6
 
 typedef struct {            // holds all data needed to control/init one of the PWM channels
-    uint8_t             sequence;       // 0: available slot, 1 - 6: PWM channel assigned to that slot
-    pin_t               pin;
-    uint16_t            PWM_mask;       // MASK TO CHECK/WRITE THE IR REGISTER
-    volatile uint32_t*  set_register;
-    volatile uint32_t*  clr_register;
-    uint32_t            write_mask;     // USED BY SET/CLEAR COMMANDS
-    uint32_t            microseconds;   // value written to MR register
-    uint32_t            min;            // lower value limit checked by WRITE routine before writing to the MR register
-    uint32_t            max;            // upper value limit checked by WRITE routine before writing to the MR register
-    bool                PWM_flag;       // 0 - USED BY sERVO, 1 - USED BY ANALOGWRITE
-    uint8_t             servo_index;    // 0 - MAX_SERVO -1 : servo index,  0xFF : PWM channel
-    bool                active_flag;    // THIS TABLE ENTRY IS ACTIVELY TOGGLING A PIN
-    uint8_t             assigned_MR;    // Which MR (1-6) is used by this logical channel
-    uint32_t            PCR_bit;        // PCR register bit to enable PWM1 control of this pin
-    uint32_t            PINSEL3_bits;   // PINSEL3 register bits to set pin mode to PWM1 control
+  uint8_t             sequence;       // 0: available slot, 1 - 6: PWM channel assigned to that slot
+  pin_t               pin;
+  uint16_t            PWM_mask;       // MASK TO CHECK/WRITE THE IR REGISTER
+  volatile uint32_t*  set_register;
+  volatile uint32_t*  clr_register;
+  uint32_t            write_mask;     // USED BY SET/CLEAR COMMANDS
+  uint32_t            microseconds;   // value written to MR register
+  uint32_t            min;            // lower value limit checked by WRITE routine before writing to the MR register
+  uint32_t            max;            // upper value limit checked by WRITE routine before writing to the MR register
+  bool                PWM_flag;       // 0 - USED BY hardware PWM, 1 - USED BY ANALOGWRITE
+  uint8_t             servo_index;    // 0 - MAX_SERVO -1 : servo index,  0xFF : PWM channel
+  bool                active_flag;    // THIS TABLE ENTRY IS ACTIVELY TOGGLING A PIN
+  uint32_t            PCR_bit;        // PCR register bit to enable PWM1 control of this pin
+  volatile uint32_t*  PINSEL_reg;     // PINSEL register
+  uint32_t            PINSEL_bits;    // PINSEL register bits to set pin mode to PWM1 control
 
 } PWM_map;
 
 
-#define MICRO_MAX 0xffffffff
+#define MICRO_MAX 0xFFFFFFFF
 
-#define PWM_MAP_INIT_ROW {0, P_NC, 0, 0, 0, 0, MICRO_MAX, 0, 0, 0, 0, 0, 0, 0, 0}
-#define PWM_MAP_INIT {PWM_MAP_INIT_ROW,\
-                      PWM_MAP_INIT_ROW,\
-                      PWM_MAP_INIT_ROW,\
-                      PWM_MAP_INIT_ROW,\
-                      PWM_MAP_INIT_ROW,\
-                      PWM_MAP_INIT_ROW,\
+#define PWM_MAP_INIT_ROW { 0, 0x7FFF, 0, 0, 0, 0, MICRO_MAX, 0, 0, 0, 0, 0, 0, 0, 0 }
+#define PWM_MAP_INIT { PWM_MAP_INIT_ROW, PWM_MAP_INIT_ROW, PWM_MAP_INIT_ROW, \
+                       PWM_MAP_INIT_ROW, PWM_MAP_INIT_ROW, PWM_MAP_INIT_ROW, \
                      };
 
-PWM_map PWM1_map_A[NUM_PWMS] = PWM_MAP_INIT;
-PWM_map PWM1_map_B[NUM_PWMS] = PWM_MAP_INIT;
+PWM_map ISR_table[NUM_PWMS] = PWM_MAP_INIT;
 
-PWM_map *active_table = PWM1_map_A;
-PWM_map *work_table = PWM1_map_B;
-PWM_map *ISR_table;
+#define IR_BIT(p) ((p) >= 0 && (p) <= 3 ? (p) : p + 4 )
+#define PIN_IS_INVERTED(p) 0  // placeholder in case inverting PWM output is offered
 
 
-#define IR_BIT(p) (p >= 0 && p <= 3 ? p : p + 4 )
-#define COPY_ACTIVE_TABLE    for (uint8_t i = 0; i < 6 ; i++) work_table[i] = active_table[i]
-#define PIN_IS_INVERTED(p) 0  // place holder in case inverting PWM output is offered
 
+#define P1_18_PWM_channel  1  // servo 3
+#define P1_20_PWM_channel  2  // servo 0
+#define P1_21_PWM_channel  3  // servo 1
+#define P2_4_PWM_channel   5  // D9
+#define P2_5_PWM_channel   6  // D10
+
+// used to keep track of which Match Registers have been used and if they will be used by the
+// PWM1 module to directly control the pin or will be used to generate an interrupt
+typedef struct {                    // status of PWM1 channel
+  uint8_t             map_used;     // 0 - this MR register not used/assigned
+  uint8_t             map_PWM_INT;  // 0 - available for interrupts, 1 - in use by PWM
+  pin_t               map_PWM_PIN;  // pin for this PwM1 controlled pin / port
+  volatile uint32_t*  MR_register;  // address of the MR register for this PWM1 channel
+  uint32_t            PCR_bit;      // PCR register bit to enable PWM1 control of this pin
+                                    // 0 - don't switch to PWM1 direct control
+  volatile uint32_t*  PINSEL_reg;   // PINSEL register
+  uint32_t            PINSEL_bits;  // PINSEL register bits to set pin mode to PWM1 control
+} MR_map;
+
+MR_map map_MR[NUM_PWMS];
+
+void LPC1768_PWM_update_map_MR(void) {
+  map_MR[0] = { 0, (uint8_t) (LPC_PWM1->PCR & _BV(8 + P1_18_PWM_channel) ? 1 : 0), P1_18, &LPC_PWM1->MR1, 0, 0, 0 };
+  map_MR[1] = { 0, (uint8_t) (LPC_PWM1->PCR & _BV(8 + P1_20_PWM_channel) ? 1 : 0), P1_20, &LPC_PWM1->MR2, 0, 0, 0 };
+  map_MR[2] = { 0, (uint8_t) (LPC_PWM1->PCR & _BV(8 + P1_21_PWM_channel) ? 1 : 0), P1_21, &LPC_PWM1->MR3, 0, 0, 0 };
+  map_MR[3] = { 0, 0, P_NC, &LPC_PWM1->MR4, 0, 0, 0 };
+  map_MR[4] = { 0, (uint8_t) (LPC_PWM1->PCR & _BV(8 + P2_4_PWM_channel) ? 1 : 0), P2_4, &LPC_PWM1->MR5, 0, 0, 0 };
+  map_MR[5] = { 0, (uint8_t) (LPC_PWM1->PCR & _BV(8 + P2_5_PWM_channel) ? 1 : 0), P2_5, &LPC_PWM1->MR6, 0, 0, 0 };
+}
 
 /**
  *  Prescale register and MR0 register values
@@ -144,317 +175,322 @@ PWM_map *ISR_table;
  *
  */
 
+bool ISR_table_update = false;  // flag to tell the ISR that the tables need to be updated & swapped
 
 void LPC1768_PWM_init(void) {
-  #define SBIT_CNTEN     0  // PWM1 counter & pre-scaler enable/disable
-  #define SBIT_CNTRST    1  // reset counters to known state
-  #define SBIT_PWMEN     3  // 1 - PWM, 0 - timer
-  #define SBIT_PWMMR0R   1
-  #define PCPWM1         6
+  #define SBIT_CNTEN      0  // PWM1 counter & pre-scaler enable/disable
+  #define SBIT_CNTRST     1  // reset counters to known state
+  #define SBIT_PWMEN      3  // 1 - PWM, 0 - timer
+  #define SBIT_PWMMR0R    1
+  #define PCPWM1          6
   #define PCLK_PWM1      12
 
-  LPC_SC->PCONP |= (1 << PCPWM1);      // enable PWM1 controller (enabled on power up)
+  SBI(LPC_SC->PCONP, PCPWM1);                                             // Enable PWM1 controller (enabled on power up)
   LPC_SC->PCLKSEL0 &= ~(0x3 << PCLK_PWM1);
   LPC_SC->PCLKSEL0 |= (LPC_PWM1_PCLKSEL0 << PCLK_PWM1);
-  LPC_PWM1->MR0 = LPC_PWM1_MR0;                // TC resets every 19,999 + 1 cycles - sets PWM cycle(Ton+Toff) to 20 mS
-                                        // MR0 must be set before TCR enables the PWM
-  LPC_PWM1->TCR = _BV(SBIT_CNTEN) | _BV(SBIT_CNTRST)| _BV(SBIT_PWMEN);;  // enable counters, reset counters, set mode to PWM
-  LPC_PWM1->TCR &= ~(_BV(SBIT_CNTRST));  // take counters out of reset
-  LPC_PWM1->PR  =  LPC_PWM1_PR;
-  LPC_PWM1->MCR = (_BV(SBIT_PWMMR0R) | _BV(0));     // Reset TC if it matches MR0, disable all interrupts except for MR0
-  LPC_PWM1->CTCR = 0;                   // disable counter mode (enable PWM mode)
-
-  LPC_PWM1->LER = 0x07F;  // Set the latch Enable Bits to load the new Match Values for MR0 - MR6
-  // Set all PWMs to single edge
-  LPC_PWM1->PCR = 0;    // single edge mode for all channels, PWM1 control of outputs off
-
-  NVIC_EnableIRQ(PWM1_IRQn);     // Enable interrupt handler
-  //      NVIC_SetPriority(PWM1_IRQn, NVIC_EncodePriority(0, 10, 0));  // normal priority for PWM module
-  NVIC_SetPriority(PWM1_IRQn, NVIC_EncodePriority(0, 0, 0));  // minimizes jitter due to higher priority ISRs
+
+  LPC_PWM1->MR0  = LPC_PWM1_MR0;                                          // TC resets every 19,999 + 1 cycles - sets PWM cycle(Ton+Toff) to 20 mS
+  // MR0 must be set before TCR enables the PWM
+  LPC_PWM1->TCR  = _BV(SBIT_CNTEN) | _BV(SBIT_CNTRST) | _BV(SBIT_PWMEN);  // Enable counters, reset counters, set mode to PWM
+  LPC_PWM1->TCR  &= ~(_BV(SBIT_CNTRST));                                  // Take counters out of reset
+  LPC_PWM1->PR   =  LPC_PWM1_PR;
+  LPC_PWM1->MCR  = _BV(SBIT_PWMMR0R) | _BV(0);                            // Reset TC if it matches MR0, disable all interrupts except for MR0
+  LPC_PWM1->CTCR = 0;                                                     // Disable counter mode (enable PWM mode)
+  LPC_PWM1->LER  = 0x07F;                                                 // Set the latch Enable Bits to load the new Match Values for MR0 - MR6
+  LPC_PWM1->PCR  = 0;                                                     // Single edge mode for all channels, PWM1 control of outputs off
+
+  NVIC_EnableIRQ(PWM1_IRQn);                                              // Enable interrupt handler
+  NVIC_SetPriority(PWM1_IRQn, NVIC_EncodePriority(0, 10, 0));             // Normal priority for PWM module
+  //NVIC_SetPriority(PWM1_IRQn, NVIC_EncodePriority(0, 0, 0));            // Minimizes jitter due to higher priority ISRs
 }
 
 
-bool PWM_table_swap = false;  // flag to tell the ISR that the tables have been swapped
-bool PWM_MR0_wait = false;  // flag to ensure don't delay MR0 interrupt
+bool LPC1768_PWM_attach_pin(pin_t pin, uint32_t min /* = 1 */, uint32_t max /* = (LPC_PWM1_MR0 - MR0_MARGIN) */, uint8_t servo_index /* = 0xff */) {
 
+  pin = GET_PIN_MAP_PIN(GET_PIN_MAP_INDEX(pin & 0xFF));  // Sometimes the upper byte is garbled
 
-bool LPC1768_PWM_attach_pin(pin_t pin, uint32_t min /* = 1 */, uint32_t max /* = (LPC_PWM1_MR0 - MR0_MARGIN) */, uint8_t servo_index /* = 0xff */) {
-  while (PWM_table_swap) delay(5);  // don't do anything until the previous change has been implemented by the ISR
-  COPY_ACTIVE_TABLE;  // copy active table into work table
+  NVIC_DisableIRQ(PWM1_IRQn);    // make it safe to update the active table
+                                 // OK to update the active table because the
+                                 // ISR doesn't use any of the changed items
   uint8_t slot = 0;
   for (uint8_t i = 0; i < NUM_PWMS ; i++)         // see if already in table
-    if (work_table[i].pin == pin) return 1;
+    if (ISR_table[i].pin == pin) {
+      NVIC_EnableIRQ(PWM1_IRQn);  // re-enable PWM interrupts
+      return 1;
+    }
 
   for (uint8_t i = 1; (i < NUM_PWMS + 1) && !slot; i++)         // find empty slot
-    if ( !(work_table[i - 1].set_register)) slot = i;  // any item that can't be zero when active or just attached is OK
+    if ( !(ISR_table[i - 1].set_register)) { slot = i; break; }  // any item that can't be zero when active or just attached is OK
   if (!slot) return 0;
   slot--;  // turn it into array index
 
-  work_table[slot].pin = pin;     // init slot
-  work_table[slot].PWM_mask = 0;  // real value set by PWM_write
-  work_table[slot].set_register = PIN_IS_INVERTED(pin) ? &LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOCLR : &LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOSET;
-  work_table[slot].clr_register = PIN_IS_INVERTED(pin) ? &LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOSET : &LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOCLR;
-  work_table[slot].write_mask = LPC_PIN(LPC1768_PIN_PIN(pin));
-  work_table[slot].microseconds = MICRO_MAX;
-  work_table[slot].min = min;
-  work_table[slot].max = MIN(max, LPC_PWM1_MR0 - MR0_MARGIN);
-  work_table[slot].servo_index = servo_index;
-  work_table[slot].active_flag = false;
-
-  //swap tables
-  PWM_MR0_wait = true;
-  while (PWM_MR0_wait) delay(5);  //wait until MR0 interrupt has happend so don't delay it.
+  ISR_table[slot].pin          = pin;     // init slot
+  ISR_table[slot].PWM_mask     = 0;  // real value set by PWM_write
+  ISR_table[slot].set_register = PIN_IS_INVERTED(pin) ? &LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOCLR : &LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOSET;
+  ISR_table[slot].clr_register = PIN_IS_INVERTED(pin) ? &LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOSET : &LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOCLR;
+  ISR_table[slot].write_mask   = LPC_PIN(LPC1768_PIN_PIN(pin));
+  ISR_table[slot].microseconds = MICRO_MAX;
+  ISR_table[slot].min          = min;
+  ISR_table[slot].max          = MIN(max, LPC_PWM1_MR0 - MR0_MARGIN);
+  ISR_table[slot].servo_index  = servo_index;
+  ISR_table[slot].active_flag  = false;
 
-  NVIC_DisableIRQ(PWM1_IRQn);
-  PWM_map *pointer_swap = active_table;
-  active_table = work_table;
-  work_table = pointer_swap;
-  PWM_table_swap = true;  // tell the ISR that the tables have been swapped
   NVIC_EnableIRQ(PWM1_IRQn);  // re-enable PWM interrupts
 
   return 1;
 }
 
-#define pin_11_PWM_channel 2
-#define pin_6_PWM_channel  3
-#define pin_4_PWM_channel  1
 
-// used to keep track of which Match Registers have been used and if they will be used by the
-// PWM1 module to directly control the pin or will be used to generate an interrupt
-typedef struct {                        // status of PWM1 channel
-    uint8_t map_used;                   // 0 - this MR register not used/assigned
-    uint8_t map_PWM_INT;                // 0 - available for interrupts, 1 - in use by PWM
-    pin_t map_PWM_PIN;                  // pin for this PwM1 controlled pin / port
-    volatile uint32_t* MR_register;     // address of the MR register for this PWM1 channel
-    uint32_t PCR_bit;                   // PCR register bit to enable PWM1 control of this pin
-    uint32_t PINSEL3_bits;              // PINSEL3 register bits to set pin mode to PWM1 control
-} MR_map;
+bool LPC1768_PWM_detach_pin(pin_t pin) {
 
-MR_map map_MR[NUM_PWMS];
+  pin = GET_PIN_MAP_PIN(GET_PIN_MAP_INDEX(pin & 0xFF));
 
-void LPC1768_PWM_update_map_MR(void) {
-  map_MR[0] = {0, (uint8_t) (LPC_PWM1->PCR & _BV(8 + pin_4_PWM_channel)  ? 1 : 0), P1_18, &LPC_PWM1->MR1, 0, 0};
-  map_MR[1] = {0, (uint8_t) (LPC_PWM1->PCR & _BV(8 + pin_11_PWM_channel) ? 1 : 0), P1_20, &LPC_PWM1->MR2, 0, 0};
-  map_MR[2] = {0, (uint8_t) (LPC_PWM1->PCR & _BV(8 + pin_6_PWM_channel)  ? 1 : 0), P1_21, &LPC_PWM1->MR3, 0, 0};
-  map_MR[3] = {0, 0, P_NC, &LPC_PWM1->MR4, 0, 0};
-  map_MR[4] = {0, 0, P_NC, &LPC_PWM1->MR5, 0, 0};
-  map_MR[5] = {0, 0, P_NC, &LPC_PWM1->MR6, 0, 0};
-}
+  NVIC_EnableIRQ(PWM1_IRQn);   // ?? fixes compiler problem??  ISR won't start
+                               // unless put in an extra "enable"
+  NVIC_DisableIRQ(PWM1_IRQn);
 
+  uint8_t slot = 0xFF;
+  for (uint8_t i = 0; i < NUM_PWMS; i++)         // find slot
+    if (ISR_table[i].pin == pin) { slot = i; break; }
+  if (slot == 0xFF) return false;    // return error if pin not found
 
-uint32_t LPC1768_PWM_interrupt_mask = 1;
+  LPC1768_PWM_update_map_MR();
 
-void LPC1768_PWM_update(void) {
-  for (uint8_t i = NUM_PWMS; --i;) {  // (bubble) sort table by microseconds
-    bool didSwap = false;
-    PWM_map temp;
-    for (uint16_t j = 0; j < i; ++j) {
-      if (work_table[j].microseconds > work_table[j + 1].microseconds) {
-        temp = work_table[j + 1];
-        work_table[j + 1] = work_table[j];
-        work_table[j] = temp;
-        didSwap = true;
-      }
-    }
-    if (!didSwap) break;
+  // OK to make these changes before the MR0 interrupt
+  switch(pin) {
+    case P1_20:                        // Servo 0, PWM1 channel 2  (Pin 11  P1.20 PWM1.2)
+      LPC_PWM1->PCR &= ~(_BV(8 + P1_20_PWM_channel));                 // disable PWM1 module control of this pin
+      map_MR[P1_20_PWM_channel - 1].PCR_bit = 0;
+      LPC_PINCON->PINSEL3 &= ~(0x3 <<  8);    // return pin to general purpose I/O
+      map_MR[P1_20_PWM_channel - 1].PINSEL_bits = 0;
+      map_MR[P1_20_PWM_channel - 1].map_PWM_INT = 0;               // 0 - available for interrupts, 1 - in use by PWM
+      break;
+    case P1_21:                        // Servo 1, PWM1 channel 3  (Pin 6  P1.21 PWM1.3)
+      LPC_PWM1->PCR &= ~(_BV(8 + P1_21_PWM_channel));                  // disable PWM1 module control of this pin
+      map_MR[P1_21_PWM_channel - 1].PCR_bit = 0;
+      LPC_PINCON->PINSEL3 &= ~(0x3 << 10);  // return pin to general purpose I/O
+      map_MR[P1_21_PWM_channel - 1].PINSEL_bits = 0;
+      map_MR[P1_21_PWM_channel - 1].map_PWM_INT = 0;                // 0 - available for interrupts, 1 - in use by PWM
+      break;
+    case P1_18:                        // Servo 3, PWM1 channel 1 (Pin 4  P1.18 PWM1.1)
+      LPC_PWM1->PCR &= ~(_BV(8 + P1_18_PWM_channel));                  // disable PWM1 module control of this pin
+      map_MR[P1_18_PWM_channel - 1].PCR_bit =  0;
+      LPC_PINCON->PINSEL3 &= ~(0x3 <<  4);  // return pin to general purpose I/O
+      map_MR[P1_18_PWM_channel - 1].PINSEL_bits =  0;
+      map_MR[P1_18_PWM_channel - 1].map_PWM_INT = 0;                // 0 - available for interrupts, 1 - in use by PWM
+      break;
+    case P2_4:                        // D9 FET, PWM1 channel 5  (Pin 9  P2_4 PWM1.5)
+      LPC_PWM1->PCR &= ~(_BV(8 + P2_4_PWM_channel));                  // disable PWM1 module control of this pin
+      map_MR[P2_4_PWM_channel - 1].PCR_bit = 0;
+      LPC_PINCON->PINSEL4 &= ~(0x3 << 10);  // return pin to general purpose I/O
+      map_MR[P2_4_PWM_channel - 1].PINSEL_bits = 0;
+      map_MR[P2_4_PWM_channel - 1].map_PWM_INT = 0;                // 0 - available for interrupts, 1 - in use by PWM
+      break;
+    case P2_5:                        // D10 FET, PWM1 channel 6 (Pin 10  P2_5 PWM1.6)
+      LPC_PWM1->PCR &= ~(_BV(8 + P2_5_PWM_channel));                  // disable PWM1 module control of this pin
+      map_MR[P2_5_PWM_channel - 1].PCR_bit =  0;
+      LPC_PINCON->PINSEL4 &= ~(0x3 <<  4);  // return pin to general purpose I/O
+      map_MR[P2_5_PWM_channel - 1].PINSEL_bits =  0;
+      map_MR[P2_5_PWM_channel - 1].map_PWM_INT = 0;                // 0 - available for interrupts, 1 - in use by PWM
+      break;
+    default:
+      break;
   }
 
-  LPC1768_PWM_interrupt_mask = 0;                          // set match registers to new values, build IRQ mask
-  for (uint8_t i = 0; i < NUM_PWMS; i++) {
-    if (work_table[i].active_flag == true) {
-      work_table[i].sequence = i + 1;
+  ISR_table[slot] = PWM_MAP_INIT_ROW;
 
-      // first see if there is a PWM1 controlled pin for this entry
-      bool found = false;
-      for (uint8_t j = 0; (j < NUM_PWMS) && !found; j++) {
-        if ( (map_MR[j].map_PWM_PIN == work_table[i].pin) && map_MR[j].map_PWM_INT ) {
-          *map_MR[j].MR_register = work_table[i].microseconds;  // found one of the PWM pins
-          work_table[i].PWM_mask = 0;
-          work_table[i].PCR_bit = map_MR[j].PCR_bit;            // PCR register bit to enable PWM1 control of this pin
-          work_table[i].PINSEL3_bits = map_MR[j].PINSEL3_bits;  // PINSEL3 register bits to set pin mode to PWM1 control} MR_map;
-          map_MR[j].map_used = 2;
-          work_table[i].assigned_MR = j +1;                    // only used to help in debugging
-          found = true;
-        }
-      }
+  ISR_table_update = true;
+  NVIC_EnableIRQ(PWM1_IRQn);  // re-enable PWM interrupts
 
-      // didn't find a PWM1 pin so get an interrupt
-      for (uint8_t k = 0; (k < NUM_PWMS) && !found; k++) {
-        if ( !(map_MR[k].map_PWM_INT || map_MR[k].map_used)) {
-          *map_MR[k].MR_register = work_table[i].microseconds;  // found one for an interrupt pin
-          map_MR[k].map_used = 1;
-          LPC1768_PWM_interrupt_mask |= _BV(3 * (k + 1));  // set bit in the MCR to enable this MR to generate an interrupt
-          work_table[i].PWM_mask = _BV(IR_BIT(k + 1));  // bit in the IR that will go active when this MR generates an interrupt
-          work_table[i].assigned_MR = k +1;                // only used to help in debugging
-          found = true;
-        }
-      }
-    }
-    else
-      work_table[i].sequence = 0;
-  }
-  LPC1768_PWM_interrupt_mask |= (uint32_t) _BV(0);  // add in MR0 interrupt
+  return 1;
+}
 
-   // swap tables
 
-  PWM_MR0_wait = true;
-  while (PWM_MR0_wait) delay(5);  //wait until MR0 interrupt has happend so don't delay it.
+bool LPC1768_PWM_write(pin_t pin, uint32_t value) {
 
-  NVIC_DisableIRQ(PWM1_IRQn);
-  LPC_PWM1->LER = 0x07E;  // Set the latch Enable Bits to load the new Match Values for MR1 - MR6
-  PWM_map *pointer_swap = active_table;
-  active_table = work_table;
-  work_table = pointer_swap;
-  PWM_table_swap = true;  // tell the ISR that the tables have been swapped
-  NVIC_EnableIRQ(PWM1_IRQn);  // re-enable PWM interrupts
-}
+  pin = GET_PIN_MAP_PIN(GET_PIN_MAP_INDEX(pin & 0xFF));
 
+  NVIC_DisableIRQ(PWM1_IRQn);
 
-bool LPC1768_PWM_write(pin_t pin, uint32_t value) {
-  while (PWM_table_swap) delay(5);  // don't do anything until the previous change has been implemented by the ISR
-  COPY_ACTIVE_TABLE;  // copy active table into work table
   uint8_t slot = 0xFF;
   for (uint8_t i = 0; i < NUM_PWMS; i++)         // find slot
-    if (work_table[i].pin == pin) slot = i;
+    if (ISR_table[i].pin == pin) { slot = i; break; }
   if (slot == 0xFF) return false;    // return error if pin not found
 
   LPC1768_PWM_update_map_MR();
 
   switch(pin) {
     case P1_20:                        // Servo 0, PWM1 channel 2 (Pin 11  P1.20 PWM1.2)
-      map_MR[pin_11_PWM_channel - 1].PCR_bit = _BV(8 + pin_11_PWM_channel);  // enable PWM1 module control of this pin
-      map_MR[pin_11_PWM_channel - 1].map_PWM_INT = 1;               // 0 - available for interrupts, 1 - in use by PWM
-      map_MR[pin_11_PWM_channel - 1].PINSEL3_bits = 0x2 <<  8;      // ISR must do this AFTER setting PCR
+      map_MR[P1_20_PWM_channel - 1].PCR_bit = _BV(8 + P1_20_PWM_channel);  // enable PWM1 module control of this pin
+      map_MR[P1_20_PWM_channel - 1].PINSEL_reg = &LPC_PINCON->PINSEL3;
+      map_MR[P1_20_PWM_channel - 1].PINSEL_bits = 0x2 <<  8;      // ISR must do this AFTER setting PCR
       break;
     case P1_21:                        // Servo 1, PWM1 channel 3 (Pin 6  P1.21 PWM1.3)
-      map_MR[pin_6_PWM_channel - 1].PCR_bit = _BV(8 + pin_6_PWM_channel);                  // enable PWM1 module control of this pin
-      map_MR[pin_6_PWM_channel - 1].map_PWM_INT = 1;                // 0 - available for interrupts, 1 - in use by PWM
-      map_MR[pin_6_PWM_channel - 1].PINSEL3_bits = 0x2 << 10;       // ISR must do this AFTER setting PCR
+      map_MR[P1_21_PWM_channel - 1].PCR_bit = _BV(8 + P1_21_PWM_channel);                  // enable PWM1 module control of this pin
+      map_MR[P1_21_PWM_channel - 1].PINSEL_reg = &LPC_PINCON->PINSEL3;
+      map_MR[P1_21_PWM_channel - 1].PINSEL_bits = 0x2 << 10;       // ISR must do this AFTER setting PCR
       break;
     case P1_18:                        // Servo 3, PWM1 channel 1 (Pin 4  P1.18 PWM1.1)
-      map_MR[pin_4_PWM_channel - 1].PCR_bit = _BV(8 + pin_4_PWM_channel);                  // enable PWM1 module control of this pin
-      map_MR[pin_4_PWM_channel - 1].map_PWM_INT = 1;                // 0 - available for interrupts, 1 - in use by PWM
-      map_MR[pin_4_PWM_channel - 1].PINSEL3_bits =  0x2 <<  4;       // ISR must do this AFTER setting PCR
+      map_MR[P1_18_PWM_channel - 1].PCR_bit = _BV(8 + P1_18_PWM_channel);                  // enable PWM1 module control of this pin
+      map_MR[P1_18_PWM_channel - 1].PINSEL_reg = &LPC_PINCON->PINSEL3;
+      map_MR[P1_18_PWM_channel - 1].PINSEL_bits =  0x2 <<  4;       // ISR must do this AFTER setting PCR
+      break;
+    case P2_4:                        // D9 FET, PWM1 channel 5 (Pin 9  P2_4 PWM1.5)
+      map_MR[P2_4_PWM_channel - 1].PCR_bit = _BV(8 + P2_4_PWM_channel);                  // enable PWM1 module control of this pin
+      map_MR[P2_4_PWM_channel - 1].PINSEL_reg = &LPC_PINCON->PINSEL4;
+      map_MR[P2_4_PWM_channel - 1].PINSEL_bits = 0x1 << 8;       // ISR must do this AFTER setting PCR
+      break;
+    case P2_5:                        // D10 FET, PWM1 channel 6 (Pin 10  P2_5 PWM1.6)
+      map_MR[P2_5_PWM_channel - 1].PCR_bit = _BV(8 + P2_5_PWM_channel);                  // enable PWM1 module control of this pin
+      map_MR[P2_5_PWM_channel - 1].PINSEL_reg = &LPC_PINCON->PINSEL4;
+      map_MR[P2_5_PWM_channel - 1].PINSEL_bits =  0x1 <<  10;       // ISR must do this AFTER setting PCR
       break;
-    default:                                                        // ISR pins
-      pinMode(pin, OUTPUT);  // set pin to output but don't write anything in case it's already in use
+    default:  // ISR pins
+      pinMode(pin, OUTPUT);  // set pin to output
       break;
   }
 
-  work_table[slot].microseconds = MAX(MIN(value, work_table[slot].max), work_table[slot].min);
-  work_table[slot].active_flag = true;
+  ISR_table[slot].microseconds = MAX(MIN(value, ISR_table[slot].max), ISR_table[slot].min);
+  ISR_table[slot].active_flag = 1;
 
-  LPC1768_PWM_update();
+  ISR_table_update = true;
+
+  NVIC_EnableIRQ(PWM1_IRQn);  // re-enable PWM interrupts
 
   return 1;
 }
 
 
-bool LPC1768_PWM_detach_pin(pin_t pin) {
-  while (PWM_table_swap) delay(5);  // don't do anything until the previous change has been implemented by the ISR
-  COPY_ACTIVE_TABLE;  // copy active table into work table
-  uint8_t slot = 0xFF;
-  for (uint8_t i = 0; i < NUM_PWMS; i++)         // find slot
-    if (work_table[i].pin == pin) slot = i;
-  if (slot == 0xFF) return false;    // return error if pin not found
+uint32_t LPC1768_PWM_interrupt_mask = 1;
 
-  LPC1768_PWM_update_map_MR();
 
-  // OK to make these changes before the MR0 interrupt
-  switch(pin) {
-    case P1_20:                        // Servo 0, PWM1 channel 2  (Pin 11  P1.20 PWM1.2)
-      LPC_PWM1->PCR &= ~(_BV(8 + pin_11_PWM_channel));                 // disable PWM1 module control of this pin
-      map_MR[pin_11_PWM_channel - 1].PCR_bit = 0;
-      LPC_PINCON->PINSEL3 &= ~(0x3 <<  8);    // return pin to general purpose I/O
-      map_MR[pin_11_PWM_channel - 1].PINSEL3_bits = 0;
-      map_MR[pin_11_PWM_channel - 1].map_PWM_INT = 0;               // 0 - available for interrupts, 1 - in use by PWM
-      break;
-    case P1_21:                        // Servo 1, PWM1 channel 3  (Pin 6  P1.21 PWM1.3)
-      LPC_PWM1->PCR &= ~(_BV(8 + pin_6_PWM_channel));                  // disable PWM1 module control of this pin
-      map_MR[pin_6_PWM_channel - 1].PCR_bit = 0;
-      LPC_PINCON->PINSEL3 &= ~(0x3 << 10);  // return pin to general purpose I/O
-      map_MR[pin_6_PWM_channel - 1].PINSEL3_bits = 0;
-      map_MR[pin_6_PWM_channel - 1].map_PWM_INT = 0;                // 0 - available for interrupts, 1 - in use by PWM
-      break;
-    case P1_18:                        // Servo 3, PWM1 channel 1 (Pin 4  P1.18 PWM1.1)
-      LPC_PWM1->PCR &= ~(_BV(8 + pin_4_PWM_channel));                  // disable PWM1 module control of this pin
-      map_MR[pin_4_PWM_channel - 1].PCR_bit =  0;
-      LPC_PINCON->PINSEL3 &= ~(0x3 <<  4);  // return pin to general purpose I/O
-      map_MR[pin_4_PWM_channel - 1].PINSEL3_bits =  0;
-      map_MR[pin_4_PWM_channel - 1].map_PWM_INT = 0;                // 0 - available for interrupts, 1 - in use by PWM
-      break;
-    default:
-      break;
+void LPC1768_PWM_update(void) {     // only called by the ISR
+  LPC1768_PWM_interrupt_mask = 0;                          // set match registers to new values, build IRQ mask
+  // first setup directly controlled PWM pin slots
+
+  bool found;
+  for (uint8_t i = 0; i < NUM_PWMS; i++) {
+    ISR_table[i].PCR_bit = 0;     // clear entries
+    ISR_table[i].PINSEL_reg = 0;
+    ISR_table[i].PINSEL_bits = 0;
+    ISR_table[i].PWM_flag = 1;    // mark slot as interrupt mode until find differently
+
+    if (ISR_table[i].active_flag) {
+      ISR_table[i].sequence = i + 1;
+
+      // first see if there is a PWM1 controlled pin for this entry
+      found = false;
+      for (uint8_t j = 0; (j < NUM_PWMS) && !found; j++) {
+        if ( (map_MR[j].map_PWM_PIN == ISR_table[i].pin)) {
+          map_MR[j].map_PWM_INT = 1;                            // flag that it's already setup for direct control
+          ISR_table[i].PWM_mask = 0;
+          ISR_table[i].PCR_bit = map_MR[j].PCR_bit;            // PCR register bit to enable PWM1 control of this pin
+          ISR_table[i].PINSEL_reg = map_MR[j].PINSEL_reg;  // PINSEL register address to set pin mode to PWM1 control} MR_map;
+          ISR_table[i].PINSEL_bits = map_MR[j].PINSEL_bits;  // PINSEL register bits to set pin mode to PWM1 control} MR_map;
+          map_MR[j].map_used = 2;
+          ISR_table[i].PWM_flag = 0;
+          *map_MR[j].MR_register = ISR_table[i].microseconds;
+          found = true;
+        }
+      }
+    }
+    else
+    ISR_table[i].sequence = 0;
   }
 
-  pinMode(pin, INPUT);
+  // next fill in interrupt slots
+  for (uint8_t i = 0; i < NUM_PWMS; i++) {
 
-  work_table[slot] = PWM_MAP_INIT_ROW;
+    if (ISR_table[i].active_flag && ISR_table[i].PWM_flag) {
 
-  LPC1768_PWM_update();
+      // setup interrupt slot
+      found = false;
+      for (uint8_t k = 0; (k < NUM_PWMS) && !found; k++) {
+        if ( !(map_MR[k].map_PWM_INT || map_MR[k].map_used)) {
+          *map_MR[k].MR_register = ISR_table[i].microseconds;  // found one for an interrupt pin
+          map_MR[k].map_used = 1;
+          LPC1768_PWM_interrupt_mask |= _BV(3 * (k + 1));  // set bit in the MCR to enable this MR to generate an interrupt
+          ISR_table[i].set_register = PIN_IS_INVERTED(ISR_table[i].pin) ? &LPC_GPIO(LPC1768_PIN_PORT(ISR_table[i].pin))->FIOCLR : &LPC_GPIO(LPC1768_PIN_PORT(ISR_table[i].pin))->FIOSET;
+          ISR_table[i].clr_register = PIN_IS_INVERTED(ISR_table[i].pin) ? &LPC_GPIO(LPC1768_PIN_PORT(ISR_table[i].pin))->FIOSET : &LPC_GPIO(LPC1768_PIN_PORT(ISR_table[i].pin))->FIOCLR;
+          ISR_table[i].write_mask = LPC_PIN(LPC1768_PIN_PIN(ISR_table[i].pin));
+          ISR_table[i].PWM_mask = _BV(IR_BIT(k + 1));  // bit in the IR that will go active when this MR generates an interrupt
+          ISR_table[i].PWM_flag = 1;
+          found = true;
+        }
+      }
+    }
+  }
 
-  return 1;
+  LPC1768_PWM_interrupt_mask |= (uint32_t) _BV(0);  // add in MR0 interrupt
+
+  LPC_PWM1->LER = 0x07E;  // Set the latch Enable Bits to load the new Match Values for MR1 - MR6
 }
 
 
 bool useable_hardware_PWM(pin_t pin) {
-  COPY_ACTIVE_TABLE;  // copy active table into work table
+
+  pin = GET_PIN_MAP_PIN(GET_PIN_MAP_INDEX(pin & 0xFF));
+
+  NVIC_DisableIRQ(PWM1_IRQn);
+
+  bool return_flag = false;
   for (uint8_t i = 0; i < NUM_PWMS; i++)         // see if it's already setup
-    if (work_table[i].pin == pin && work_table[i].sequence) return true;
+    if (ISR_table[i].pin == pin && ISR_table[i].sequence) return_flag = true;
   for (uint8_t i = 0; i < NUM_PWMS; i++)         // see if there is an empty slot
-    if (!work_table[i].sequence) return true;
-  return false;    // only get here if neither the above are true
+    if (!ISR_table[i].sequence) return_flag = true;
+  NVIC_EnableIRQ(PWM1_IRQn);  // re-enable PWM interrupts
+  return return_flag;
 }
 
 ////////////////////////////////////////////////////////////////////////////////
 
 #define HAL_PWM_LPC1768_ISR  extern "C" void PWM1_IRQHandler(void)
 
-
 // Both loops could be terminated when the last active channel is found but that would
 // result in variations ISR run time which results in variations in pulse width
 
 /**
- * Changes to PINSEL3, PCR and MCR are only done during the MR0 interrupt otherwise
+ * Changes to PINSEL, PCR and MCR are only done during the MR0 interrupt otherwise
  * the wrong pin may be toggled or even have the system hang.
  */
 
 
 HAL_PWM_LPC1768_ISR {
-  if (PWM_table_swap) ISR_table = work_table;   // use old table if a swap was just done
-  else ISR_table = active_table;
-
-  if (LPC_PWM1->IR & 0x1) {                                      // MR0 interrupt
-    ISR_table = active_table;                    // MR0 means new values could have been loaded so set everything
-    if (PWM_table_swap) LPC_PWM1->MCR = LPC1768_PWM_interrupt_mask; // enable new PWM individual channel interrupts
 
+  if (LPC_PWM1->IR & 0x1) {                       // MR0 interrupt
+    if (ISR_table_update) {                       // new values have been loaded so set everything
+      LPC1768_PWM_update();                       // update & swap table
+      LPC_PWM1->MCR = LPC1768_PWM_interrupt_mask; // enable new PWM individual channel interrupts
+    }
     for (uint8_t i = 0; i < NUM_PWMS; i++) {
-      if(ISR_table[i].active_flag && !((ISR_table[i].pin == P1_20) ||
-                                       (ISR_table[i].pin == P1_21) ||
-                                       (ISR_table[i].pin == P1_18)))
+      if (ISR_table[i].active_flag && !((ISR_table[i].pin == P1_20) ||
+                                        (ISR_table[i].pin == P1_21) ||
+                                        (ISR_table[i].pin == P1_18) ||
+                                        (ISR_table[i].pin == P2_4)  ||
+                                        (ISR_table[i].pin == P2_5))
+      ) {
         *ISR_table[i].set_register = ISR_table[i].write_mask;       // set pins for all enabled interrupt channels active
-      if (PWM_table_swap && ISR_table[i].PCR_bit) {
-        LPC_PWM1->PCR |= ISR_table[i].PCR_bit;              // enable PWM1 module control of this pin
-        LPC_PINCON->PINSEL3 |= ISR_table[i].PINSEL3_bits;   // set pin mode to PWM1 control - must be done after PCR
+      }
+      if (ISR_table_update && ISR_table[i].PCR_bit) {
+        LPC_PWM1->PCR |= ISR_table[i].PCR_bit;                  // enable PWM1 module control of this pin
+        *ISR_table[i].PINSEL_reg |= ISR_table[i].PINSEL_bits;   // set pin mode to PWM1 control - must be done after PCR
       }
     }
-    PWM_table_swap = false;
-    PWM_MR0_wait = false;
+    ISR_table_update = false;
     LPC_PWM1->IR = 0x01;                                             // clear the MR0 interrupt flag bit
   }
   else {
-    for (uint8_t i = 0; i < NUM_PWMS ; i++)
-      if (ISR_table[i].active_flag && (LPC_PWM1->IR & ISR_table[i].PWM_mask) ){
+    for (uint8_t i = 0; i < NUM_PWMS; i++)
+      if (ISR_table[i].active_flag && (LPC_PWM1->IR & ISR_table[i].PWM_mask)) {
         LPC_PWM1->IR = ISR_table[i].PWM_mask;       // clear the interrupt flag bits for expected interrupts
         *ISR_table[i].clr_register = ISR_table[i].write_mask;   // set channel to inactive
       }
   }
 
   LPC_PWM1->IR = 0x70F;  // guarantees all interrupt flags are cleared which, if there is an unexpected
-                           // PWM interrupt, will keep the ISR from hanging which will crash the controller
-
-return;
+                         // PWM interrupt, will keep the ISR from hanging which will crash the controller
 }
+
 #endif
 
 /////////////////////////////////////////////////////////////////
@@ -466,44 +502,26 @@ return;
  *  interrupt.  The only exception is detaching pins.  It doesn't matter when they go
  *  tristate.
  *
- *  The LPC1768_PWM_init routine kicks off the MR0 interrupt.  This interrupt is never disabled or
- *  delayed.
+ *  The LPC1768_PWM_init routine kicks off the MR0 interrupt.  This interrupt is never disabled.  It
+ *  can be delayed by higher priority interrupts.  Actions on directly controlled pins are not delayed
+ *  by other interrupts
  *
- *  The PWM_table_swap flag is set when the firmware has swapped in an updated table.  It is
- *  cleared by the ISR during the MR0 interrupt as it completes the swap and accompanying updates.
- *  It serves two purposes:
- *    1) Tells the ISR that the tables have been swapped
- *    2) Keeps the firmware from starting a new update until the previous one has been completed.
- *
- *  The PWM_MR0_wait flag is set when the firmware is ready to swap in an updated table and cleared by
- *  the ISR during the MR0 interrupt.  It is used to avoid delaying the MR0 interrupt when swapping in
- *  an updated table.  This avoids glitches in pulse width and/or repetition rate.
+ *  The ISR_table_update flag is set when the ISR table needs to be rebuilt.  It is
+ *  cleared by the ISR during the MR0 interrupt after it rebuilds the ISR table.
  *
  *  The sequence of events during a write to a PWM channel is:
- *    1) Waits until PWM_table_swap flag is false before starting
- *    2) Copies the active table into the work table
- *    3) Updates the work table
- *         NOTES - MR1-MR6 are updated at this time.  The updates aren't put into use until the first
+ *    1) Attach routine puts the pin number in the ISR table but doesn't mark it active.
+ *    2) Write routine marks the pin as active, updates the helper table and flags the ISR that the
+ *       ISR table needs to be rebuilt.
+ *    3) On the MR0 interrupt the ISR:
+ *         a. Rebuilds the ISR table if needed.
+ *                 MR1-MR6 are updated at this time.  The updates aren't put into use until the first
  *                 MR0 after the LER register has been written.  The LER register is written during the
- *                 table swap process.
- *               - The MCR mask is created at this time.  It is not used until the ISR writes the MCR
- *                 during the MR0 interrupt in the table swap process.
- *    4) Sets the PWM_MR0_wait flag
- *    5) ISR clears the PWM_MR0_wait flag during the next MR0 interrupt
- *    6) Once the PWM_MR0_wait flag is cleared then the firmware:
- *          disables the ISR interrupt
- *          swaps the pointers to the tables
- *          writes to the LER register
- *          sets the PWM_table_swap flag active
- *          re-enables the ISR
- *     7) On the next interrupt the ISR changes its pointer to the work table which is now the old,
- *        unmodified, active table.
- *     8) On the next MR0 interrupt the ISR:
- *          switches over to the active table
- *          clears the PWM_table_swap and PWM_MR0_wait flags
- *          updates the MCR register with the possibly new interrupt sources/assignments
- *          writes to the PCR register to enable the direct control of the Servo 0, 1 & 3 pins by the PWM1 module
- *          sets the PINSEL3 register to function/mode 0x2 for the Servo 0, 1 & 3 pins
- *             NOTE - PCR must be set before PINSEL
- *          sets the pins controlled by the ISR to their active states
+ *                 table rebuild process.  The result is new timing takes 20-40 mS to be implemented.
+ *         b. Sets the interrupt controlled pin(s) to their active state
+ *         c. Writes to the PCR register to enable the directly controlled pins
+ *         d. Sets the PINSEL register to the function/mode for the directly controlled pins
+ *
+ *    4) For each interrupt controlled pin there is another ISR call.  During this call the pin is set
+ *       to its inactive state.  The call is initiated when a MR1-MR6 reg times out.
  */

Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.h

@@ -69,9 +69,7 @@
 #define MR0_MARGIN 200       // if channel value too close to MR0 the system locks up
 
 void LPC1768_PWM_init(void);
-bool LPC1768_PWM_attach_pin(pin_t pin, uint32_t min = 1, uint32_t max = (LPC_PWM1_MR0 - MR0_MARGIN), uint8_t servo_index = 0xff);
-void LPC1768_PWM_update_map_MR(void);
-void LPC1768_PWM_update(void);
+bool LPC1768_PWM_attach_pin(pin_t pin, uint32_t min=1, uint32_t max=(LPC_PWM1_MR0 - (MR0_MARGIN)), uint8_t servo_index=0xFF);
 bool LPC1768_PWM_write(pin_t pin, uint32_t value);
 bool LPC1768_PWM_detach_pin(pin_t pin);
 bool useable_hardware_PWM(pin_t pin);

Marlin/src/HAL/HAL_LPC1768/fastio.h

@@ -39,7 +39,7 @@
 #include "arduino.h"
 #include "pinmapping.h"
 
-bool useable_hardware_PWM(uint8_t pin);
+bool useable_hardware_PWM(pin_t pin);
 #define USEABLE_HARDWARE_PWM(pin) useable_hardware_PWM(pin)
 
 #define LPC_PORT_OFFSET         (0x0020)

Marlin/src/Marlin.cpp

@@ -522,7 +522,12 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
     tmc2130_checkOverTemp();
   #endif
 
-  planner.check_axes_activity();
+  // Limit check_axes_activity frequency to 10Hz
+  static millis_t next_check_axes_ms = 0;
+  if (ELAPSED(ms, next_check_axes_ms)) {
+    planner.check_axes_activity();
+    next_check_axes_ms = ms + 100UL;
+  }
 }
 
 /**

Marlin/src/gcode/control/M42.cpp

@@ -28,6 +28,11 @@
  * M42: Change pin status via GCode
  *
  *  P<pin>  Pin number (LED if omitted)
+ *            For LPC1768 use M42 P1.20 S255 if wanting to set P1_20 to logic 1
+ *              NOTE - Repetier Host truncates trailing zeros on a decimal when
+ *                     sending commands so typing M42 P1.20 S255 results in 
+ *                     M42 P1.2 S255 being sent.  Pronterface doesn't have this issue.
+ *                     
  *  S<byte> Pin status from 0 - 255
  */
 void GcodeSuite::M42() {

Marlin/src/module/temperature.cpp

@@ -1271,57 +1271,38 @@ void Temperature::init() {
 
 #if ENABLED(FAST_PWM_FAN)
 
-  void Temperature::setPwmFrequency(const uint8_t pin, int val) {
-    val &= 0x07;
-    switch (digitalPinToTimer(pin)) {
-      #ifdef TCCR0A
-        #if !AVR_AT90USB1286_FAMILY
-          case TIMER0A:
+  void Temperature::setPwmFrequency(const pin_t pin, int val) {
+    #ifdef ARDUINO
+      val &= 0x07;
+      switch (digitalPinToTimer(pin)) {
+        #ifdef TCCR0A
+          #if !AVR_AT90USB1286_FAMILY
+            case TIMER0A:
+          #endif
+          case TIMER0B:                           //_SET_CS(0, val);
+                                                    break;
         #endif
-        case TIMER0B:
-          //_SET_CS(0, val);
-          break;
-      #endif
-      #ifdef TCCR1A
-        case TIMER1A:
-        case TIMER1B:
-          //_SET_CS(1, val);
-          break;
-      #endif
-      #ifdef TCCR2
-        case TIMER2:
-        case TIMER2:
-          _SET_CS(2, val);
-          break;
-      #endif
-      #ifdef TCCR2A
-        case TIMER2A:
-        case TIMER2B:
-          _SET_CS(2, val);
-          break;
-      #endif
-      #ifdef TCCR3A
-        case TIMER3A:
-        case TIMER3B:
-        case TIMER3C:
-          _SET_CS(3, val);
-          break;
-      #endif
-      #ifdef TCCR4A
-        case TIMER4A:
-        case TIMER4B:
-        case TIMER4C:
-          _SET_CS(4, val);
-          break;
-      #endif
-      #ifdef TCCR5A
-        case TIMER5A:
-        case TIMER5B:
-        case TIMER5C:
-          _SET_CS(5, val);
-          break;
-      #endif
-    }
+        #ifdef TCCR1A
+          case TIMER1A: case TIMER1B:             //_SET_CS(1, val);
+                                                    break;
+        #endif
+        #ifdef TCCR2
+          case TIMER2: case TIMER2:                 _SET_CS(2, val); break;
+        #endif
+        #ifdef TCCR2A
+          case TIMER2A: case TIMER2B:               _SET_CS(2, val); break;
+        #endif
+        #ifdef TCCR3A
+          case TIMER3A: case TIMER3B: case TIMER3C: _SET_CS(3, val); break;
+        #endif
+        #ifdef TCCR4A
+          case TIMER4A: case TIMER4B: case TIMER4C: _SET_CS(4, val); break;
+        #endif
+        #ifdef TCCR5A
+          case TIMER5A: case TIMER5B: case TIMER5C: _SET_CS(5, val); break;
+        #endif
+      }
+    #endif  
   }
 
 #endif // FAST_PWM_FAN
@@ -1332,7 +1313,7 @@ void Temperature::init() {
    * their target temperature by a configurable margin.
    * This is called when the temperature is set. (M104, M109)
    */
-  void Temperature::start_watching_heater(uint8_t e) {
+  void Temperature::start_watching_heater(const uint8_t e) {
     #if HOTENDS == 1
       UNUSED(e);
     #endif

Marlin/src/module/temperature.h

@@ -361,14 +361,14 @@ class Temperature {
     static int16_t degTargetBed() { return target_temperature_bed; }
 
     #if WATCH_HOTENDS
-      static void start_watching_heater(uint8_t e = 0);
+      static void start_watching_heater(const uint8_t e = 0);
     #endif
 
     #if WATCH_THE_BED
       static void start_watching_bed();
     #endif
 
-    static void setTargetHotend(const int16_t celsius, uint8_t e) {
+    static void setTargetHotend(const int16_t celsius, const uint8_t e) {
       #if HOTENDS == 1
         UNUSED(e);
       #endif