Servo & PWM support for LPC1768 (#7500)

Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.h

@@ -0,0 +1,382 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2017 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * The class Servo uses the PWM class to implement it's functions
+ *
+ * The PWM1 module is only used to generate interrups at specified times. It
+ * is NOT used to directly toggle pins. The ISR writes to the pin assigned to
+ * that interrupt
+ *
+ * All PWMs use the same repetition rate - 20mS because that's the normal servo rate
+ *
+ */
+
+/**
+ * The data structures are setup to minimize the computation done by the ISR which
+ * minimizes ISR execution time.  Execution times are 1.7 to 1.9 microseconds.
+ *
+ * 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.
+ *
+ * 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 ISR's priority is set to the maximum otherwise other ISRs can cause considerable
+ * jitter in the PWM high time.
+ */
+
+
+#ifdef TARGET_LPC1768
+#include <lpc17xx_pinsel.h>
+//#include "../HAL.h"
+//#include "../../../macros.h"
+#include "serial.h"
+
+typedef struct {       // holds all data needed to control the 6 PWM channels
+    uint8_t sequence;  // 0: available slot, 1 - 6: PWM channel assigned to that slot
+    uint8_t logical_pin;
+    uint16_t PWM_mask;
+    volatile uint32_t* set_register;
+    volatile uint32_t* clr_register;
+    uint32_t write_mask;
+    uint32_t microseconds;
+    uint32_t min;
+    uint32_t max;
+    bool    PWM_flag;     //
+    uint8_t servo_index;  // 0 - MAX_SERVO -1 : servo index,  0xFF : PWM channel
+    bool    active_flag;
+
+} PWM_map;
+
+#define MICRO_MAX 0xffffffff
+
+#define PWM_MAP_INIT_ROW {0, 0xff, 0, 0, 0, 0, MICRO_MAX, 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[6] = PWM_MAP_INIT;
+PWM_map PWM1_map_B[6] = PWM_MAP_INIT;
+
+PWM_map *active_table = PWM1_map_A;
+PWM_map *work_table = PWM1_map_B;
+PWM_map *ISR_table;
+
+
+#define NUM_PWMS 6
+
+volatile uint8_t PWM1_ISR_index = 0;
+
+#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
+
+
+/**
+ *  Prescale register and MR0 register values
+ *
+ *               100MHz PCLK          50MHz PCLK          25MHz PCLK         12.5MHz PCLK
+ *             -----------------   -----------------   -----------------   -----------------
+ *  desired    prescale  MR0       prescale  MR0       prescale  MR0       prescale  MR0        resolution
+ *  prescale   register  register  register  register  register  register  register  register   in degrees
+ *  freq       value     value     value     value     value     value     value     value
+ *
+ *      8        11.5    159,999      5.25   159,999       2.13  159,999    0.5625   159,999    0.023
+ *      4        24       79,999     11.5     79,999       5.25   79,999    2.125     79,999    0.045
+ *      2        49       39,999     24       39,999      11.5    39,999    5.25      39,999    0.090
+ *      1        99       19,999     49       19,999      24      19,999   11.5       19,999    0.180
+ *    0.5       199        9,999     99        9,999      49       9,999   24          9,999    0.360
+ *   0.25       399        4,999    199        4,999      99       4,999   49          4,999    0.720
+ *  0.125       799        2,499    399        2,499     199       2,499   99          2,499    1.440
+ *
+ *  The desired prescale frequency comes from an input in the range of 544 - 2400 microseconds and the
+ *  desire to just shift the input left or right as needed.
+ *
+ *  A resolution of 0.2 degrees seems reasonable so a prescale frequency output of 1MHz is being used.
+ *  It also means we don't need to scale the input.
+ *
+ *  The PCLK is set to 25MHz because that's the slowest one that gives whole numbers for prescale and
+ *  MR0 registers.
+ *
+ *  Final settings:
+ *   PCLKSEL0: 0x0
+ *   PWM1PR:   0x018 (24)
+ *   PWM1MR0:  0x04E1F (19,999)
+ *
+ */
+
+#define LPC_PWM1_MR0 19999  // base repetition rate minus one count - 20mS
+#define LPC_PWM1_PR 24      // prescaler value - prescaler divide by 24 + 1  -  1 MHz output
+#define LPC_PWM1_PCLKSEL0 0x00   // select clock source for prescaler - defaults to 25MHz on power up
+                                 // 0: 25MHz, 1: 100MHz, 2: 50MHz, 3: 12.5MHZ to PWM1 prescaler
+#define MR0_MARGIN 200       // if channel value too close to MR0 the system locks up
+
+
+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 PCLK_PWM1      12
+
+  LPC_SC->PCONP |= (1 << 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
+}
+
+
+bool PWM_table_swap;  // flag to tell the ISR that the tables have been swapped
+
+
+
+bool LPC1768_PWM_attach_pin(uint8_t pin, uint32_t min = 1, uint32_t max = (LPC_PWM1_MR0 - MR0_MARGIN), uint8_t servo_index = 0xff) {
+  COPY_ACTIVE_TABLE;  // copy active table into work table
+  uint8_t slot = 0;
+  for (uint8_t i = 0; i < NUM_PWMS ; i++)         // see if already in table
+    if (work_table[i].logical_pin == pin) 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 (!slot) return 0;
+  slot--;  // turn it into array index
+
+  work_table[slot].logical_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(pin_map[pin].port)->FIOCLR : &LPC_GPIO(pin_map[pin].port)->FIOSET;
+  work_table[slot].clr_register = PIN_IS_INVERTED(pin) ? &LPC_GPIO(pin_map[pin].port)->FIOSET : &LPC_GPIO(pin_map[pin].port)->FIOCLR;
+  work_table[slot].write_mask = LPC_PIN(pin_map[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
+  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;
+}
+
+
+
+bool LPC1768_PWM_write(uint8_t pin, uint32_t value) {
+  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].logical_pin == pin) slot = i;
+  if (slot == 0xFF) return false;    // return error if pin not found
+  digitalWrite(pin, 0);  // set pin to output & set it low
+  work_table[slot].microseconds = MAX(MIN(value, work_table[slot].max), work_table[slot].min);
+  work_table[slot].active_flag = true;
+
+  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;
+  }
+
+  for (uint8_t i = 0; i < NUM_PWMS; i++)                             // set the index & PWM_mask
+    if (work_table[i].active_flag == true) {
+      work_table[i].sequence = i + 1;
+      work_table[i].PWM_mask = _BV(IR_BIT(i + 1));
+    }
+    else work_table[i].sequence = 0;
+
+  uint32_t interrupt_mask = 0;                          // set match registers to new values, build IRQ mask
+  if (work_table[0].active_flag == true) {
+    LPC_PWM1->MR1 = work_table[0].microseconds;
+    interrupt_mask |= _BV(3);
+  }
+  if (work_table[1].active_flag == true) {
+    LPC_PWM1->MR2 = work_table[1].microseconds;
+    interrupt_mask |= _BV(6);
+  }
+  if (work_table[2].active_flag == true) {
+    LPC_PWM1->MR3 = work_table[2].microseconds;
+    interrupt_mask |= _BV(9);
+  }
+  if (work_table[3].active_flag == true) {
+    LPC_PWM1->MR4 = work_table[3].microseconds;
+    interrupt_mask |= _BV(12);
+  }
+  if (work_table[4].active_flag == true) {
+    LPC_PWM1->MR5 = work_table[4].microseconds;
+    interrupt_mask |= _BV(15);
+  }
+  if (work_table[5].active_flag == true) {
+    LPC_PWM1->MR6 = work_table[5].microseconds;
+    interrupt_mask |= _BV(18);
+  }
+  interrupt_mask |= _BV(0);  // add in MR0 interrupt
+
+  // swap tables
+  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
+  LPC_PWM1->MCR = interrupt_mask;          // enable new PWM individual channel interrupts
+  NVIC_EnableIRQ(PWM1_IRQn);  // re-enable PWM interrupts
+
+  return 1;
+}
+
+
+
+bool LPC1768_PWM_detach_pin(uint8_t pin) {
+  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].logical_pin == pin) slot = i;
+  if (slot == 0xFF) return false;    // return error if pin not found
+  pinMode(pin, INPUT_PULLUP);  // set pin to input with pullup
+  work_table[slot] = PWM_MAP_INIT_ROW;
+
+  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;
+  }
+
+  for (uint8_t i = 0; i < NUM_PWMS; i++)                             // set the index & PWM_mask
+    if (work_table[i].active_flag == true) {
+      work_table[i].sequence = i + 1;
+      work_table[i].PWM_mask = _BV(IR_BIT(i + 1));
+    }
+    else work_table[i].sequence = 0;
+
+  uint32_t interrupt_mask = 0;                        // set match registers to new values, build IRQ mask
+  if (work_table[0].active_flag == true) {
+    LPC_PWM1->MR1 = work_table[0].microseconds;
+    interrupt_mask |= _BV(3);
+  }
+  if (work_table[1].active_flag == true) {
+    LPC_PWM1->MR2 = work_table[1].microseconds;
+    interrupt_mask |= _BV(6);
+  }
+  if (work_table[2].active_flag == true) {
+    LPC_PWM1->MR3 = work_table[2].microseconds;
+    interrupt_mask |= _BV(9);
+  }
+  if (work_table[3].active_flag == true) {
+    LPC_PWM1->MR4 = work_table[3].microseconds;
+    interrupt_mask |= _BV(12);
+  }
+  if (work_table[4].active_flag == true) {
+    LPC_PWM1->MR5 = work_table[4].microseconds;
+    interrupt_mask |= _BV(15);
+  }
+  if (work_table[5].active_flag == true) {
+    LPC_PWM1->MR6 = work_table[5].microseconds;
+    interrupt_mask |= _BV(18);
+  }
+
+  interrupt_mask |= _BV(0);  // add in MR0 interrupt
+
+  // swap tables
+  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
+  LPC_PWM1->MCR = interrupt_mask;          // enable remaining PWM individual channel interrupts
+  NVIC_EnableIRQ(PWM1_IRQn);  // re-enable PWM interrupts
+
+  return 1;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+
+#define HAL_PWM_LPC1768_ISR  extern "C" void PWM1_IRQHandler(void)
+
+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
+    PWM_table_swap = false;                                         // MR0 means new values could have been
+    ISR_table = active_table;                                       // loaded so set everything to normal operation
+    for (uint8_t i = 0; (i < NUM_PWMS) && ISR_table[i].active_flag ; i++)
+      *ISR_table[i].set_register = ISR_table[i].write_mask;     // set all enabled channels active
+    LPC_PWM1->IR = 0x01;                                             // clear the MR0 interrupt flag bit
+    PWM1_ISR_index = 0;
+  }
+  else {
+    if (ISR_table[PWM1_ISR_index].active_flag && (LPC_PWM1->IR & ISR_table[PWM1_ISR_index].PWM_mask)) {
+      LPC_PWM1->IR = ISR_table[PWM1_ISR_index].PWM_mask;       // clear the interrupt flag bit
+      *ISR_table[PWM1_ISR_index].clr_register = ISR_table[PWM1_ISR_index].write_mask;   // set channel to inactive
+    }
+    PWM1_ISR_index++;                                           // should be the index for the next interrupt
+  }
+
+return;
+}
+
+#endif

Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.cpp

