From 97444391e0d13a7fac4ef9f291b69094fe3b64fa Mon Sep 17 00:00:00 2001 From: Bob-the-Kuhn Date: Mon, 14 Aug 2017 14:40:13 -0500 Subject: [PATCH] Servo & PWM support for LPC1768 (#7500) --- Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.h | 382 +++++++++++++++++++ Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.cpp | 169 ++++++++ Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.h | 69 ++++ Marlin/src/HAL/HAL_LPC1768/Servo.cpp | 44 --- Marlin/src/HAL/HAL_LPC1768/arduino.cpp | 39 +- Marlin/src/HAL/HAL_LPC1768/main.cpp | 4 + Marlin/src/HAL/HAL_LPC1768/servo_private.h | 87 +++++ Marlin/src/HAL/HAL_LPC1768/servotimers.h | 25 -- Marlin/src/HAL/servo.cpp | 2 +- Marlin/src/HAL/servo.h | 4 + 10 files changed, 739 insertions(+), 86 deletions(-) create mode 100644 Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.h create mode 100644 Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.cpp create mode 100644 Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.h delete mode 100644 Marlin/src/HAL/HAL_LPC1768/Servo.cpp create mode 100644 Marlin/src/HAL/HAL_LPC1768/servo_private.h delete mode 100644 Marlin/src/HAL/HAL_LPC1768/servotimers.h diff --git a/Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.h b/Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.h new file mode 100644 index 000000000..706bf24ef --- /dev/null +++ b/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 . + * + */ + +/** + * 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 +//#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 diff --git a/Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.cpp b/Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.cpp new file mode 100644 index 000000000..641325089 --- /dev/null +++ b/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 . + * +*/ + +/** + * 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 + + + diff --git a/Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.h b/Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.h new file mode 100644 index 000000000..8162e75bc --- /dev/null +++ b/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 . + * + */ + +/** + * 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 + + + 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 + + + diff --git a/Marlin/src/HAL/HAL_LPC1768/Servo.cpp b/Marlin/src/HAL/HAL_LPC1768/Servo.cpp deleted file mode 100644 index 94cd77483..000000000 --- a/Marlin/src/HAL/HAL_LPC1768/Servo.cpp +++ /dev/null @@ -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 . - * - */ - -/* - 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 diff --git a/Marlin/src/HAL/HAL_LPC1768/arduino.cpp b/Marlin/src/HAL/HAL_LPC1768/arduino.cpp index 6a1344834..7fec1ac13 100644 --- a/Marlin/src/HAL/HAL_LPC1768/arduino.cpp +++ b/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(); diff --git a/Marlin/src/HAL/HAL_LPC1768/main.cpp b/Marlin/src/HAL/HAL_LPC1768/main.cpp index d624b7416..eebac4a2c 100644 --- a/Marlin/src/HAL/HAL_LPC1768/main.cpp +++ b/Marlin/src/HAL/HAL_LPC1768/main.cpp @@ -30,6 +30,7 @@ extern "C" { #include #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(); diff --git a/Marlin/src/HAL/HAL_LPC1768/servo_private.h b/Marlin/src/HAL/HAL_LPC1768/servo_private.h new file mode 100644 index 000000000..e7192b801 --- /dev/null +++ b/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 . + * + */ + +/** + 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 + + +// 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 diff --git a/Marlin/src/HAL/HAL_LPC1768/servotimers.h b/Marlin/src/HAL/HAL_LPC1768/servotimers.h deleted file mode 100644 index c5566b26c..000000000 --- a/Marlin/src/HAL/HAL_LPC1768/servotimers.h +++ /dev/null @@ -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. - */ diff --git a/Marlin/src/HAL/servo.cpp b/Marlin/src/HAL/servo.cpp index 472117cd7..46ab76aa0 100644 --- a/Marlin/src/HAL/servo.cpp +++ b/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 diff --git a/Marlin/src/HAL/servo.h b/Marlin/src/HAL/servo.h index 8d099f311..d189aaa09 100644 --- a/Marlin/src/HAL/servo.h +++ b/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