@@ -0,0 +1,169 @@
+/**
+  * Marlin 3D Printer Firmware
+  * Copyright (C) 2016, 2017 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+  *
+  * Based on Sprinter and grbl.
+  * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+  *
+  * This program is free software: you can redistribute it and/or modify
+  * it under the terms of the GNU General Public License as published by
+  * the Free Software Foundation, either version 3 of the License, or
+  * (at your option) any later version.
+  *
+  * This program is distributed in the hope that it will be useful,
+  * but WITHOUT ANY WARRANTY; without even the implied warranty of
+  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  * GNU General Public License for more details.
+  *
+  * You should have received a copy of the GNU General Public License
+  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+  *
+*/
+
+/**
+  * Based on servo.cpp - Interrupt driven Servo library for Arduino using 16 bit
+  * timers- Version 2  Copyright (c) 2009 Michael Margolis.  All right reserved.
+*/
+
+/**
+  * A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
+  * The servos are pulsed in the background using the value most recently written using the write() method
+  *
+  * Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached.
+  * Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four.
+  *
+  * The methods are:
+  *
+  * Servo - Class for manipulating servo motors connected to Arduino pins.
+  *
+  * attach(pin)           - Attach a servo motor to an i/o pin.
+  * attach(pin, min, max) - Attach to a pin, setting min and max values in microseconds
+  *                         Default min is 544, max is 2400
+  *
+  * write()               - Set the servo angle in degrees. (Invalid angles —over MIN_PULSE_WIDTH— are treated as µs.)
+  * writeMicroseconds()   - Set the servo pulse width in microseconds.
+  * move(pin, angle)      - Sequence of attach(pin), write(angle), delay(SERVO_DELAY).
+  *                         With DEACTIVATE_SERVOS_AFTER_MOVE it detaches after SERVO_DELAY.
+  * read()                - Get the last-written servo pulse width as an angle between 0 and 180.
+  * readMicroseconds()    - Get the last-written servo pulse width in microseconds.
+  * attached()            - Return true if a servo is attached.
+  * detach()              - Stop an attached servo from pulsing its i/o pin.
+  *
+*/
+
+/**
+ *  The only time that this library wants physical movement is when a WRITE
+ *  command is issued.  Before that all the attach & detach activity is solely
+ *  within the data base.
+ *
+ *  The PWM output is inactive until the first WRITE.  After that it stays active
+ *  unless DEACTIVATE_SERVOS_AFTER_MOVE is enabled and a MOVE command was issued.
+ *
+ */
+
+
+#if HAS_SERVOS
+
+
+#include "LPC1768_Servo.h"
+#include "servo_private.h"
+
+
+  extern bool LPC1768_PWM_attach_pin(uint8_t, uint32_t, uint32_t, uint8_t);
+  extern bool LPC1768_PWM_write(uint8_t, uint32_t);
+  extern bool LPC1768_PWM_detach_pin(uint8_t);
+
+
+
+  ServoInfo_t servo_info[MAX_SERVOS];                  // static array of servo info structures
+  uint8_t ServoCount = 0;                              // the total number of attached servos
+
+
+  #define US_TO_PULSE_WIDTH(p) p
+  #define PULSE_WIDTH_TO_US(p) p
+  #define TRIM_DURATION 0
+  #define SERVO_MIN() MIN_PULSE_WIDTH  // minimum value in uS for this servo
+  #define SERVO_MAX() MAX_PULSE_WIDTH  // maximum value in uS for this servo
+
+  Servo::Servo() {
+    if (ServoCount < MAX_SERVOS) {
+      this->servoIndex = ServoCount++;                    // assign a servo index to this instance
+      servo_info[this->servoIndex].pulse_width = US_TO_PULSE_WIDTH(DEFAULT_PULSE_WIDTH);   // store default values  - 12 Aug 2009
+    }
+    else
+    this->servoIndex = INVALID_SERVO;  // too many servos
+  }
+
+  int8_t Servo::attach(int pin) {
+    return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
+  }
+
+  int8_t Servo::attach(int pin, int min, int max) {
+
+    if (this->servoIndex >= MAX_SERVOS) return -1;
+
+    if (pin > 0) servo_info[this->servoIndex].Pin.nbr = pin;  // only assign a pin value if the pin info is
+                                                              // greater than zero. This way the init routine can
+                                                              // assign the pin and the MOVE command only needs the value.
+
+
+    this->min = MIN_PULSE_WIDTH; //resolution of min/max is 1 uS
+    this->max = MAX_PULSE_WIDTH;
+
+    servo_info[this->servoIndex].Pin.isActive = true;
+
+    return this->servoIndex;
+  }
+
+  void Servo::detach() {
+    servo_info[this->servoIndex].Pin.isActive = false;
+  }
+
+  void Servo::write(int value) {
+    if (value < MIN_PULSE_WIDTH) { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
+      value = map(constrain(value, 0, 180), 0, 180, SERVO_MIN(), SERVO_MAX());
+        // odd - this sets zero degrees to 544 and 180 degrees to 2400 microseconds but the literature says
+        //          zero degrees should be 500 microseconds and 180 should be 2500
+    }
+    this->writeMicroseconds(value);
+  }
+
+  void Servo::writeMicroseconds(int value) {
+    // calculate and store the values for the given channel
+    byte channel = this->servoIndex;
+    if (channel < MAX_SERVOS) {  // ensure channel is valid
+      // ensure pulse width is valid
+      value = constrain(value, SERVO_MIN(), SERVO_MAX()) - (TRIM_DURATION);
+      value = US_TO_PULSE_WIDTH(value);  // convert to pulse_width after compensating for interrupt overhead - 12 Aug 2009
+
+      servo_info[channel].pulse_width = value;
+      LPC1768_PWM_attach_pin(servo_info[this->servoIndex].Pin.nbr, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH, this->servoIndex);
+      LPC1768_PWM_write(servo_info[this->servoIndex].Pin.nbr, value);
+
+    }
+  }
+
+  // return the value as degrees
+  int Servo::read() { return map(this->readMicroseconds() + 1, SERVO_MIN(), SERVO_MAX(), 0, 180); }
+
+  int Servo::readMicroseconds() {
+    return (this->servoIndex == INVALID_SERVO) ? 0 : PULSE_WIDTH_TO_US(servo_info[this->servoIndex].pulse_width) + TRIM_DURATION;
+  }
+
+  bool Servo::attached() { return servo_info[this->servoIndex].Pin.isActive; }
+
+  void Servo::move(int value) {
+    if (this->attach(0) >= 0) {    // notice the pin number is zero here
+      this->write(value);
+      delay(SERVO_DELAY);
+      #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
+        this->detach();
+        LPC1768_PWM_detach_pin(servo_info[this->servoIndex].Pin.nbr);  // shut down the PWM signal
+      #endif
+    }
+  }
+
+#endif // HAS_SERVOS
+
+
+

Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.h

@@ -0,0 +1,69 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2017 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * The class Servo uses the PWM class to implement it's functions
+ *
+ * The PWM1 module is only used to generate interrups at specified times. It
+ * is NOT used to directly toggle pins. The ISR writes to the pin assigned to
+ * that interrupt
+ *
+ * All PWMs use the same repetition rate - 20mS because that's the normal servo rate
+ *
+ */
+
+#ifndef LPC1768_SERVO_h
+  #define LPC1768_SERVO_h
+
+  #ifdef TARGET_LPC1768
+    #include <inttypes.h>
+
+
+    class Servo {
+      public:
+        Servo();
+        int8_t attach(int pin);            // attach the given pin to the next free channel, set pinMode, return channel number (-1 on fail)
+        int8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes.
+        void detach();
+        void write(int value);             // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds
+        void writeMicroseconds(int value); // write pulse width in microseconds
+        void move(int value);              // attach the servo, then move to value
+                                           // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds
+                                           // if DEACTIVATE_SERVOS_AFTER_MOVE wait SERVO_DELAY, then detach
+        int read();                        // returns current pulse width as an angle between 0 and 180 degrees
+        int readMicroseconds();            // returns current pulse width in microseconds for this servo (was read_us() in first release)
+        bool attached();                   // return true if this servo is attached, otherwise false
+
+      private:
+        uint8_t servoIndex;               // index into the channel data for this servo
+        int min;
+        int max;
+    };
+
+
+    #define HAL_SERVO_LIB Servo
+
+  #endif
+#endif
+
+
+

Marlin/src/HAL/HAL_LPC1768/Servo.cpp

@@ -1,44 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- *
- */
-
-/*
-  Copyright (c) 2013 Arduino LLC. All right reserved.
-
-  This library is free software; you can redistribute it and/or
-  modify it under the terms of the GNU Lesser General Public
-  License as published by the Free Software Foundation; either
-  version 2.1 of the License, or (at your option) any later version.
-
-  This library is distributed in the hope that it will be useful,
-  but WITHOUT ANY WARRANTY; without even the implied warranty of
-  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-  Lesser General Public License for more details.
-
-  You should have received a copy of the GNU Lesser General Public
-  License along with this library; if not, write to the Free Software
-  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
-*/
-
-#ifdef TARGET_LPC1768
-
-
-#endif // ARDUINO_ARCH_SAM

Marlin/src/HAL/HAL_LPC1768/arduino.cpp

@@ -140,27 +140,34 @@ bool digitalRead(int pin) {
   return LPC_GPIO(pin_map[pin].port)->FIOPIN & LPC_PIN(pin_map[pin].pin) ? 1 : 0;
 }
 
-void analogWrite(int pin, int pwm_value) {
-/*
+
+
+void analogWrite(int pin, int pwm_value) {  // 1 - 254: pwm_value, 0: LOW, 255: HIGH
+
+  extern bool LPC1768_PWM_attach_pin(uint8_t, uint32_t, uint32_t, uint8_t);
+  extern bool LPC1768_PWM_write(uint8_t, uint32_t);
+  extern bool LPC1768_PWM_detach_pin(uint8_t);
+  #define MR0_MARGIN 200       // if channel value too close to MR0 the system locks up
+
+  static bool out_of_PWM_slots = false;
+
   if (!WITHIN(pin, 0, NUM_DIGITAL_PINS - 1) || pin_map[pin].port == 0xFF)
     return;
 
-  int old_pin = pin;
-  int old_value = pwm_value;
-
-  if(old_value != 0) {
-    for(uint16_t x = 0; x <= 5000; x++) {
-      LPC_GPIO(pin_map[pin].port)->FIOSET = LPC_PIN(pin_map[pin].pin);
-      //digitalWrite(old_pin, HIGH);
-      delayMicroseconds(old_value);
-      LPC_GPIO(pin_map[pin].port)->FIOCLR = LPC_PIN(pin_map[pin].pin);
-      //pinMode(pin, OUTPUT);
-      //digitalWrite(old_pin, LOW);
-      delayMicroseconds(255 - old_value);
+  uint value = MAX(MIN(pwm_value, 255), 0);
+  if (value == 0 || value == 255) {  // treat as digital pin
+    LPC1768_PWM_detach_pin(pin);    // turn off PWM
+    digitalWrite(pin, value);
+  }
+  else {
+    if (LPC1768_PWM_attach_pin(pin, 1, (LPC_PWM1->MR0 - MR0_MARGIN),  0xff))   // locks up if get too close to MR0 value
+      LPC1768_PWM_write(pin, map(value, 1, 254, 1, (LPC_PWM1->MR0 - MR0_MARGIN)));  // map 1-254 onto PWM range
+    else {                                                                 // out of PWM channels
+      if (!out_of_PWM_slots) usb_serial.printf(".\nWARNING - OUT OF PWM CHANNELS\n.\n");  //only warn once
+      out_of_PWM_slots = true;
+      digitalWrite(pin, value);  // treat as a digital pin if out of channels
     }
   }
-*/
-
 }
 
 extern bool HAL_adc_finished();

Marlin/src/HAL/HAL_LPC1768/main.cpp

@@ -30,6 +30,7 @@ extern "C" {
 #include <stdarg.h>
 #include "arduino.h"
 #include "serial.h"
+#include "LPC1768_PWM.h"
 
 static __INLINE uint32_t SysTick_Config(uint32_t ticks) {
   if (ticks > SysTick_LOAD_RELOAD_Msk)
@@ -86,6 +87,9 @@ int main(void) {
 
   HAL_timer_init();
 
+  extern void LPC1768_PWM_init();
+  LPC1768_PWM_init();
+
   setup();
   while (true) {
     loop();

Marlin/src/HAL/HAL_LPC1768/servo_private.h

@@ -0,0 +1,87 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016, 2017 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+  servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
+  Copyright (c) 2009 Michael Margolis.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+/**
+ * Based on "servo.h - Interrupt driven Servo library for Arduino using 16 bit timers -
+ *           Version 2 Copyright (c) 2009 Michael Margolis.  All right reserved.
+ *
+ * The only modification was to update/delete macros to match the LPC176x.
+ *
+ */
+
+
+#ifndef servo_private_h
+#define servo_private_h
+
+#include <inttypes.h>
+
+
+// Macros
+//values in microseconds
+#define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo
+#define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo
+#define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached
+#define REFRESH_INTERVAL    20000     // minimum time to refresh servos in microseconds
+
+#define MAX_SERVOS             4
+
+#define INVALID_SERVO         255     // flag indicating an invalid servo index
+
+
+// Types
+
+typedef struct {
+  uint8_t nbr        : 8 ;            // a pin number from 0 to 254 (255 signals invalid pin)
+  uint8_t isActive   : 1 ;            // true if this channel is enabled, pin not pulsed if false
+} ServoPin_t;
+
+typedef struct {
+  ServoPin_t Pin;
+  unsigned int pulse_width;           // pulse width in microseconds
+} ServoInfo_t;
+
+// Global variables
+
+extern uint8_t ServoCount;
+extern ServoInfo_t servo_info[MAX_SERVOS];
+
+
+#endif

Marlin/src/HAL/HAL_LPC1768/servotimers.h

@@ -1,25 +0,0 @@
-/*
-  Copyright (c) 2013 Arduino LLC. All right reserved.
-
-  This library is free software; you can redistribute it and/or
-  modify it under the terms of the GNU Lesser General Public
-  License as published by the Free Software Foundation; either
-  version 2.1 of the License, or (at your option) any later version.
-
-  This library is distributed in the hope that it will be useful,
-  but WITHOUT ANY WARRANTY; without even the implied warranty of
-  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-  Lesser General Public License for more details.
-
-  You should have received a copy of the GNU Lesser General Public
-  License along with this library; if not, write to the Free Software
-  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
-*/
-
-/**
- * Defines for 16 bit timers used with  Servo library
- *
- * If _useTimerX is defined then TimerX is a 32 bit timer on the current board
- * timer16_Sequence_t enumerates the sequence that the timers should be allocated
- * _Nbr_16timers indicates how many timers are available.
- */

Marlin/src/HAL/servo.cpp

@@ -56,7 +56,7 @@
 
 #include "HAL.h"
 
-#if HAS_SERVOS && !IS_32BIT_TEENSY
+#if HAS_SERVOS && !(IS_32BIT_TEENSY || defined(TARGET_LPC1768))
 
 //#include <Arduino.h>
 

Marlin/src/HAL/servo.h

@@ -71,6 +71,10 @@
 
 #if IS_32BIT_TEENSY
   #include "HAL_TEENSY35_36/HAL_Servo_Teensy.h" // Teensy HAL uses an inherited library
+
+#elif defined(TARGET_LPC1768)
+  #include "HAL_LPC1768/LPC1768_Servo.cpp"
+
 #else
 
 #include <inttypes.h>