From 5bc99066349d536fc1192577edee7b8eeda5f5ae Mon Sep 17 00:00:00 2001 From: peteGSX <97784652+peteGSX@users.noreply.github.com> Date: Mon, 17 Jul 2023 14:37:36 +1000 Subject: [PATCH 01/16] Add Servo/Dimmer libraries --- EXIODimmer.h | 57 +++++++ Servo.h | 125 +++++++++++++++ arduino_avr_mega.h | 130 ++++++++++++++++ arduino_avr_nano.h | 41 +++++ arduino_avr_uno.h | 41 +++++ defines.h | 24 ++- display_functions.cpp | 21 ++- globals.h | 13 ++ pin_io_functions.cpp | 50 ++++-- pin_io_functions.h | 2 +- servo_functions.cpp | 58 ++++++- servo_functions.h | 10 ++ src/avr/EXIODimmer.cpp | 100 ++++++++++++ src/avr/Servo.cpp | 317 ++++++++++++++++++++++++++++++++++++++ src/avr/ServoTimers.h | 58 +++++++ src/mbed/Servo.cpp | 139 +++++++++++++++++ src/mbed/ServoTimers.h | 1 + src/megaavr/Servo.cpp | 214 +++++++++++++++++++++++++ src/megaavr/ServoTimers.h | 54 +++++++ src/nrf52/Servo.cpp | 134 ++++++++++++++++ src/nrf52/ServoTimers.h | 38 +++++ src/renesas/Servo.cpp | 223 +++++++++++++++++++++++++++ src/renesas/ServoTimers.h | 1 + src/sam/Servo.cpp | 282 +++++++++++++++++++++++++++++++++ src/sam/ServoTimers.h | 87 +++++++++++ src/samd/Servo.cpp | 297 +++++++++++++++++++++++++++++++++++ src/samd/ServoTimers.h | 71 +++++++++ src/stm32f4/Servo.cpp | 194 +++++++++++++++++++++++ src/stm32f4/ServoTimers.h | 207 +++++++++++++++++++++++++ 29 files changed, 2963 insertions(+), 26 deletions(-) create mode 100644 EXIODimmer.h create mode 100644 Servo.h create mode 100644 src/avr/EXIODimmer.cpp create mode 100644 src/avr/Servo.cpp create mode 100644 src/avr/ServoTimers.h create mode 100644 src/mbed/Servo.cpp create mode 100644 src/mbed/ServoTimers.h create mode 100644 src/megaavr/Servo.cpp create mode 100644 src/megaavr/ServoTimers.h create mode 100644 src/nrf52/Servo.cpp create mode 100644 src/nrf52/ServoTimers.h create mode 100644 src/renesas/Servo.cpp create mode 100644 src/renesas/ServoTimers.h create mode 100644 src/sam/Servo.cpp create mode 100644 src/sam/ServoTimers.h create mode 100644 src/samd/Servo.cpp create mode 100644 src/samd/ServoTimers.h create mode 100644 src/stm32f4/Servo.cpp create mode 100644 src/stm32f4/ServoTimers.h diff --git a/EXIODimmer.h b/EXIODimmer.h new file mode 100644 index 0000000..637c541 --- /dev/null +++ b/EXIODimmer.h @@ -0,0 +1,57 @@ +/* + * © 2023, Peter Cole. All rights reserved. + * + * This file is part of EX-IOExpander. + * + * This 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. + * + * It 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 CommandStation. If not, see . + */ + +#ifndef EXIO_DIMMER_H +#define EXIO_DIMMER_H + +#include + +#if !defined(ARDUINO_ARCH_AVR) +#error "This library only works with AVR." +#endif + +#if defined(ARDUINO_AVR_NANO) || defined(ARDUINO_AVR_PRO) || defined(ARDUINO_AVR_UNO) +#define MAX_DIMMERS 16 +#elif defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_AVR_MEGA) +#define MAX_DIMMERS 62 +#endif +#define INVALID_DIMMER 255 + +#define MIN_ON 0 +#define MAX_ON 255 + +typedef struct { + uint8_t physicalPin; + bool isActive; + uint8_t onValue; +} dimmerDefinition; + +class EXIODimmer { + public: + EXIODimmer(); + uint8_t attach(uint8_t pin); + bool attached(); + void detach(); + void write(uint8_t value); + + private: + uint8_t dimmerIndex; +}; + +#endif \ No newline at end of file diff --git a/Servo.h b/Servo.h new file mode 100644 index 0000000..aab9c16 --- /dev/null +++ b/Servo.h @@ -0,0 +1,125 @@ +/* + 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 +*/ + +/* + 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 sequence used to seize timers is defined in timers.h + + The methods are: + + Servo - Class for manipulating servo motors connected to Arduino pins. + + attach(pin ) - Attaches a servo motor to an I/O pin. + attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds + default min is 544, max is 2400 + + write() - Sets the servo angle in degrees. (invalid angle that is valid as pulse in microseconds is treated as microseconds) + writeMicroseconds() - Sets the servo pulse width in microseconds + read() - Gets the last written servo pulse width as an angle between 0 and 180. + readMicroseconds() - Gets the last written servo pulse width in microseconds. (was read_us() in first release) + attached() - Returns true if there is a servo attached. + detach() - Stops an attached servos from pulsing its I/O pin. + */ + +#ifndef Servo_h +#define Servo_h + +#include + +/* + * Defines for 16 bit timers used with Servo library + * + * If _useTimerX is defined then TimerX is a 16 bit timer on the current board + * timer16_Sequence_t enumerates the sequence that the timers should be allocated + * _Nbr_16timers indicates how many 16 bit timers are available. + */ + +// Architecture specific include +#if defined(ARDUINO_ARCH_AVR) +#include "avr/ServoTimers.h" +#elif defined(ARDUINO_ARCH_SAM) +#include "sam/ServoTimers.h" +#elif defined(ARDUINO_ARCH_SAMD) +#include "samd/ServoTimers.h" +#elif defined(ARDUINO_ARCH_STM32F4) +#include "stm32f4/ServoTimers.h" +#elif defined(ARDUINO_ARCH_NRF52) +#include "nrf52/ServoTimers.h" +#elif defined(ARDUINO_ARCH_MEGAAVR) +#include "megaavr/ServoTimers.h" +#elif defined(ARDUINO_ARCH_MBED) +#include "mbed/ServoTimers.h" +#elif defined(ARDUINO_ARCH_RENESAS) +#include "renesas/ServoTimers.h" +#else +#error "This library only supports boards with an AVR, SAM, SAMD, NRF52 or STM32F4 processor." +#endif + +#define Servo_VERSION 2 // software version of this library + +#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 SERVOS_PER_TIMER 12 // the maximum number of servos controlled by one timer +#define MAX_SERVOS (_Nbr_16timers * SERVOS_PER_TIMER) + +#define INVALID_SERVO 255 // flag indicating an invalid servo index + +#if !defined(ARDUINO_ARCH_STM32F4) + +typedef struct { + uint8_t nbr :6 ; // a pin number from 0 to 63 + uint8_t isActive :1 ; // true if this channel is enabled, pin not pulsed if false +} ServoPin_t ; + +typedef struct { + ServoPin_t Pin; + volatile unsigned int ticks; +} servo_t; + +class Servo +{ +public: + Servo(); + uint8_t attach(int pin); // attach the given pin to the next free channel, sets pinMode, returns channel number or INVALID_SERVO if failure + uint8_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 its treated as an angle, otherwise as pulse width in microseconds + void writeMicroseconds(int value); // Write pulse width in microseconds + 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 + int8_t min; // minimum is this value times 4 added to MIN_PULSE_WIDTH + int8_t max; // maximum is this value times 4 added to MAX_PULSE_WIDTH +}; + +#endif +#endif diff --git a/arduino_avr_mega.h b/arduino_avr_mega.h index 6a50551..b3c3ae2 100644 --- a/arduino_avr_mega.h +++ b/arduino_avr_mega.h @@ -43,4 +43,134 @@ pinName pinNameMap[TOTAL_PINS] = { {A12,"A12"},{A13,"A13"},{A14,"A14"},{A15,"A15"}, }; +// Servo support here +Servo servo1; +Servo servo2; +Servo servo3; +Servo servo4; +Servo servo5; +Servo servo6; +Servo servo7; +Servo servo8; +Servo servo9; +Servo servo10; +Servo servo11; +Servo servo12; +Servo servo13; +Servo servo14; +Servo servo15; +Servo servo16; +Servo servo17; +Servo servo18; +Servo servo19; +Servo servo20; +Servo servo21; +Servo servo22; +Servo servo23; +Servo servo24; +Servo servo25; +Servo servo26; +Servo servo27; +Servo servo28; +Servo servo29; +Servo servo30; +Servo servo31; +Servo servo32; +Servo servo33; +Servo servo34; +Servo servo35; +Servo servo36; +Servo servo37; +Servo servo38; +Servo servo39; +Servo servo40; +Servo servo41; +Servo servo42; +Servo servo43; +Servo servo44; +Servo servo45; +Servo servo46; +Servo servo47; +Servo servo48; + +Servo servoMap[MAX_SERVOS] = { + servo1,servo2,servo3,servo4,servo5,servo6,servo7,servo8,servo9,servo10,servo11,servo12, + servo13,servo14,servo15,servo16,servo17,servo18,servo19,servo20,servo21,servo22,servo23,servo24, + servo25,servo26,servo27,servo28,servo29,servo30,servo31,servo32,servo33,servo34,servo35,servo36, + servo37,servo38,servo39,servo40,servo41,servo42,servo43,servo44,servo45,servo46,servo47,servo48, +}; + +// Dimmer support here +EXIODimmer dimmer1; +EXIODimmer dimmer2; +EXIODimmer dimmer3; +EXIODimmer dimmer4; +EXIODimmer dimmer5; +EXIODimmer dimmer6; +EXIODimmer dimmer7; +EXIODimmer dimmer8; +EXIODimmer dimmer9; +EXIODimmer dimmer10; +EXIODimmer dimmer11; +EXIODimmer dimmer12; +EXIODimmer dimmer13; +EXIODimmer dimmer14; +EXIODimmer dimmer15; +EXIODimmer dimmer16; +EXIODimmer dimmer17; +EXIODimmer dimmer18; +EXIODimmer dimmer19; +EXIODimmer dimmer20; +EXIODimmer dimmer21; +EXIODimmer dimmer22; +EXIODimmer dimmer23; +EXIODimmer dimmer24; +EXIODimmer dimmer25; +EXIODimmer dimmer26; +EXIODimmer dimmer27; +EXIODimmer dimmer28; +EXIODimmer dimmer29; +EXIODimmer dimmer30; +EXIODimmer dimmer31; +EXIODimmer dimmer32; +EXIODimmer dimmer33; +EXIODimmer dimmer34; +EXIODimmer dimmer35; +EXIODimmer dimmer36; +EXIODimmer dimmer37; +EXIODimmer dimmer38; +EXIODimmer dimmer39; +EXIODimmer dimmer40; +EXIODimmer dimmer41; +EXIODimmer dimmer42; +EXIODimmer dimmer43; +EXIODimmer dimmer44; +EXIODimmer dimmer45; +EXIODimmer dimmer46; +EXIODimmer dimmer47; +EXIODimmer dimmer48; +EXIODimmer dimmer49; +EXIODimmer dimmer50; +EXIODimmer dimmer51; +EXIODimmer dimmer52; +EXIODimmer dimmer53; +EXIODimmer dimmer54; +EXIODimmer dimmer55; +EXIODimmer dimmer56; +EXIODimmer dimmer57; +EXIODimmer dimmer58; +EXIODimmer dimmer59; +EXIODimmer dimmer60; +EXIODimmer dimmer61; +EXIODimmer dimmer62; + +EXIODimmer dimmerMap[MAX_DIMMERS] = { + dimmer1,dimmer2,dimmer3,dimmer4,dimmer5,dimmer6,dimmer7,dimmer8,dimmer9,dimmer10,dimmer11,dimmer12, + dimmer13,dimmer14,dimmer15,dimmer16,dimmer17,dimmer18,dimmer19,dimmer20,dimmer21,dimmer22,dimmer23,dimmer24, + dimmer25,dimmer26,dimmer27,dimmer28,dimmer29,dimmer30,dimmer31,dimmer32,dimmer33,dimmer34,dimmer35,dimmer36, + dimmer37,dimmer38,dimmer39,dimmer40,dimmer41,dimmer42,dimmer43,dimmer44,dimmer45,dimmer46,dimmer47,dimmer48, + dimmer49,dimmer50,dimmer51,dimmer52,dimmer53,dimmer54,dimmer55,dimmer56,dimmer57,dimmer58,dimmer59,dimmer60, + dimmer61,dimmer62, +}; + #endif \ No newline at end of file diff --git a/arduino_avr_nano.h b/arduino_avr_nano.h index 7d279f9..9f412ca 100644 --- a/arduino_avr_nano.h +++ b/arduino_avr_nano.h @@ -37,4 +37,45 @@ pinName pinNameMap[TOTAL_PINS] = { {A0,"A0"},{A1,"A1"},{A2,"A2"},{A3,"A3"},{A6,"A6"},{A7,"A7"}, }; +// Servo support here +Servo servo1; +Servo servo2; +Servo servo3; +Servo servo4; +Servo servo5; +Servo servo6; +Servo servo7; +Servo servo8; +Servo servo9; +Servo servo10; +Servo servo11; +Servo servo12; + +Servo servoMap[MAX_SERVOS] = { + servo1, servo2, servo3, servo4, servo5, servo6, servo7, servo8, servo9, servo10, servo11, servo12, +}; + +// Dimmer support here +EXIODimmer dimmer1; +EXIODimmer dimmer2; +EXIODimmer dimmer3; +EXIODimmer dimmer4; +EXIODimmer dimmer5; +EXIODimmer dimmer6; +EXIODimmer dimmer7; +EXIODimmer dimmer8; +EXIODimmer dimmer9; +EXIODimmer dimmer10; +EXIODimmer dimmer11; +EXIODimmer dimmer12; +EXIODimmer dimmer13; +EXIODimmer dimmer14; +EXIODimmer dimmer15; +EXIODimmer dimmer16; + +EXIODimmer dimmerMap[MAX_DIMMERS] = { + dimmer1,dimmer2,dimmer3,dimmer4,dimmer5,dimmer6,dimmer7,dimmer8, + dimmer9,dimmer10,dimmer11,dimmer12,dimmer13,dimmer14,dimmer15,dimmer16, +}; + #endif \ No newline at end of file diff --git a/arduino_avr_uno.h b/arduino_avr_uno.h index 48baeff..3296e2e 100644 --- a/arduino_avr_uno.h +++ b/arduino_avr_uno.h @@ -37,4 +37,45 @@ pinName pinNameMap[TOTAL_PINS] = { {A0,"A0"},{A1,"A1"},{A2,"A2"},{A3,"A3"}, }; +// Servo support here +Servo servo1; +Servo servo2; +Servo servo3; +Servo servo4; +Servo servo5; +Servo servo6; +Servo servo7; +Servo servo8; +Servo servo9; +Servo servo10; +Servo servo11; +Servo servo12; + +Servo servoMap[MAX_SERVOS] = { + servo1, servo2, servo3, servo4, servo5, servo6, servo7, servo8, servo9, servo10, servo11, servo12, +}; + +// Dimmer support here +EXIODimmer dimmer1; +EXIODimmer dimmer2; +EXIODimmer dimmer3; +EXIODimmer dimmer4; +EXIODimmer dimmer5; +EXIODimmer dimmer6; +EXIODimmer dimmer7; +EXIODimmer dimmer8; +EXIODimmer dimmer9; +EXIODimmer dimmer10; +EXIODimmer dimmer11; +EXIODimmer dimmer12; +EXIODimmer dimmer13; +EXIODimmer dimmer14; +EXIODimmer dimmer15; +EXIODimmer dimmer16; + +EXIODimmer dimmerMap[MAX_DIMMERS] = { + dimmer1,dimmer2,dimmer3,dimmer4,dimmer5,dimmer6,dimmer7,dimmer8, + dimmer9,dimmer10,dimmer11,dimmer12,dimmer13,dimmer14,dimmer15,dimmer16, +}; + #endif \ No newline at end of file diff --git a/defines.h b/defines.h index 44ce71c..db4e77e 100644 --- a/defines.h +++ b/defines.h @@ -31,19 +31,25 @@ #define BOARD_TYPE F("Pro Mini") #endif #define TOTAL_PINS 18 -#define NUM_PWM_PINS 6 +// #define NUM_PWM_PINS 6 +#define HAS_SERVO_LIB +#define HAS_DIMMER_LIB #define HAS_EEPROM // Arduino Uno #elif defined(ARDUINO_AVR_UNO) #define BOARD_TYPE F("Uno") #define TOTAL_PINS 16 -#define NUM_PWM_PINS 6 +// #define NUM_PWM_PINS 6 +#define HAS_SERVO_LIB +#define HAS_DIMMER_LIB #define HAS_EEPROM // Arduino Mega2560 #elif defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_AVR_MEGA) #define BOARD_TYPE F("Mega") #define TOTAL_PINS 62 -#define NUM_PWM_PINS 12 +// #define NUM_PWM_PINS 12 +#define HAS_SERVO_LIB +#define HAS_DIMMER_LIB #define HAS_EEPROM #elif defined(ARDUINO_NUCLEO_F411RE) #define BOARD_TYPE F("Nucleo-F411RE") @@ -61,6 +67,8 @@ #define BOARD_TYPE F("BLUEPILL-STM32F103C8") #define TOTAL_PINS 28 #define NUM_PWM_PINS 19 +#else +#define CPU_TYPE_ERROR #endif ///////////////////////////////////////////////////////////////////////////////////// @@ -99,11 +107,12 @@ struct pinDefinition { Define the structure of the pin config */ struct pinConfig { - uint8_t mode; // 1 = digital, 2 = analogue, 3 = PWM + uint8_t mode; // 1 = digital, 2 = analogue, 3 = PWM, 4 = PWM LED bool direction; // 0 = output, 1 = input bool pullup; // 0 = no pullup, 1 = pullup (input only) bool enable; // 0 = disabled (default), 1 = enabled uint8_t analogueLSBByte; // Stores the byte number of the LSB byte in analoguePinStates + uint8_t servoIndex; // Stores the servo or dimmer object array index used by the pin }; /* @@ -133,13 +142,13 @@ struct ServoData { ///////////////////////////////////////////////////////////////////////////////////// // Define the servo profile type values // -#define SERVO_INSTANT 0x00 // Moves immediately between positions (if duration not specified) -#define SERVO_USEDURATION 0x00 // Use specified duration +#define SERVO_INSTANT 0x00 // Moves immediately between positions (if duration not specified) +#define SERVO_USEDURATION 0x00 // Use specified duration #define SERVO_FAST 0x01 // Takes around 500ms end-to-end #define SERVO_MEDIUM 0x02 // 1 second end-to-end #define SERVO_SLOW 0x03 // 2 seconds end-to-end #define SERVO_BOUNCE 0x04 // For semaphores/turnouts with a bit of bounce!! -#define SERVO_NOPOWEROFF 0x80 // Flag to power off after move complete +#define USE_DIMMER 0x80 // Flag to use dimmer rather than servo (NoPowerOff in device driver) ///////////////////////////////////////////////////////////////////////////////////// // Define the register hex values we need to act on or respond with @@ -201,6 +210,7 @@ struct ServoData { #define MODE_DIGITAL 1 #define MODE_ANALOGUE 2 #define MODE_PWM 3 +#define MODE_PWM_LED 4 ///////////////////////////////////////////////////////////////////////////////////// // Ensure test modes defined in myConfig.h have values diff --git a/display_functions.cpp b/display_functions.cpp index 0ac0ad8..3e04548 100644 --- a/display_functions.cpp +++ b/display_functions.cpp @@ -96,8 +96,25 @@ void displayPins() { break; } case MODE_PWM: { - USB_SERIAL.print(F("PWM Output Pin:")); - USB_SERIAL.println(pinLabel); + uint8_t dPinByte = pin / 8; + uint8_t dPinBit = pin - dPinByte * 8; + USB_SERIAL.print(F("PWM Output Pin|Servo|State:")); + USB_SERIAL.print(pinLabel); + USB_SERIAL.print(F("|")); + USB_SERIAL.print(exioPins[pin].servoIndex); + USB_SERIAL.print(F("|")); + USB_SERIAL.println(bitRead(digitalPinStates[dPinByte], dPinBit)); + break; + } + case MODE_PWM_LED: { + uint8_t dPinByte = pin / 8; + uint8_t dPinBit = pin - dPinByte * 8; + USB_SERIAL.print(F("LED Output Pin|Servo|State:")); + USB_SERIAL.print(pinLabel); + USB_SERIAL.print(F("|")); + USB_SERIAL.print(exioPins[pin].servoIndex); + USB_SERIAL.print(F("|")); + USB_SERIAL.println(bitRead(digitalPinStates[dPinByte], dPinBit)); break; } default: diff --git a/globals.h b/globals.h index 4e97315..383f072 100644 --- a/globals.h +++ b/globals.h @@ -23,6 +23,13 @@ #include #include "defines.h" +#if defined(HAS_SERVO_LIB) +#include "Servo.h" +#endif +#if defined(HAS_DIMMER_LIB) +#include "EXIODimmer.h" +#endif + extern pinDefinition pinMap[TOTAL_PINS]; extern pinName pinNameMap[TOTAL_PINS]; extern pinConfig exioPins[TOTAL_PINS]; @@ -46,5 +53,11 @@ extern bool inputTesting; extern bool outputTesting; extern bool pullupTesting; extern ServoData** servoDataArray; +#if defined(HAS_SERVO_LIB) +extern Servo servoMap[MAX_SERVOS]; +#endif +#if defined(HAS_DIMMER_LIB) +extern EXIODimmer dimmerMap[MAX_DIMMERS]; +#endif #endif \ No newline at end of file diff --git a/pin_io_functions.cpp b/pin_io_functions.cpp index 3fca6e2..f321fee 100644 --- a/pin_io_functions.cpp +++ b/pin_io_functions.cpp @@ -25,8 +25,8 @@ pinConfig exioPins[TOTAL_PINS]; int digitalPinBytes = 0; // Used for configuring and sending/receiving digital pins int analoguePinBytes = 0; // Used for sending analogue 16 bit values -byte* digitalPinStates; // Store digital pin states to send to device driver -byte* analoguePinStates; +byte* digitalPinStates; // Store digital pin states to send to device driver +byte* analoguePinStates; // Store analogue pin states to send to device driver unsigned long lastOutputTest = 0; /* @@ -56,6 +56,11 @@ void setupPinDetails() { */ void initialisePins() { for (uint8_t pin = 0; pin < numPins; pin++) { +#if defined(HAS_SERVO_LIB) + if (exioPins[pin].servoIndex != 255 && servoMap[exioPins[pin].servoIndex].attached()) { + servoMap[exioPins[pin].servoIndex].detach(); + } +#endif if (bitRead(pinMap[pin].capability, DIGITAL_INPUT) || bitRead(pinMap[pin].capability, ANALOGUE_INPUT)) { pinMode(pinMap[pin].physicalPin, INPUT); exioPins[pin].direction = 1; @@ -65,6 +70,7 @@ void initialisePins() { exioPins[pin].enable = 0; exioPins[pin].mode = 0; exioPins[pin].pullup = 0; + exioPins[pin].servoIndex = 255; } for (uint8_t dPinByte = 0; dPinByte < digitalPinBytes; dPinByte++) { digitalPinStates[dPinByte] = 0; @@ -75,6 +81,9 @@ void initialisePins() { for (uint8_t pin = 0; pin < numPins; pin++) { servoDataArray[pin] = NULL; } +#if defined(HAS_SERVO_LIB) + nextServoObject = 0; +#endif } /* @@ -176,16 +185,31 @@ bool enableAnalogue(uint8_t pin) { * Function to write PWM output to a pin */ bool writeAnalogue(uint8_t pin, uint16_t value, uint8_t profile, uint16_t duration) { +#if defined(HAS_SERVO_LIB) || defined(HAS_DIMMER_LIB) + if (bitRead(pinMap[pin].capability, DIGITAL_OUTPUT)) { +#else if (bitRead(pinMap[pin].capability, PWM_OUTPUT)) { - if (exioPins[pin].enable && (exioPins[pin].direction || exioPins[pin].mode != MODE_PWM)) { +#endif + if (exioPins[pin].enable && (exioPins[pin].direction || + (exioPins[pin].mode != MODE_PWM && exioPins[pin].mode != MODE_PWM_LED))) { USB_SERIAL.print(F("ERROR! pin ")); USB_SERIAL.print(pinMap[pin].physicalPin); USB_SERIAL.println(F(" already in use, cannot use as a PWM output pin")); return false; } else { - exioPins[pin].enable = 1; - exioPins[pin].mode = MODE_PWM; - exioPins[pin].direction = 0; + bool isLED = bitRead(profile, 7); +#if defined(HAS_SERVO_LIB) || defined(HAS_DIMMER_LIB) + if (!configureServo(pin, isLED)) return false; +#endif + if (!exioPins[pin].enable) { + exioPins[pin].enable = 1; + if (isLED) { + exioPins[pin].mode = MODE_PWM_LED; + } else { + exioPins[pin].mode = MODE_PWM; + } + exioPins[pin].direction = 0; + } uint8_t pinByte = pin / 8; uint8_t pinBit = pin - pinByte * 8; @@ -202,12 +226,12 @@ bool writeAnalogue(uint8_t pin, uint16_t value, uint8_t profile, uint16_t durati s->activePosition = 4095; s->inactivePosition = 0; s->currentPosition = value; - s->profile = SERVO_INSTANT | SERVO_NOPOWEROFF; // Use instant profile (but not this time) + s->profile = SERVO_INSTANT | USE_DIMMER; // Use instant profile (but not this time) } // Animated profile. Initiate the appropriate action. s->currentProfile = profile; - uint8_t profileValue = profile & ~SERVO_NOPOWEROFF; // Mask off 'don't-power-off' bit. + uint8_t profileValue = profile & ~USE_DIMMER; // Mask off 'don't-power-off' bit. s->numSteps = profileValue==SERVO_FAST ? 10 : // 0.5 seconds profileValue==SERVO_MEDIUM ? 20 : // 1.0 seconds profileValue==SERVO_SLOW ? 40 : // 2.0 seconds @@ -226,11 +250,11 @@ bool writeAnalogue(uint8_t pin, uint16_t value, uint8_t profile, uint16_t durati } } -void writePWM(uint8_t pin, uint16_t value) { - if (value >= 0 && value <= 255) { - analogWrite(pinMap[pin].physicalPin, value); - } -} +// void writePWM(uint8_t pin, uint16_t value) { +// if (value >= 0 && value <= 255) { +// analogWrite(pinMap[pin].physicalPin, value); +// } +// } void processInputs() { for (uint8_t pin = 0; pin < numPins; pin++) { diff --git a/pin_io_functions.h b/pin_io_functions.h index a078c4d..ef37bb1 100644 --- a/pin_io_functions.h +++ b/pin_io_functions.h @@ -29,7 +29,7 @@ bool enableDigitalInput(uint8_t pin, bool pullup); bool writeDigitalOutput(uint8_t pin, bool state); bool enableAnalogue(uint8_t pin); bool writeAnalogue(uint8_t pin, uint16_t value, uint8_t profile, uint16_t duration); -void writePWM(uint8_t pin, uint16_t value); +// void writePWM(uint8_t pin, uint16_t value); void processInputs(); bool processOutputTest(bool testState); diff --git a/servo_functions.cpp b/servo_functions.cpp index 74a19b8..d6c95e4 100644 --- a/servo_functions.cpp +++ b/servo_functions.cpp @@ -21,6 +21,14 @@ #include "globals.h" #include "servo_functions.h" #include "pin_io_functions.h" +#if defined(HAS_SERVO_LIB) +#include "Servo.h" +uint8_t nextServoObject = 0; +#endif +#if defined(HAS_DIMMER_LIB) +#include "EXIODimmer.h" +uint8_t nextDimmerObject = 0; +#endif const unsigned int refreshInterval = 50; unsigned long lastRefresh = 0; @@ -58,7 +66,7 @@ void updatePosition(uint8_t pin) { bitSet(digitalPinStates[pinByte], pinBit); // Animation in progress, reposition servo s->stepNumber++; - if ((s->currentProfile & ~SERVO_NOPOWEROFF) == SERVO_BOUNCE) { + if ((s->currentProfile & ~USE_DIMMER) == SERVO_BOUNCE) { // Retrieve step positions from array in flash uint8_t profileValue = bounceProfile[s->stepNumber]; s->currentPosition = map(profileValue, 0, 100, s->fromPosition, s->toPosition); @@ -67,7 +75,9 @@ void updatePosition(uint8_t pin) { s->currentPosition = map(s->stepNumber, 0, s->numSteps, s->fromPosition, s->toPosition); } // Send servo command - writePWM(pin, s->currentPosition); + bitSet(digitalPinStates[pinByte], pinBit); + // writePWM(pin, s->currentPosition); + writeServo(pin, s->currentPosition); } else if (s->stepNumber < s->numSteps + _catchupSteps) { bitSet(digitalPinStates[pinByte], pinBit); // We've finished animation, wait a little to allow servo to catch up @@ -77,4 +87,46 @@ void updatePosition(uint8_t pin) { bitClear(digitalPinStates[pinByte], pinBit); s->numSteps = 0; // Done now. } -} \ No newline at end of file +} + +#if defined(HAS_SERVO_LIB) || defined(HAS_DIMMER_LIB) +bool configureServo(uint8_t pin, bool isLED) { + if (exioPins[pin].servoIndex == 255) { + if (((!isLED && nextServoObject < MAX_SERVOS) || (isLED && nextDimmerObject < MAX_DIMMERS)) && bitRead(pinMap[pin].capability, DIGITAL_OUTPUT)) { + if (isLED) { + exioPins[pin].servoIndex = nextDimmerObject; + nextDimmerObject++; + } else { + exioPins[pin].servoIndex = nextServoObject; + nextServoObject++; + } + } else { + return false; + } + } + if (isLED) { + if (!dimmerMap[exioPins[pin].servoIndex].attached()) { + dimmerMap[exioPins[pin].servoIndex].attach(pinMap[pin].physicalPin); + } + } else { + if (!servoMap[exioPins[pin].servoIndex].attached()) { + servoMap[exioPins[pin].servoIndex].attach(pinMap[pin].physicalPin); + } + } + return true; +} +#endif + +void writeServo(uint8_t pin, uint16_t value) { +#if defined(HAS_SERVO_LIB) || defined(HAS_DIMMER_LIB) + if (exioPins[pin].mode == MODE_PWM) { + servoMap[exioPins[pin].servoIndex].writeMicroseconds(value); + } else if (exioPins[pin].mode == MODE_PWM_LED) { + dimmerMap[exioPins[pin].servoIndex].write(value); + } +#else + if (value >= 0 && value <= 255) { + analogWrite(pinMap[pin].physicalPin, value); + } +#endif +} diff --git a/servo_functions.h b/servo_functions.h index cd99b05..8331aa8 100644 --- a/servo_functions.h +++ b/servo_functions.h @@ -28,8 +28,18 @@ extern const unsigned int refreshInterval; extern unsigned long lastRefresh; const uint8_t bounceProfile[30] = {0,2,3,7,13,33,50,83,100,83,75,70,65,60,60,65,74,84,100,83,75,70,70,72,75,80,87,92,97,100}; +#if defined(HAS_SERVO_LIB) +extern uint8_t nextServoObject; +#endif +#if defined(HAS_DIMMER_LIB) +extern uint8_t nextDimmerObject; +#endif void processServos(); void updatePosition(uint8_t pin); +void writeServo(uint8_t pin, uint16_t value); +#if defined(HAS_SERVO_LIB) || defined(HAS_DIMMER_LIB) +bool configureServo(uint8_t pin, bool isLED); +#endif #endif \ No newline at end of file diff --git a/src/avr/EXIODimmer.cpp b/src/avr/EXIODimmer.cpp new file mode 100644 index 0000000..e8ad7ae --- /dev/null +++ b/src/avr/EXIODimmer.cpp @@ -0,0 +1,100 @@ +#if defined(ARDUINO_ARCH_AVR) + +#include +#include "../../EXIODimmer.h" + +/* + Variables +*/ +static dimmerDefinition dimmers[MAX_DIMMERS]; +uint8_t dimmerCount = 0; +static volatile uint8_t counter = 0; + +/* + Static functions +*/ +static inline void handle_interrupts() { + for (uint8_t i = 0; i < MAX_DIMMERS; i++) { + if (dimmers[i].isActive) { + if (counter < dimmers[i].onValue) { + digitalWrite(dimmers[i].physicalPin, HIGH); + } else { + digitalWrite(dimmers[i].physicalPin, LOW); + } + } + } + counter++; +} + +SIGNAL (TIMER2_COMPA_vect) { + handle_interrupts(); +} + +static void initISR() { + TCCR2A = 0x00; + TCCR2B = (1<dimmerIndex = dimmerCount++; + } else { + this->dimmerIndex = INVALID_DIMMER; + } +} + +uint8_t EXIODimmer::attach(uint8_t pin) { + if (this->dimmerIndex < MAX_DIMMERS) { + if (isTimerActive(this->dimmerIndex) == false) { + initISR(); + dimmers[this->dimmerIndex].isActive = true; + dimmers[this->dimmerIndex].physicalPin = pin; + pinMode(dimmers[this->dimmerIndex].physicalPin, OUTPUT); + } + } + return this->dimmerIndex; +} + +bool EXIODimmer::attached() { + if (dimmers[this->dimmerIndex].isActive) { + return true; + } else { + return false; + } +} + +void EXIODimmer::detach() { + dimmers[this->dimmerIndex].isActive = false; +} + +void EXIODimmer::write(uint8_t value) { + uint8_t channel = this->dimmerIndex; + if (channel < MAX_DIMMERS) { + if (value < MIN_ON) { + value = MIN_ON; + } else if (value > MAX_ON) { + value = MAX_ON; + } + uint8_t oldSREG = SREG; + cli(); + dimmers[channel].onValue = value; + SREG = oldSREG; + } +} + +#endif \ No newline at end of file diff --git a/src/avr/Servo.cpp b/src/avr/Servo.cpp new file mode 100644 index 0000000..11fecc5 --- /dev/null +++ b/src/avr/Servo.cpp @@ -0,0 +1,317 @@ +/* + Servo.cpp - 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 + */ + +#if defined(ARDUINO_ARCH_AVR) + +#include +#include + +#include "Servo.h" + +#define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to tick (assumes prescale of 8) // 12 Aug 2009 +#define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds + + +#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009 + +//#define NBR_TIMERS (MAX_SERVOS / SERVOS_PER_TIMER) + +static servo_t servos[MAX_SERVOS]; // static array of servo structures +static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval) + +uint8_t ServoCount = 0; // the total number of attached servos + + +// convenience macros +#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo +#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer +#define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel +#define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel + +#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in us for this servo +#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in us for this servo + +/************ static functions common to all instances ***********************/ + +static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA) +{ + if( Channel[timer] < 0 ) + *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer + else{ + if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true ) + digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated + } + + Channel[timer]++; // increment to the next channel + if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) { + *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks; + if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated + digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high + } + else { + // finished all channels so wait for the refresh period to expire before starting over + if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed + *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL); + else + *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed + Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel + } +} + +#ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform +// Interrupt handlers for Arduino +#if defined(_useTimer1) +SIGNAL (TIMER1_COMPA_vect) +{ + handle_interrupts(_timer1, &TCNT1, &OCR1A); +} +#endif + +#if defined(_useTimer3) +SIGNAL (TIMER3_COMPA_vect) +{ + handle_interrupts(_timer3, &TCNT3, &OCR3A); +} +#endif + +#if defined(_useTimer4) +SIGNAL (TIMER4_COMPA_vect) +{ + handle_interrupts(_timer4, &TCNT4, &OCR4A); +} +#endif + +#if defined(_useTimer5) +SIGNAL (TIMER5_COMPA_vect) +{ + handle_interrupts(_timer5, &TCNT5, &OCR5A); +} +#endif + +#elif defined WIRING +// Interrupt handlers for Wiring +#if defined(_useTimer1) +void Timer1Service() +{ + handle_interrupts(_timer1, &TCNT1, &OCR1A); +} +#endif +#if defined(_useTimer3) +void Timer3Service() +{ + handle_interrupts(_timer3, &TCNT3, &OCR3A); +} +#endif +#endif + + +static void initISR(timer16_Sequence_t timer) +{ +#if defined (_useTimer1) + if(timer == _timer1) { + TCCR1A = 0; // normal counting mode + TCCR1B = _BV(CS11); // set prescaler of 8 + TCNT1 = 0; // clear the timer count +#if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__) + TIFR |= _BV(OCF1A); // clear any pending interrupts + TIMSK |= _BV(OCIE1A) ; // enable the output compare interrupt +#else + // here if not ATmega8 or ATmega128 + TIFR1 |= _BV(OCF1A); // clear any pending interrupts + TIMSK1 |= _BV(OCIE1A) ; // enable the output compare interrupt +#endif +#if defined(WIRING) + timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service); +#endif + } +#endif + +#if defined (_useTimer3) + if(timer == _timer3) { + TCCR3A = 0; // normal counting mode + TCCR3B = _BV(CS31); // set prescaler of 8 + TCNT3 = 0; // clear the timer count +#if defined(__AVR_ATmega128__) + TIFR |= _BV(OCF3A); // clear any pending interrupts + ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt +#else + TIFR3 = _BV(OCF3A); // clear any pending interrupts + TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt +#endif +#if defined(WIRING) + timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only +#endif + } +#endif + +#if defined (_useTimer4) + if(timer == _timer4) { + TCCR4A = 0; // normal counting mode + TCCR4B = _BV(CS41); // set prescaler of 8 + TCNT4 = 0; // clear the timer count + TIFR4 = _BV(OCF4A); // clear any pending interrupts + TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt + } +#endif + +#if defined (_useTimer5) + if(timer == _timer5) { + TCCR5A = 0; // normal counting mode + TCCR5B = _BV(CS51); // set prescaler of 8 + TCNT5 = 0; // clear the timer count + TIFR5 = _BV(OCF5A); // clear any pending interrupts + TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt + } +#endif +} + +static void finISR(timer16_Sequence_t timer) +{ + //disable use of the given timer +#if defined WIRING // Wiring + if(timer == _timer1) { + #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__) + TIMSK1 &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt + #else + TIMSK &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt + #endif + timerDetach(TIMER1OUTCOMPAREA_INT); + } + else if(timer == _timer3) { + #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__) + TIMSK3 &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt + #else + ETIMSK &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt + #endif + timerDetach(TIMER3OUTCOMPAREA_INT); + } +#else + //For Arduino - in future: call here to a currently undefined function to reset the timer + (void) timer; // squash "unused parameter 'timer' [-Wunused-parameter]" warning +#endif +} + +static boolean isTimerActive(timer16_Sequence_t timer) +{ + // returns true if any servo is active on this timer + for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) { + if(SERVO(timer,channel).Pin.isActive == true) + return true; + } + return false; +} + + +/****************** end of static functions ******************************/ + +Servo::Servo() +{ + if( ServoCount < MAX_SERVOS) { + this->servoIndex = ServoCount++; // assign a servo index to this instance + servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009 + } + else + this->servoIndex = INVALID_SERVO ; // too many servos +} + +uint8_t Servo::attach(int pin) +{ + return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH); +} + +uint8_t Servo::attach(int pin, int min, int max) +{ + if(this->servoIndex < MAX_SERVOS ) { + pinMode( pin, OUTPUT) ; // set servo pin to output + servos[this->servoIndex].Pin.nbr = pin; + // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128 + this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 us + this->max = (MAX_PULSE_WIDTH - max)/4; + // initialize the timer if it has not already been initialized + timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); + if(isTimerActive(timer) == false) + initISR(timer); + servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive + } + return this->servoIndex ; +} + +void Servo::detach() +{ + servos[this->servoIndex].Pin.isActive = false; + timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); + if(isTimerActive(timer) == false) { + finISR(timer); + } +} + +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) + if(value < 0) value = 0; + if(value > 180) value = 180; + value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX()); + } + 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 + { + if( value < SERVO_MIN() ) // ensure pulse width is valid + value = SERVO_MIN(); + else if( value > SERVO_MAX() ) + value = SERVO_MAX(); + + value = value - TRIM_DURATION; + value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009 + + uint8_t oldSREG = SREG; + cli(); + servos[channel].ticks = value; + SREG = oldSREG; + } +} + +int Servo::read() // return the value as degrees +{ + return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180); +} + +int Servo::readMicroseconds() +{ + unsigned int pulsewidth; + if( this->servoIndex != INVALID_SERVO ) + pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION ; // 12 aug 2009 + else + pulsewidth = 0; + + return pulsewidth; +} + +bool Servo::attached() +{ + return servos[this->servoIndex].Pin.isActive ; +} + +#endif // ARDUINO_ARCH_AVR diff --git a/src/avr/ServoTimers.h b/src/avr/ServoTimers.h new file mode 100644 index 0000000..8a35c59 --- /dev/null +++ b/src/avr/ServoTimers.h @@ -0,0 +1,58 @@ +/* + 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 +*/ + +/* + * Defines for 16 bit timers used with Servo library + * + * If _useTimerX is defined then TimerX is a 16 bit timer on the current board + * timer16_Sequence_t enumerates the sequence that the timers should be allocated + * _Nbr_16timers indicates how many 16 bit timers are available. + */ + +/** + * AVR Only definitions + * -------------------- + */ + +// Say which 16 bit timers can be used and in what order +#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) +#define _useTimer5 +#define _useTimer1 +#define _useTimer3 +#define _useTimer4 +typedef enum { _timer5, _timer1, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t; + +#elif defined(__AVR_ATmega32U4__) +#define _useTimer1 +typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t; + +#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) +#define _useTimer3 +#define _useTimer1 +typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t; + +#elif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1281__) || defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega2561__) +#define _useTimer3 +#define _useTimer1 +typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t; + +#else // everything else +#define _useTimer1 +typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t; +#endif diff --git a/src/mbed/Servo.cpp b/src/mbed/Servo.cpp new file mode 100644 index 0000000..efb67f9 --- /dev/null +++ b/src/mbed/Servo.cpp @@ -0,0 +1,139 @@ +#if defined(ARDUINO_ARCH_MBED) + +#include +#include +#include + +#if defined __has_include +# if __has_include ("pinDefinitions.h") +# include "pinDefinitions.h" +# endif +#endif + +class ServoImpl { + mbed::DigitalOut *pin; + mbed::Timeout timeout; // calls a callback once when a timeout expires + mbed::Ticker ticker; // calls a callback repeatedly with a timeout + +public: + ServoImpl(PinName _pin) { + pin = new mbed::DigitalOut(_pin); + } + + ~ServoImpl() { + ticker.detach(); + timeout.detach(); + delete pin; + } + + void start(uint32_t duration_us) { + duration = duration_us; + ticker.attach(mbed::callback(this, &ServoImpl::call), 0.02f); + } + + void call() { + timeout.attach(mbed::callback(this, &ServoImpl::toggle), duration / 1e6); + toggle(); + } + + void toggle() { + *pin = !*pin; + } + + int32_t duration = -1; +}; + +static ServoImpl* servos[MAX_SERVOS]; // static array of servo structures +uint8_t ServoCount = 0; // the total number of attached servos + +#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min) // minimum value in us for this servo +#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max) // maximum value in us for this servo + +#define TRIM_DURATION 15 //callback overhead (35 us) -> 15 us if toggle() is called after starting the timeout + +Servo::Servo() +{ + if (ServoCount < MAX_SERVOS) { + this->servoIndex = ServoCount++; + } else { + this->servoIndex = INVALID_SERVO; // too many servos + } +} + +uint8_t Servo::attach(int pin) +{ + return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH); +} + +uint8_t Servo::attach(int pin, int min, int max) +{ + pinMode(pin, OUTPUT); // set servo pin to output + servos[this->servoIndex] = new ServoImpl(digitalPinToPinName(pin)); + + this->min = (MIN_PULSE_WIDTH - min); + this->max = (MAX_PULSE_WIDTH - max); + return this->servoIndex; +} + +void Servo::detach() +{ + delete servos[this->servoIndex]; + servos[this->servoIndex] = NULL; +} + +void Servo::write(int value) +{ + // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds) + if (value < MIN_PULSE_WIDTH) + { + if (value < 0) + value = 0; + else if (value > 180) + value = 180; + + value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX()); + } + writeMicroseconds(value); +} + +void Servo::writeMicroseconds(int value) +{ + if (!servos[this->servoIndex]) { + return; + } + // calculate and store the values for the given channel + byte channel = this->servoIndex; + if( (channel < MAX_SERVOS) ) // ensure channel is valid + { + if (value < SERVO_MIN()) // ensure pulse width is valid + value = SERVO_MIN(); + else if (value > SERVO_MAX()) + value = SERVO_MAX(); + + value = value - TRIM_DURATION; + if (servos[this->servoIndex]->duration == -1) { + servos[this->servoIndex]->start(value); + } + servos[this->servoIndex]->duration = value; + } +} + +int Servo::read() // return the value as degrees +{ + return map(readMicroseconds(), SERVO_MIN(), SERVO_MAX(), 0, 180); +} + +int Servo::readMicroseconds() +{ + if (!servos[this->servoIndex]) { + return 0; + } + return servos[this->servoIndex]->duration; +} + +bool Servo::attached() +{ + return servos[this->servoIndex] != NULL; +} + +#endif diff --git a/src/mbed/ServoTimers.h b/src/mbed/ServoTimers.h new file mode 100644 index 0000000..47f226f --- /dev/null +++ b/src/mbed/ServoTimers.h @@ -0,0 +1 @@ +#define _Nbr_16timers 32 diff --git a/src/megaavr/Servo.cpp b/src/megaavr/Servo.cpp new file mode 100644 index 0000000..59b3e44 --- /dev/null +++ b/src/megaavr/Servo.cpp @@ -0,0 +1,214 @@ +#if defined(ARDUINO_ARCH_MEGAAVR) + +#include +#include + +#define usToTicks(_us) ((clockCyclesPerMicrosecond() / 16 * _us) / 4) // converts microseconds to tick +#define ticksToUs(_ticks) (((unsigned) _ticks * 16) / (clockCyclesPerMicrosecond() / 4)) // converts from ticks back to microseconds + +#define TRIM_DURATION 5 // compensation ticks to trim adjust for digitalWrite delays + +static servo_t servos[MAX_SERVOS]; // static array of servo structures + +uint8_t ServoCount = 0; // the total number of attached servos + +static volatile int8_t currentServoIndex[_Nbr_16timers]; // index for the servo being pulsed for each timer (or -1 if refresh interval) + +// convenience macros +#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo +#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer +#define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel +#define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel + +#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in us for this servo +#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in us for this servo + +#undef REFRESH_INTERVAL +#define REFRESH_INTERVAL 16000 + +void ServoHandler(int timer) +{ + if (currentServoIndex[timer] < 0) { + // Write compare register + _timer->CCMP = 0; + } else { + if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) { + digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW); // pulse this channel low if activated + } + } + + // Select the next servo controlled by this timer + currentServoIndex[timer]++; + + if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && currentServoIndex[timer] < SERVOS_PER_TIMER) { + if (SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) { // check if activated + digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high + } + + // Get the counter value + uint16_t tcCounterValue = 0; //_timer->CCMP; + _timer->CCMP = (uint16_t) (tcCounterValue + SERVO(timer, currentServoIndex[timer]).ticks); + } + else { + // finished all channels so wait for the refresh period to expire before starting over + + // Get the counter value + uint16_t tcCounterValue = _timer->CCMP; + + if (tcCounterValue + 4UL < usToTicks(REFRESH_INTERVAL)) { // allow a few ticks to ensure the next OCR1A not missed + _timer->CCMP = (uint16_t) usToTicks(REFRESH_INTERVAL); + } + else { + _timer->CCMP = (uint16_t) (tcCounterValue + 4UL); // at least REFRESH_INTERVAL has elapsed + } + + currentServoIndex[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel + } + + /* Clear flag */ + _timer->INTFLAGS = TCB_CAPT_bm; +} + +#if defined USE_TIMERB0 +ISR(TCB0_INT_vect) +#elif defined USE_TIMERB1 +ISR(TCB1_INT_vect) +#elif defined USE_TIMERB2 +ISR(TCB2_INT_vect) +#endif +{ + ServoHandler(0); +} + +static void initISR(timer16_Sequence_t timer) +{ + //TCA0.SINGLE.CTRLA = (TCA_SINGLE_CLKSEL_DIV16_gc) | (TCA_SINGLE_ENABLE_bm); + + _timer->CTRLA = TCB_CLKSEL_CLKTCA_gc; + // Timer to Periodic interrupt mode + // This write will also disable any active PWM outputs + _timer->CTRLB = TCB_CNTMODE_INT_gc; + // Enable interrupt + _timer->INTCTRL = TCB_CAPTEI_bm; + // Enable timer + _timer->CTRLA |= TCB_ENABLE_bm; +} + +static void finISR(timer16_Sequence_t timer) +{ + // Disable interrupt + _timer->INTCTRL = 0; +} + +static boolean isTimerActive(timer16_Sequence_t timer) +{ + // returns true if any servo is active on this timer + for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) { + if(SERVO(timer,channel).Pin.isActive == true) + return true; + } + return false; +} + +/****************** end of static functions ******************************/ + +Servo::Servo() +{ + if (ServoCount < MAX_SERVOS) { + this->servoIndex = ServoCount++; // assign a servo index to this instance + servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values + } else { + this->servoIndex = INVALID_SERVO; // too many servos + } +} + +uint8_t Servo::attach(int pin) +{ + return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH); +} + +uint8_t Servo::attach(int pin, int min, int max) +{ + timer16_Sequence_t timer; + + if (this->servoIndex < MAX_SERVOS) { + pinMode(pin, OUTPUT); // set servo pin to output + servos[this->servoIndex].Pin.nbr = pin; + // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128 + this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 us + this->max = (MAX_PULSE_WIDTH - max)/4; + // initialize the timer if it has not already been initialized + timer = SERVO_INDEX_TO_TIMER(servoIndex); + if (isTimerActive(timer) == false) { + initISR(timer); + } + servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive + } + return this->servoIndex; +} + +void Servo::detach() +{ + timer16_Sequence_t timer; + + servos[this->servoIndex].Pin.isActive = false; + timer = SERVO_INDEX_TO_TIMER(servoIndex); + if(isTimerActive(timer) == false) { + finISR(timer); + } +} + +void Servo::write(int value) +{ + // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds) + if (value < MIN_PULSE_WIDTH) + { + if (value < 0) + value = 0; + else if (value > 180) + value = 180; + + value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX()); + } + 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 + { + if (value < SERVO_MIN()) // ensure pulse width is valid + value = SERVO_MIN(); + else if (value > SERVO_MAX()) + value = SERVO_MAX(); + + value = value - TRIM_DURATION; + value = usToTicks(value); // convert to ticks after compensating for interrupt overhead + servos[channel].ticks = value; + } +} + +int Servo::read() // return the value as degrees +{ + return map(readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180); +} + +int Servo::readMicroseconds() +{ + unsigned int pulsewidth; + if (this->servoIndex != INVALID_SERVO) + pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION; + else + pulsewidth = 0; + + return pulsewidth; +} + +bool Servo::attached() +{ + return servos[this->servoIndex].Pin.isActive; +} + +#endif diff --git a/src/megaavr/ServoTimers.h b/src/megaavr/ServoTimers.h new file mode 100644 index 0000000..56746dc --- /dev/null +++ b/src/megaavr/ServoTimers.h @@ -0,0 +1,54 @@ +/* + Copyright (c) 2018 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 + * + */ + +#ifndef __SERVO_TIMERS_H__ +#define __SERVO_TIMERS_H__ + +#include + +//#define USE_TIMERB1 // interferes with PWM on pin 3 +#define USE_TIMERB2 // interferes with PWM on pin 11 +//#define USE_TIMERB0 // interferes with PWM on pin 6 + +#if !defined(USE_TIMERB1) && !defined(USE_TIMERB2) && !defined(USE_TIMERB0) + # error "No timers allowed for Servo" + /* Please uncomment a timer above and rebuild */ +#endif + +static volatile TCB_t* _timer = +#if defined(USE_TIMERB0) +&TCB0; +#endif +#if defined(USE_TIMERB1) +&TCB1; +#endif +#if defined(USE_TIMERB2) +&TCB2; +#endif + +typedef enum { + timer0, + _Nbr_16timers } timer16_Sequence_t; + + +#endif /* __SERVO_TIMERS_H__ */ diff --git a/src/nrf52/Servo.cpp b/src/nrf52/Servo.cpp new file mode 100644 index 0000000..72bd504 --- /dev/null +++ b/src/nrf52/Servo.cpp @@ -0,0 +1,134 @@ +/* + Copyright (c) 2016 Arduino. 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 +*/ + +#if defined(ARDUINO_ARCH_NRF52) + +#include +#include + + +static servo_t servos[MAX_SERVOS]; // static array of servo structures + +uint8_t ServoCount = 0; // the total number of attached servos + + + +uint32_t group_pins[3][NRF_PWM_CHANNEL_COUNT]={{NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED}, {NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED}, {NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED}}; +static uint16_t seq_values[3][NRF_PWM_CHANNEL_COUNT]={{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}; + +Servo::Servo() +{ + if (ServoCount < MAX_SERVOS) { + this->servoIndex = ServoCount++; // assign a servo index to this instance + } else { + this->servoIndex = INVALID_SERVO; // too many servos + } + +} + +uint8_t Servo::attach(int pin) +{ + + return this->attach(pin, 0, 2500); +} + + +uint8_t Servo::attach(int pin, int min, int max) +{ + int servo_min, servo_max; + if (this->servoIndex < MAX_SERVOS) { + pinMode(pin, OUTPUT); // set servo pin to output + servos[this->servoIndex].Pin.nbr = pin; + + if(min < servo_min) min = servo_min; + if (max > servo_max) max = servo_max; + this->min = min; + this->max = max; + + servos[this->servoIndex].Pin.isActive = true; + + } + return this->servoIndex; +} + +void Servo::detach() +{ + servos[this->servoIndex].Pin.isActive = false; +} + + +void Servo::write(int value) +{ + if (value < 0) + value = 0; + else if (value > 180) + value = 180; + value = map(value, 0, 180, MIN_PULSE, MAX_PULSE); + + writeMicroseconds(value); +} + + +void Servo::writeMicroseconds(int value) +{ + uint8_t channel, instance; + uint8_t pin = servos[this->servoIndex].Pin.nbr; + //instance of PWM module is MSB - look at VWariant.h + instance=(g_APinDescription[pin].ulPWMChannel & 0xF0)/16; + //index of PWM channel is LSB - look at VWariant.h + channel=g_APinDescription[pin].ulPWMChannel & 0x0F; + group_pins[instance][channel]=g_APinDescription[pin].ulPin; + NRF_PWM_Type * PWMInstance = instance == 0 ? NRF_PWM0 : (instance == 1 ? NRF_PWM1 : NRF_PWM2); + //configure PWM instance and enable it + seq_values[instance][channel]= value | 0x8000; + nrf_pwm_sequence_t const seq={ + seq_values[instance], + NRF_PWM_VALUES_LENGTH(seq_values), + 0, + 0 + }; + nrf_pwm_pins_set(PWMInstance, group_pins[instance]); + nrf_pwm_enable(PWMInstance); + nrf_pwm_configure(PWMInstance, NRF_PWM_CLK_125kHz, NRF_PWM_MODE_UP, 2500); // 20ms - 50Hz + nrf_pwm_decoder_set(PWMInstance, NRF_PWM_LOAD_INDIVIDUAL, NRF_PWM_STEP_AUTO); + nrf_pwm_sequence_set(PWMInstance, 0, &seq); + nrf_pwm_loop_set(PWMInstance, 0UL); + nrf_pwm_task_trigger(PWMInstance, NRF_PWM_TASK_SEQSTART0); +} + +int Servo::read() // return the value as degrees +{ + return map(readMicroseconds(), MIN_PULSE, MAX_PULSE, 0, 180); +} + +int Servo::readMicroseconds() +{ + uint8_t channel, instance; + uint8_t pin=servos[this->servoIndex].Pin.nbr; + instance=(g_APinDescription[pin].ulPWMChannel & 0xF0)/16; + channel=g_APinDescription[pin].ulPWMChannel & 0x0F; + // remove the 16th bit we added before + return seq_values[instance][channel] & 0x7FFF; +} + +bool Servo::attached() +{ + return servos[this->servoIndex].Pin.isActive; +} + +#endif // ARDUINO_ARCH_NRF52 diff --git a/src/nrf52/ServoTimers.h b/src/nrf52/ServoTimers.h new file mode 100644 index 0000000..f4fc176 --- /dev/null +++ b/src/nrf52/ServoTimers.h @@ -0,0 +1,38 @@ +/* + Copyright (c) 2016 Arduino. 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 +*/ + +/* + * nRF52 doesn't use timer, but PWM. This file include definitions to keep + * compatibility with the Servo library standards. + */ + +#ifndef __SERVO_TIMERS_H__ +#define __SERVO_TIMERS_H__ + +/** + * nRF52 only definitions + * --------------------- + */ + +#define MIN_PULSE 55 +#define MAX_PULSE 284 + +// define one timer in order to have MAX_SERVOS = 12 +typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t; + +#endif // __SERVO_TIMERS_H__ diff --git a/src/renesas/Servo.cpp b/src/renesas/Servo.cpp new file mode 100644 index 0000000..eaeb073 --- /dev/null +++ b/src/renesas/Servo.cpp @@ -0,0 +1,223 @@ +/* The MIT License (MIT) + * + * Copyright (c) 2022 Arduino SA + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ +#if defined(ARDUINO_ARCH_RENESAS) + +#include "Arduino.h" +#include "Servo.h" +#include "ServoTimers.h" +#include "math.h" +#include "FspTimer.h" + +#define SERVO_MAX_SERVOS (_Nbr_16timers * SERVOS_PER_TIMER) +#define SERVO_INVALID_INDEX (255) +// Lower the timer ticks for finer resolution. +#define SERVO_TIMER_TICK_US (100) +#define SERVO_US_PER_CYCLE (20000) +#define SERVO_IO_PORT_ADDR(pn) &((R_PORT0 + ((uint32_t) (R_PORT1 - R_PORT0) * (pn)))->PCNTR3) + +// Internal Servo sturct to keep track of RA configuration. +typedef struct { + // Servo period in microseconds. + uint32_t period_us; + // Store min/max pulse width here, because min/max in + // Servo class are not wide enough for the pulse width. + uint32_t period_min; + uint32_t period_max; + // Period period_count in microseconds. + uint32_t period_count; + // Internal FSP GPIO port/pin control bits. + volatile uint32_t *io_port; + uint32_t io_mask; +} ra_servo_t; + +// Keep track of the total number of servos attached. +static size_t n_servos=0; +static ra_servo_t ra_servos[SERVO_MAX_SERVOS]; + +static FspTimer servo_timer; +static bool servo_timer_started = false; +void servo_timer_callback(timer_callback_args_t *args); + +static int servo_timer_config(uint32_t period_us) +{ + static bool configured = false; + if (configured == false) { + // Configure and enable the servo timer. + uint8_t type = 0; + int8_t channel = FspTimer::get_available_timer(type); + if (channel != -1) { + servo_timer.begin(TIMER_MODE_PERIODIC, type, channel, + 1000000.0f/period_us, 50.0f, servo_timer_callback, nullptr); + servo_timer.setup_overflow_irq(); + servo_timer.open(); + servo_timer.stop(); + configured = true; + } + } + return configured ? 0 : -1; +} + +static int servo_timer_start() +{ + // Start the timer if it's not started + if (servo_timer_started == false && + servo_timer.start() == false) { + return -1; + } + servo_timer_started = true; + return 0; +} + +static int servo_timer_stop() +{ + // Start the timer if it's not started + if (servo_timer_started == true && + servo_timer.stop() == false) { + return -1; + } + servo_timer_started = false; + return 0; +} + +void servo_timer_callback(timer_callback_args_t *args) +{ + for (size_t i=0; iperiod_us) { + servo->period_count += SERVO_TIMER_TICK_US; + if (servo->period_count <= servo->period_us) { + *servo->io_port = (uint32_t) servo->io_mask; + } else { + *servo->io_port = (uint32_t) (servo->io_mask << 16); + } + if (servo->period_count == SERVO_US_PER_CYCLE) { + servo->period_count = 0; + } + } + } +} + +Servo::Servo() +{ + servoIndex = SERVO_INVALID_INDEX; +} + +uint8_t Servo::attach(int pin) +{ + return attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH); +} + +bool Servo::attached() +{ + return (servoIndex != SERVO_INVALID_INDEX); +} + +uint8_t Servo::attach(int pin, int min, int max) +{ + //assert(pin < NUM_DIGITAL_PINS); ? + if (n_servos == SERVO_MAX_SERVOS) { + return 0; + } + + // Try to find a free servo slot. + ra_servo_t *servo = NULL; + bsp_io_port_pin_t io_pin = g_pin_cfg[pin].pin; + for (size_t i=0; iperiod_us == 0) { + n_servos++; + servoIndex = i; + servo->period_min = min; + servo->period_max = max; + servo->io_mask = (1U << (io_pin & 0xFF)); + servo->io_port = SERVO_IO_PORT_ADDR(((io_pin >> 8U) & 0xFF)); + writeMicroseconds(DEFAULT_PULSE_WIDTH); + break; + } + } + + if (servoIndex == SERVO_INVALID_INDEX) { + return 0; + } + + // Configure GPIO pin for the servo. + R_IOPORT_PinCfg(&g_ioport_ctrl, io_pin, + IOPORT_CFG_PORT_DIRECTION_OUTPUT | IOPORT_CFG_PORT_OUTPUT_HIGH); + + // Configure and start the timer if it's not started. + if (servo_timer_config(SERVO_TIMER_TICK_US) != 0 || + servo_timer_start() != 0) { + return 0; + } + return 1; +} + +void Servo::detach() +{ + if (servoIndex != SERVO_INVALID_INDEX) { + ra_servo_t *servo = &ra_servos[servoIndex]; + servo_timer_stop(); + servo->period_us = 0; + if (--n_servos) { + servo_timer_start(); + } + servoIndex = SERVO_INVALID_INDEX; + } +} + +void Servo::write(int angle) +{ + if (servoIndex != SERVO_INVALID_INDEX) { + ra_servo_t *servo = &ra_servos[servoIndex]; + angle = constrain(angle, 0, 180); + writeMicroseconds(map(angle, 0, 180, servo->period_min, servo->period_max)); + } +} + +int Servo::read() +{ + if (servoIndex != SERVO_INVALID_INDEX) { + ra_servo_t *servo = &ra_servos[servoIndex]; + return map(servo->period_us, servo->period_min, servo->period_max, 0, 180); + } + return 0; +} + +void Servo::writeMicroseconds(int us) +{ + if (servoIndex != SERVO_INVALID_INDEX) { + ra_servo_t *servo = &ra_servos[servoIndex]; + servo->period_count = 0; + servo->period_us = constrain(us, servo->period_min, servo->period_max); + } +} + +int Servo::readMicroseconds() +{ + if (servoIndex != SERVO_INVALID_INDEX) { + ra_servo_t *servo = &ra_servos[servoIndex]; + return servo->period_us; + } + return 0; +} +#endif // defined(ARDUINO_ARCH_RENESAS) diff --git a/src/renesas/ServoTimers.h b/src/renesas/ServoTimers.h new file mode 100644 index 0000000..c310497 --- /dev/null +++ b/src/renesas/ServoTimers.h @@ -0,0 +1 @@ +#define _Nbr_16timers 1 diff --git a/src/sam/Servo.cpp b/src/sam/Servo.cpp new file mode 100644 index 0000000..df5058f --- /dev/null +++ b/src/sam/Servo.cpp @@ -0,0 +1,282 @@ +/* + 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 +*/ + +#if defined(ARDUINO_ARCH_SAM) + +#include +#include + +#define usToTicks(_us) (( clockCyclesPerMicrosecond() * _us) / 32) // converts microseconds to tick +#define ticksToUs(_ticks) (( (unsigned)_ticks * 32)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds + +#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays + +static servo_t servos[MAX_SERVOS]; // static array of servo structures + +uint8_t ServoCount = 0; // the total number of attached servos + +static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval) + +// convenience macros +#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo +#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer +#define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel +#define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel + +#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in us for this servo +#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in us for this servo + +/************ static functions common to all instances ***********************/ + +//------------------------------------------------------------------------------ +/// Interrupt handler for the TC0 channel 1. +//------------------------------------------------------------------------------ +void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel); +#if defined (_useTimer1) +void HANDLER_FOR_TIMER1(void) { + Servo_Handler(_timer1, TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); +} +#endif +#if defined (_useTimer2) +void HANDLER_FOR_TIMER2(void) { + Servo_Handler(_timer2, TC_FOR_TIMER2, CHANNEL_FOR_TIMER2); +} +#endif +#if defined (_useTimer3) +void HANDLER_FOR_TIMER3(void) { + Servo_Handler(_timer3, TC_FOR_TIMER3, CHANNEL_FOR_TIMER3); +} +#endif +#if defined (_useTimer4) +void HANDLER_FOR_TIMER4(void) { + Servo_Handler(_timer4, TC_FOR_TIMER4, CHANNEL_FOR_TIMER4); +} +#endif +#if defined (_useTimer5) +void HANDLER_FOR_TIMER5(void) { + Servo_Handler(_timer5, TC_FOR_TIMER5, CHANNEL_FOR_TIMER5); +} +#endif + +void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel) +{ + // clear interrupt + tc->TC_CHANNEL[channel].TC_SR; + if (Channel[timer] < 0) { + tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer + } else { + if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true) { + digitalWrite(SERVO(timer,Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated + } + } + + Channel[timer]++; // increment to the next channel + if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) { + tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks; + if(SERVO(timer,Channel[timer]).Pin.isActive == true) { // check if activated + digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high + } + } + else { + // finished all channels so wait for the refresh period to expire before starting over + if( (tc->TC_CHANNEL[channel].TC_CV) + 4 < usToTicks(REFRESH_INTERVAL) ) { // allow a few ticks to ensure the next OCR1A not missed + tc->TC_CHANNEL[channel].TC_RA = (unsigned int)usToTicks(REFRESH_INTERVAL); + } + else { + tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + 4; // at least REFRESH_INTERVAL has elapsed + } + Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel + } +} + +static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) +{ + pmc_enable_periph_clk(id); + TC_Configure(tc, channel, + TC_CMR_TCCLKS_TIMER_CLOCK3 | // MCK/32 + TC_CMR_WAVE | // Waveform mode + TC_CMR_WAVSEL_UP_RC ); // Counter running up and reset when equals to RC + + /* 84 MHz, MCK/32, for 1.5 ms: 3937 */ + TC_SetRA(tc, channel, 2625); // 1ms + + /* Configure and enable interrupt */ + NVIC_EnableIRQ(irqn); + // TC_IER_CPAS: RA Compare + tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPAS; + + // Enables the timer clock and performs a software reset to start the counting + TC_Start(tc, channel); +} + +static void initISR(timer16_Sequence_t timer) +{ +#if defined (_useTimer1) + if (timer == _timer1) + _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1); +#endif +#if defined (_useTimer2) + if (timer == _timer2) + _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2); +#endif +#if defined (_useTimer3) + if (timer == _timer3) + _initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3); +#endif +#if defined (_useTimer4) + if (timer == _timer4) + _initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4); +#endif +#if defined (_useTimer5) + if (timer == _timer5) + _initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5); +#endif +} + +static void finISR(timer16_Sequence_t timer) +{ +#if defined (_useTimer1) + TC_Stop(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); +#endif +#if defined (_useTimer2) + TC_Stop(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2); +#endif +#if defined (_useTimer3) + TC_Stop(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3); +#endif +#if defined (_useTimer4) + TC_Stop(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4); +#endif +#if defined (_useTimer5) + TC_Stop(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5); +#endif +} + + +static boolean isTimerActive(timer16_Sequence_t timer) +{ + // returns true if any servo is active on this timer + for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) { + if(SERVO(timer,channel).Pin.isActive == true) + return true; + } + return false; +} + +/****************** end of static functions ******************************/ + +Servo::Servo() +{ + if (ServoCount < MAX_SERVOS) { + this->servoIndex = ServoCount++; // assign a servo index to this instance + servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values + } else { + this->servoIndex = INVALID_SERVO; // too many servos + } +} + +uint8_t Servo::attach(int pin) +{ + return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH); +} + +uint8_t Servo::attach(int pin, int min, int max) +{ + timer16_Sequence_t timer; + + if (this->servoIndex < MAX_SERVOS) { + pinMode(pin, OUTPUT); // set servo pin to output + servos[this->servoIndex].Pin.nbr = pin; + // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128 + this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 us + this->max = (MAX_PULSE_WIDTH - max)/4; + // initialize the timer if it has not already been initialized + timer = SERVO_INDEX_TO_TIMER(servoIndex); + if (isTimerActive(timer) == false) { + initISR(timer); + } + servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive + } + return this->servoIndex; +} + +void Servo::detach() +{ + timer16_Sequence_t timer; + + servos[this->servoIndex].Pin.isActive = false; + timer = SERVO_INDEX_TO_TIMER(servoIndex); + if(isTimerActive(timer) == false) { + finISR(timer); + } +} + +void Servo::write(int value) +{ + // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds) + if (value < MIN_PULSE_WIDTH) + { + if (value < 0) + value = 0; + else if (value > 180) + value = 180; + + value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX()); + } + 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 + { + if (value < SERVO_MIN()) // ensure pulse width is valid + value = SERVO_MIN(); + else if (value > SERVO_MAX()) + value = SERVO_MAX(); + + value = value - TRIM_DURATION; + value = usToTicks(value); // convert to ticks after compensating for interrupt overhead + servos[channel].ticks = value; + } +} + +int Servo::read() // return the value as degrees +{ + return map(readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180); +} + +int Servo::readMicroseconds() +{ + unsigned int pulsewidth; + if (this->servoIndex != INVALID_SERVO) + pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION; + else + pulsewidth = 0; + + return pulsewidth; +} + +bool Servo::attached() +{ + return servos[this->servoIndex].Pin.isActive; +} + +#endif // ARDUINO_ARCH_SAM diff --git a/src/sam/ServoTimers.h b/src/sam/ServoTimers.h new file mode 100644 index 0000000..a7ee258 --- /dev/null +++ b/src/sam/ServoTimers.h @@ -0,0 +1,87 @@ +/* + 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 16 bit timer on the current board + * timer16_Sequence_t enumerates the sequence that the timers should be allocated + * _Nbr_16timers indicates how many 16 bit timers are available. + */ + +/** + * SAM Only definitions + * -------------------- + */ + +// For SAM3X: +#define _useTimer1 +#define _useTimer2 +#define _useTimer3 +#define _useTimer4 +#define _useTimer5 + +/* + TC0, chan 0 => TC0_Handler + TC0, chan 1 => TC1_Handler + TC0, chan 2 => TC2_Handler + TC1, chan 0 => TC3_Handler + TC1, chan 1 => TC4_Handler + TC1, chan 2 => TC5_Handler + TC2, chan 0 => TC6_Handler + TC2, chan 1 => TC7_Handler + TC2, chan 2 => TC8_Handler + */ + +#if defined (_useTimer1) +#define TC_FOR_TIMER1 TC1 +#define CHANNEL_FOR_TIMER1 0 +#define ID_TC_FOR_TIMER1 ID_TC3 +#define IRQn_FOR_TIMER1 TC3_IRQn +#define HANDLER_FOR_TIMER1 TC3_Handler +#endif +#if defined (_useTimer2) +#define TC_FOR_TIMER2 TC1 +#define CHANNEL_FOR_TIMER2 1 +#define ID_TC_FOR_TIMER2 ID_TC4 +#define IRQn_FOR_TIMER2 TC4_IRQn +#define HANDLER_FOR_TIMER2 TC4_Handler +#endif +#if defined (_useTimer3) +#define TC_FOR_TIMER3 TC1 +#define CHANNEL_FOR_TIMER3 2 +#define ID_TC_FOR_TIMER3 ID_TC5 +#define IRQn_FOR_TIMER3 TC5_IRQn +#define HANDLER_FOR_TIMER3 TC5_Handler +#endif +#if defined (_useTimer4) +#define TC_FOR_TIMER4 TC0 +#define CHANNEL_FOR_TIMER4 2 +#define ID_TC_FOR_TIMER4 ID_TC2 +#define IRQn_FOR_TIMER4 TC2_IRQn +#define HANDLER_FOR_TIMER4 TC2_Handler +#endif +#if defined (_useTimer5) +#define TC_FOR_TIMER5 TC0 +#define CHANNEL_FOR_TIMER5 0 +#define ID_TC_FOR_TIMER5 ID_TC0 +#define IRQn_FOR_TIMER5 TC0_IRQn +#define HANDLER_FOR_TIMER5 TC0_Handler +#endif + +typedef enum { _timer1, _timer2, _timer3, _timer4, _timer5, _Nbr_16timers } timer16_Sequence_t ; diff --git a/src/samd/Servo.cpp b/src/samd/Servo.cpp new file mode 100644 index 0000000..d8e2ec4 --- /dev/null +++ b/src/samd/Servo.cpp @@ -0,0 +1,297 @@ +/* + Copyright (c) 2015 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 +*/ + +#if defined(ARDUINO_ARCH_SAMD) + +#include +#include + +#define usToTicks(_us) ((clockCyclesPerMicrosecond() * _us) / 16) // converts microseconds to tick +#define ticksToUs(_ticks) (((unsigned) _ticks * 16) / clockCyclesPerMicrosecond()) // converts from ticks back to microseconds + +#define TRIM_DURATION 5 // compensation ticks to trim adjust for digitalWrite delays + +static servo_t servos[MAX_SERVOS]; // static array of servo structures + +uint8_t ServoCount = 0; // the total number of attached servos + +static volatile int8_t currentServoIndex[_Nbr_16timers]; // index for the servo being pulsed for each timer (or -1 if refresh interval) + +// convenience macros +#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo +#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer +#define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel +#define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel + +#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in us for this servo +#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in us for this servo + +#define WAIT_TC16_REGS_SYNC(x) while(x->COUNT16.STATUS.bit.SYNCBUSY); + +/************ static functions common to all instances ***********************/ + +void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel, uint8_t intFlag); +#if defined (_useTimer1) +void HANDLER_FOR_TIMER1(void) { + Servo_Handler(_timer1, TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, INTFLAG_BIT_FOR_TIMER_1); +} +#endif +#if defined (_useTimer2) +void HANDLER_FOR_TIMER2(void) { + Servo_Handler(_timer2, TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, INTFLAG_BIT_FOR_TIMER_2); +} +#endif + +void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel, uint8_t intFlag) +{ + if (currentServoIndex[timer] < 0) { + tc->COUNT16.COUNT.reg = (uint16_t) 0; + WAIT_TC16_REGS_SYNC(tc) + } else { + if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) { + digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW); // pulse this channel low if activated + } + } + + // Select the next servo controlled by this timer + currentServoIndex[timer]++; + + if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && currentServoIndex[timer] < SERVOS_PER_TIMER) { + if (SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) { // check if activated + digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high + } + + // Get the counter value + uint16_t tcCounterValue = tc->COUNT16.COUNT.reg; + WAIT_TC16_REGS_SYNC(tc) + + tc->COUNT16.CC[channel].reg = (uint16_t) (tcCounterValue + SERVO(timer, currentServoIndex[timer]).ticks); + WAIT_TC16_REGS_SYNC(tc) + } + else { + // finished all channels so wait for the refresh period to expire before starting over + + // Get the counter value + uint16_t tcCounterValue = tc->COUNT16.COUNT.reg; + WAIT_TC16_REGS_SYNC(tc) + + if (tcCounterValue + 4UL < usToTicks(REFRESH_INTERVAL)) { // allow a few ticks to ensure the next OCR1A not missed + tc->COUNT16.CC[channel].reg = (uint16_t) usToTicks(REFRESH_INTERVAL); + } + else { + tc->COUNT16.CC[channel].reg = (uint16_t) (tcCounterValue + 4UL); // at least REFRESH_INTERVAL has elapsed + } + WAIT_TC16_REGS_SYNC(tc) + + currentServoIndex[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel + } + + // Clear the interrupt + tc->COUNT16.INTFLAG.reg = intFlag; +} + +static inline void resetTC (Tc* TCx) +{ + // Disable TCx + TCx->COUNT16.CTRLA.reg &= ~TC_CTRLA_ENABLE; + WAIT_TC16_REGS_SYNC(TCx) + + // Reset TCx + TCx->COUNT16.CTRLA.reg = TC_CTRLA_SWRST; + WAIT_TC16_REGS_SYNC(TCx) + while (TCx->COUNT16.CTRLA.bit.SWRST); +} + +static void _initISR(Tc *tc, uint8_t channel, uint32_t id, IRQn_Type irqn, uint8_t gcmForTimer, uint8_t intEnableBit) +{ + // Enable GCLK for timer 1 (timer counter input clock) + GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID(gcmForTimer)); + while (GCLK->STATUS.bit.SYNCBUSY); + + // Reset the timer + // TODO this is not the right thing to do if more than one channel per timer is used by the Servo library + resetTC(tc); + + // Set timer counter mode to 16 bits + tc->COUNT16.CTRLA.reg |= TC_CTRLA_MODE_COUNT16; + + // Set timer counter mode as normal PWM + tc->COUNT16.CTRLA.reg |= TC_CTRLA_WAVEGEN_NPWM; + + // Set the prescaler factor to GCLK_TC/16. At nominal 48 MHz GCLK_TC this is 3000 ticks per millisecond + tc->COUNT16.CTRLA.reg |= TC_CTRLA_PRESCALER_DIV16; + + // Count up + tc->COUNT16.CTRLBCLR.bit.DIR = 1; + WAIT_TC16_REGS_SYNC(tc) + + // First interrupt request after 1 ms + tc->COUNT16.CC[channel].reg = (uint16_t) usToTicks(1000UL); + WAIT_TC16_REGS_SYNC(tc) + + // Configure interrupt request + // TODO this should be changed if more than one channel per timer is used by the Servo library + NVIC_DisableIRQ(irqn); + NVIC_ClearPendingIRQ(irqn); + NVIC_SetPriority(irqn, 0); + NVIC_EnableIRQ(irqn); + + // Enable the match channel interrupt request + tc->COUNT16.INTENSET.reg = intEnableBit; + + // Enable the timer and start it + tc->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE; + WAIT_TC16_REGS_SYNC(tc) +} + +static void initISR(timer16_Sequence_t timer) +{ +#if defined (_useTimer1) + if (timer == _timer1) + _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1, GCM_FOR_TIMER_1, INTENSET_BIT_FOR_TIMER_1); +#endif +#if defined (_useTimer2) + if (timer == _timer2) + _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2, GCM_FOR_TIMER_2, INTENSET_BIT_FOR_TIMER_2); +#endif +} + +static void finISR(timer16_Sequence_t timer) +{ +#if defined (_useTimer1) + // Disable the match channel interrupt request + TC_FOR_TIMER1->COUNT16.INTENCLR.reg = INTENCLR_BIT_FOR_TIMER_1; +#endif +#if defined (_useTimer2) + // Disable the match channel interrupt request + TC_FOR_TIMER2->COUNT16.INTENCLR.reg = INTENCLR_BIT_FOR_TIMER_2; +#endif +} + +static boolean isTimerActive(timer16_Sequence_t timer) +{ + // returns true if any servo is active on this timer + for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) { + if(SERVO(timer,channel).Pin.isActive == true) + return true; + } + return false; +} + +/****************** end of static functions ******************************/ + +Servo::Servo() +{ + if (ServoCount < MAX_SERVOS) { + this->servoIndex = ServoCount++; // assign a servo index to this instance + servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values + } else { + this->servoIndex = INVALID_SERVO; // too many servos + } +} + +uint8_t Servo::attach(int pin) +{ + return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH); +} + +uint8_t Servo::attach(int pin, int min, int max) +{ + timer16_Sequence_t timer; + + if (this->servoIndex < MAX_SERVOS) { + pinMode(pin, OUTPUT); // set servo pin to output + servos[this->servoIndex].Pin.nbr = pin; + // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128 + this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 us + this->max = (MAX_PULSE_WIDTH - max)/4; + // initialize the timer if it has not already been initialized + timer = SERVO_INDEX_TO_TIMER(servoIndex); + if (isTimerActive(timer) == false) { + initISR(timer); + } + servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive + } + return this->servoIndex; +} + +void Servo::detach() +{ + timer16_Sequence_t timer; + + servos[this->servoIndex].Pin.isActive = false; + timer = SERVO_INDEX_TO_TIMER(servoIndex); + if(isTimerActive(timer) == false) { + finISR(timer); + } +} + +void Servo::write(int value) +{ + // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds) + if (value < MIN_PULSE_WIDTH) + { + if (value < 0) + value = 0; + else if (value > 180) + value = 180; + + value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX()); + } + 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 + { + if (value < SERVO_MIN()) // ensure pulse width is valid + value = SERVO_MIN(); + else if (value > SERVO_MAX()) + value = SERVO_MAX(); + + value = value - TRIM_DURATION; + value = usToTicks(value); // convert to ticks after compensating for interrupt overhead + servos[channel].ticks = value; + } +} + +int Servo::read() // return the value as degrees +{ + return map(readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180); +} + +int Servo::readMicroseconds() +{ + unsigned int pulsewidth; + if (this->servoIndex != INVALID_SERVO) + pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION; + else + pulsewidth = 0; + + return pulsewidth; +} + +bool Servo::attached() +{ + return servos[this->servoIndex].Pin.isActive; +} + +#endif // ARDUINO_ARCH_SAMD diff --git a/src/samd/ServoTimers.h b/src/samd/ServoTimers.h new file mode 100644 index 0000000..0b18f60 --- /dev/null +++ b/src/samd/ServoTimers.h @@ -0,0 +1,71 @@ +/* + Copyright (c) 2015 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 16 bit timer on the current board + * timer16_Sequence_t enumerates the sequence that the timers should be allocated + * _Nbr_16timers indicates how many 16 bit timers are available. + */ + +#ifndef __SERVO_TIMERS_H__ +#define __SERVO_TIMERS_H__ + +/** + * SAMD Only definitions + * --------------------- + */ + +// For SAMD: +#define _useTimer1 +//#define _useTimer2 // <- TODO do not activate until the code in Servo.cpp has been changed in order + // to manage more than one channel per timer on the SAMD architecture + +#if defined (_useTimer1) +#define TC_FOR_TIMER1 TC4 +#define CHANNEL_FOR_TIMER1 0 +#define INTENSET_BIT_FOR_TIMER_1 TC_INTENSET_MC0 +#define INTENCLR_BIT_FOR_TIMER_1 TC_INTENCLR_MC0 +#define INTFLAG_BIT_FOR_TIMER_1 TC_INTFLAG_MC0 +#define ID_TC_FOR_TIMER1 ID_TC4 +#define IRQn_FOR_TIMER1 TC4_IRQn +#define HANDLER_FOR_TIMER1 TC4_Handler +#define GCM_FOR_TIMER_1 GCM_TC4_TC5 +#endif +#if defined (_useTimer2) +#define TC_FOR_TIMER2 TC4 +#define CHANNEL_FOR_TIMER2 1 +#define INTENSET_BIT_FOR_TIMER_2 TC_INTENSET_MC1 +#define INTENCLR_BIT_FOR_TIMER_2 TC_INTENCLR_MC1 +#define ID_TC_FOR_TIMER2 ID_TC4 +#define IRQn_FOR_TIMER2 TC4_IRQn +#define HANDLER_FOR_TIMER2 TC4_Handler +#define GCM_FOR_TIMER_2 GCM_TC4_TC5 +#endif + +typedef enum { +#if defined (_useTimer1) + _timer1, +#endif +#if defined (_useTimer2) + _timer2, +#endif + _Nbr_16timers } timer16_Sequence_t; + +#endif // __SERVO_TIMERS_H__ diff --git a/src/stm32f4/Servo.cpp b/src/stm32f4/Servo.cpp new file mode 100644 index 0000000..01d05d9 --- /dev/null +++ b/src/stm32f4/Servo.cpp @@ -0,0 +1,194 @@ +/****************************************************************************** + * The MIT License + * + * Copyright (c) 2010, LeafLabs, LLC. + * + * Permission is hereby granted, free of charge, to any person + * obtaining a copy of this software and associated documentation + * files (the "Software"), to deal in the Software without + * restriction, including without limitation the rights to use, copy, + * modify, merge, publish, distribute, sublicense, and/or sell copies + * of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS + * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN + * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + *****************************************************************************/ + +#if defined(ARDUINO_ARCH_STM32F4) + +#include "ServoTimers.h" + +#include "boards.h" +#include "io.h" +#include "pwm.h" +#include "math.h" + +// 20 millisecond period config. For a 1-based prescaler, +// +// (prescaler * overflow / CYC_MSEC) msec = 1 timer cycle = 20 msec +// => prescaler * overflow = 20 * CYC_MSEC +// +// This picks the smallest prescaler that allows an overflow < 2^16. +#define MAX_OVERFLOW ((1 << 16) - 1) +#define CYC_MSEC (1000 * CYCLES_PER_MICROSECOND) +#define TAU_MSEC 20 +#define TAU_USEC (TAU_MSEC * 1000) +#define TAU_CYC (TAU_MSEC * CYC_MSEC) +#define SERVO_PRESCALER (TAU_CYC / MAX_OVERFLOW + 1) +#define SERVO_OVERFLOW ((uint16)round((double)TAU_CYC / SERVO_PRESCALER)) + +// Unit conversions +#define US_TO_COMPARE(us) ((uint16)map((us), 0, TAU_USEC, 0, SERVO_OVERFLOW)) +#define COMPARE_TO_US(c) ((uint32)map((c), 0, SERVO_OVERFLOW, 0, TAU_USEC)) +#define ANGLE_TO_US(a) ((uint16)(map((a), this->minAngle, this->maxAngle, \ + this->minPW, this->maxPW))) +#define US_TO_ANGLE(us) ((int16)(map((us), this->minPW, this->maxPW, \ + this->minAngle, this->maxAngle))) + +Servo::Servo() { + this->resetFields(); +} + +bool Servo::attach(uint8 pin, uint16 minPW, uint16 maxPW, int16 minAngle, int16 maxAngle) +{ + // SerialUSB.begin(115200); + // SerialUSB.println(MAX_OVERFLOW); + + + timer_dev *tdev = PIN_MAP[pin].timer_device; + + analogWriteResolution(16); + + int prescaler = 6; + int overflow = 65400; + int minPW_correction = 300; + int maxPW_correction = 300; + + pinMode(pin, OUTPUT); + + + if (tdev == NULL) { + // don't reset any fields or ASSERT(0), to keep driving any + // previously attach()ed servo. + return false; + } + + if ( (tdev == TIMER1) || (tdev == TIMER8) || (tdev == TIMER10) || (tdev == TIMER11)) + { + prescaler = 54; + overflow = 65400; + minPW_correction = 40; + maxPW_correction = 50; + } + + if ( (tdev == TIMER2) || (tdev == TIMER3) || (tdev == TIMER4) || (tdev == TIMER5) ) + { + prescaler = 6; + overflow = 64285; + minPW_correction = 370; + maxPW_correction = 350; + } + + if ( (tdev == TIMER6) || (tdev == TIMER7) ) + { + prescaler = 6; + overflow = 65400; + minPW_correction = 0; + maxPW_correction = 0; + } + + if ( (tdev == TIMER9) || (tdev == TIMER12) || (tdev == TIMER13) || (tdev == TIMER14) ) + { + prescaler = 6; + overflow = 65400; + minPW_correction = 30; + maxPW_correction = 0; + } + + if (this->attached()) { + this->detach(); + } + + this->pin = pin; + this->minPW = (minPW + minPW_correction); + this->maxPW = (maxPW + maxPW_correction); + this->minAngle = minAngle; + this->maxAngle = maxAngle; + + timer_pause(tdev); + timer_set_prescaler(tdev, prescaler); // prescaler is 1-based + timer_set_reload(tdev, overflow); + timer_generate_update(tdev); + timer_resume(tdev); + + return true; +} + +bool Servo::detach() { + if (!this->attached()) { + return false; + } + + timer_dev *tdev = PIN_MAP[this->pin].timer_device; + uint8 tchan = PIN_MAP[this->pin].timer_channel; + timer_set_mode(tdev, tchan, TIMER_DISABLED); + + this->resetFields(); + + return true; +} + +void Servo::write(int degrees) { + degrees = constrain(degrees, this->minAngle, this->maxAngle); + this->writeMicroseconds(ANGLE_TO_US(degrees)); +} + +int Servo::read() const { + int a = US_TO_ANGLE(this->readMicroseconds()); + // map() round-trips in a weird way we mostly correct for here; + // the round-trip is still sometimes off-by-one for write(1) and + // write(179). + return a == this->minAngle || a == this->maxAngle ? a : a + 1; +} + +void Servo::writeMicroseconds(uint16 pulseWidth) { + if (!this->attached()) { + ASSERT(0); + return; + } + pulseWidth = constrain(pulseWidth, this->minPW, this->maxPW); + analogWrite(this->pin, US_TO_COMPARE(pulseWidth)); +} + +uint16 Servo::readMicroseconds() const { + if (!this->attached()) { + ASSERT(0); + return 0; + } + + stm32_pin_info pin_info = PIN_MAP[this->pin]; + uint16 compare = timer_get_compare(pin_info.timer_device, + pin_info.timer_channel); + + return COMPARE_TO_US(compare); +} + +void Servo::resetFields(void) { + this->pin = NOT_ATTACHED; + this->minAngle = MIN_ANGLE; + this->maxAngle = MAX_ANGLE; + this->minPW = MIN_PULSE_WIDTH; + this->maxPW = MAX_PULSE_WIDTH; +} + +#endif diff --git a/src/stm32f4/ServoTimers.h b/src/stm32f4/ServoTimers.h new file mode 100644 index 0000000..53bd647 --- /dev/null +++ b/src/stm32f4/ServoTimers.h @@ -0,0 +1,207 @@ +/****************************************************************************** + * The MIT License + * + * Copyright (c) 2010, LeafLabs, LLC. + * + * Permission is hereby granted, free of charge, to any person + * obtaining a copy of this software and associated documentation + * files (the "Software"), to deal in the Software without + * restriction, including without limitation the rights to use, copy, + * modify, merge, publish, distribute, sublicense, and/or sell copies + * of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS + * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN + * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + *****************************************************************************/ + + /* + * Arduino srl - www.arduino.org + * 2017 Feb 23: Edited by Francesco Alessi (alfran) - francesco@arduino.org + */ +#ifndef _SERVO_H_ +#define _SERVO_H_ + +#include "types.h" +#include "timer.h" + +#include "wiring.h" /* hack for IDE compile */ + +/* + * Note on Arduino compatibility: + * + * In the Arduino implementation, PWM is done "by hand" in the sense + * that timer channels are hijacked in groups and an ISR is set which + * toggles Servo::attach()ed pins using digitalWrite(). + * + * While this scheme allows any pin to drive a servo, it chews up + * cycles and complicates the programmer's notion of when a particular + * timer channel will be in use. + * + * This implementation only allows Servo instances to attach() to pins + * that already have a timer channel associated with them, and just + * uses pwmWrite() to drive the wave. + * + * This introduces an incompatibility: while the Arduino + * implementation of attach() returns the affected channel on success + * and 0 on failure, this one returns true on success and false on + * failure. + * + * RC Servos expect a pulse every 20 ms. Since periods are set for + * entire timers, rather than individual channels, attach()ing a Servo + * to a pin can interfere with other pins associated with the same + * timer. As always, your board's pin map is your friend. + */ + +// Pin number of unattached pins +#define NOT_ATTACHED (-1) + +#define _Nbr_16timers 14 // Number of STM32F469 Timers +#define SERVOS_PER_TIMER 4 // Number of timer channels + + +// Default min/max pulse widths (in microseconds) and angles (in +// degrees). Values chosen for Arduino compatibility. These values +// are part of the public API; DO NOT CHANGE THEM. +#define MIN_ANGLE 0 +#define MAX_ANGLE 180 + +#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo +#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo + +/** Class for interfacing with RC servomotors. */ +class Servo { +public: + /** + * @brief Construct a new Servo instance. + * + * The new instance will not be attached to any pin. + */ + Servo(); + + /** + * @brief Associate this instance with a servomotor whose input is + * connected to pin. + * + * If this instance is already attached to a pin, it will be + * detached before being attached to the new pin. This function + * doesn't detach any interrupt attached with the pin's timer + * channel. + * + * @param pin Pin connected to the servo pulse wave input. This + * pin must be capable of PWM output. + * + * @param minPulseWidth Minimum pulse width to write to pin, in + * microseconds. This will be associated + * with a minAngle degree angle. Defaults to + * SERVO_DEFAULT_MIN_PW = 544. + * + * @param maxPulseWidth Maximum pulse width to write to pin, in + * microseconds. This will be associated + * with a maxAngle degree angle. Defaults to + * SERVO_DEFAULT_MAX_PW = 2400. + * + * @param minAngle Target angle (in degrees) associated with + * minPulseWidth. Defaults to + * SERVO_DEFAULT_MIN_ANGLE = 0. + * + * @param maxAngle Target angle (in degrees) associated with + * maxPulseWidth. Defaults to + * SERVO_DEFAULT_MAX_ANGLE = 180. + * + * @sideeffect May set pinMode(pin, PWM). + * + * @return true if successful, false when pin doesn't support PWM. + */ + + bool attach(uint8 pin, + uint16 minPulseWidth=MIN_PULSE_WIDTH, + uint16 maxPulseWidth=MAX_PULSE_WIDTH, + int16 minAngle=MIN_ANGLE, + int16 maxAngle=MAX_ANGLE); + /** + * @brief Stop driving the servo pulse train. + * + * If not currently attached to a motor, this function has no effect. + * + * @return true if this call did anything, false otherwise. + */ + bool detach(); + + /** + * @brief Set the servomotor target angle. + * + * @param angle Target angle, in degrees. If the target angle is + * outside the range specified at attach() time, it + * will be clamped to lie in that range. + * + * @see Servo::attach() + */ + void write(int angle); + + /** + * @brief Set the pulse width, in microseconds. + * + * @param pulseWidth Pulse width to send to the servomotor, in + * microseconds. If outside of the range + * specified at attach() time, it is clamped to + * lie in that range. + * + * @see Servo::attach() + */ + void writeMicroseconds(uint16 pulseWidth); + + /** + * Get the servomotor's target angle, in degrees. This will + * lie inside the range specified at attach() time. + * + * @see Servo::attach() + */ + int read() const; + + /** + * Get the current pulse width, in microseconds. This will + * lie within the range specified at attach() time. + * + * @see Servo::attach() + */ + uint16 readMicroseconds() const; + + + /** + * @brief Check if this instance is attached to a servo. + * @return true if this instance is attached to a servo, false otherwise. + * @see Servo::attachedPin() + */ + bool attached() const { return this->pin != NOT_ATTACHED; } + + /** + * @brief Get the pin this instance is attached to. + * @return Pin number if currently attached to a pin, NOT_ATTACHED + * otherwise. + * @see Servo::attach() + */ + int attachedPin() const { return this->pin; } + +private: + int16 pin; + uint16 minPW; + uint16 maxPW; + int16 minAngle; + int16 maxAngle; + + void resetFields(void); +}; + + + +#endif /* _SERVO_H_ */ From f992cd25707d2ef81335abc9058a6cd392e48e53 Mon Sep 17 00:00:00 2001 From: peteGSX <97784652+peteGSX@users.noreply.github.com> Date: Mon, 17 Jul 2023 16:33:07 +1000 Subject: [PATCH 02/16] Work on servo test --- serial_functions.cpp | 20 ++++++++++++++++++++ test_functions.cpp | 16 +++++++++++++++- 2 files changed, 35 insertions(+), 1 deletion(-) diff --git a/serial_functions.cpp b/serial_functions.cpp index bb9b952..0ad5eb9 100644 --- a/serial_functions.cpp +++ b/serial_functions.cpp @@ -63,10 +63,23 @@ void processSerialInput() { char activity = strtokIndex[0]; // activity is our first parameter strtokIndex = strtok(NULL," "); // space is null, separator unsigned long parameter; + uint8_t vpin, profile; + uint16_t value; if (activity == 'W') { parameter = strtol(strtokIndex, NULL, 16); // last parameter is the address in hex } else if (activity == 'T') { parameter = strtokIndex[0]; + if (parameter == 'S') { + strtokIndex = strtok(NULL," "); + USB_SERIAL.println(strtokIndex); + vpin = strtoul(strtokIndex, NULL, 10); // get Vpin, this needs to be disconnected from CS + strtokIndex = strtok(NULL, " "); + USB_SERIAL.println(strtokIndex); + value = strtoul(strtokIndex, NULL, 10); // get value of the angle or dimming + strtokIndex = strtok(NULL, " "); + USB_SERIAL.println(strtokIndex); + profile = strtoul(strtokIndex, NULL, 10); // get the profile + } } else { parameter = strtol(strtokIndex, NULL, 10); } @@ -89,6 +102,13 @@ void processSerialInput() { setOutputTesting(); } else if (parameter == 'P') { setPullupTesting(); + } else if (parameter == 'S') { + USB_SERIAL.print(F("Test a servo vpin|value|profile: ")); + USB_SERIAL.print(vpin); + USB_SERIAL.print(F("|")); + USB_SERIAL.print(value); + USB_SERIAL.print(F("|")); + USB_SERIAL.println(profile); } else { serialCaseT(); } diff --git a/test_functions.cpp b/test_functions.cpp index 0dc2411..c080f90 100644 --- a/test_functions.cpp +++ b/test_functions.cpp @@ -23,7 +23,7 @@ #include "i2c_functions.h" #include "pin_io_functions.h" -bool analogueTesting = false; // Flag that analogue input testing is enabled/disabled +bool analogueTesting = false; // Flag that analogue input testing is enabled/disabled bool inputTesting = false; // Flag that digital input testing without pullups is enabled/disabled bool outputTesting = false; // Flag that digital output testing is enabled/disabled bool pullupTesting = false; // Flag that digital input testing with pullups is enabled/disabled @@ -129,4 +129,18 @@ void testPullup(bool enable) { diag = false; initialisePins(); } +} + +void testServo(bool enable) { + if (enable) { + USB_SERIAL.println(F("Servo/LED testing enabled, I2C connection disabled, diags enabled, reboot once testing complete")); + setupComplete = true; + disableWire(); + testAnalogue(false); + testOutput(false); + testInput(false); + testPullup(false); + } else { + USB_SERIAL.println(F("Disable servo testing")); + } } \ No newline at end of file From 507875b7a2fd607c9de2d1e58abc142bac99ffc9 Mon Sep 17 00:00:00 2001 From: peteGSX Date: Mon, 17 Jul 2023 19:43:27 +1000 Subject: [PATCH 03/16] Something resembling function --- {src/avr => avr}/EXIODimmer.cpp | 2 +- {src/avr => avr}/Servo.cpp | 0 {src/avr => avr}/ServoTimers.h | 0 defines.h | 3 --- {src/mbed => mbed}/Servo.cpp | 0 {src/mbed => mbed}/ServoTimers.h | 0 {src/megaavr => megaavr}/Servo.cpp | 0 {src/megaavr => megaavr}/ServoTimers.h | 0 {src/nrf52 => nrf52}/Servo.cpp | 0 {src/nrf52 => nrf52}/ServoTimers.h | 0 pin_io_functions.cpp | 6 ------ pin_io_functions.h | 1 - {src/renesas => renesas}/Servo.cpp | 0 {src/renesas => renesas}/ServoTimers.h | 0 {src/sam => sam}/Servo.cpp | 0 {src/sam => sam}/ServoTimers.h | 0 {src/samd => samd}/Servo.cpp | 0 {src/samd => samd}/ServoTimers.h | 0 serial_functions.cpp | 12 ++---------- {src/stm32f4 => stm32f4}/Servo.cpp | 0 {src/stm32f4 => stm32f4}/ServoTimers.h | 0 test_functions.cpp | 20 ++++++++++++++------ test_functions.h | 1 + 23 files changed, 18 insertions(+), 27 deletions(-) rename {src/avr => avr}/EXIODimmer.cpp (98%) rename {src/avr => avr}/Servo.cpp (100%) rename {src/avr => avr}/ServoTimers.h (100%) rename {src/mbed => mbed}/Servo.cpp (100%) rename {src/mbed => mbed}/ServoTimers.h (100%) rename {src/megaavr => megaavr}/Servo.cpp (100%) rename {src/megaavr => megaavr}/ServoTimers.h (100%) rename {src/nrf52 => nrf52}/Servo.cpp (100%) rename {src/nrf52 => nrf52}/ServoTimers.h (100%) rename {src/renesas => renesas}/Servo.cpp (100%) rename {src/renesas => renesas}/ServoTimers.h (100%) rename {src/sam => sam}/Servo.cpp (100%) rename {src/sam => sam}/ServoTimers.h (100%) rename {src/samd => samd}/Servo.cpp (100%) rename {src/samd => samd}/ServoTimers.h (100%) rename {src/stm32f4 => stm32f4}/Servo.cpp (100%) rename {src/stm32f4 => stm32f4}/ServoTimers.h (100%) diff --git a/src/avr/EXIODimmer.cpp b/avr/EXIODimmer.cpp similarity index 98% rename from src/avr/EXIODimmer.cpp rename to avr/EXIODimmer.cpp index e8ad7ae..2b57093 100644 --- a/src/avr/EXIODimmer.cpp +++ b/avr/EXIODimmer.cpp @@ -1,7 +1,7 @@ #if defined(ARDUINO_ARCH_AVR) #include -#include "../../EXIODimmer.h" +#include "EXIODimmer.h" /* Variables diff --git a/src/avr/Servo.cpp b/avr/Servo.cpp similarity index 100% rename from src/avr/Servo.cpp rename to avr/Servo.cpp diff --git a/src/avr/ServoTimers.h b/avr/ServoTimers.h similarity index 100% rename from src/avr/ServoTimers.h rename to avr/ServoTimers.h diff --git a/defines.h b/defines.h index db4e77e..6a06f24 100644 --- a/defines.h +++ b/defines.h @@ -31,7 +31,6 @@ #define BOARD_TYPE F("Pro Mini") #endif #define TOTAL_PINS 18 -// #define NUM_PWM_PINS 6 #define HAS_SERVO_LIB #define HAS_DIMMER_LIB #define HAS_EEPROM @@ -39,7 +38,6 @@ #elif defined(ARDUINO_AVR_UNO) #define BOARD_TYPE F("Uno") #define TOTAL_PINS 16 -// #define NUM_PWM_PINS 6 #define HAS_SERVO_LIB #define HAS_DIMMER_LIB #define HAS_EEPROM @@ -47,7 +45,6 @@ #elif defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_AVR_MEGA) #define BOARD_TYPE F("Mega") #define TOTAL_PINS 62 -// #define NUM_PWM_PINS 12 #define HAS_SERVO_LIB #define HAS_DIMMER_LIB #define HAS_EEPROM diff --git a/src/mbed/Servo.cpp b/mbed/Servo.cpp similarity index 100% rename from src/mbed/Servo.cpp rename to mbed/Servo.cpp diff --git a/src/mbed/ServoTimers.h b/mbed/ServoTimers.h similarity index 100% rename from src/mbed/ServoTimers.h rename to mbed/ServoTimers.h diff --git a/src/megaavr/Servo.cpp b/megaavr/Servo.cpp similarity index 100% rename from src/megaavr/Servo.cpp rename to megaavr/Servo.cpp diff --git a/src/megaavr/ServoTimers.h b/megaavr/ServoTimers.h similarity index 100% rename from src/megaavr/ServoTimers.h rename to megaavr/ServoTimers.h diff --git a/src/nrf52/Servo.cpp b/nrf52/Servo.cpp similarity index 100% rename from src/nrf52/Servo.cpp rename to nrf52/Servo.cpp diff --git a/src/nrf52/ServoTimers.h b/nrf52/ServoTimers.h similarity index 100% rename from src/nrf52/ServoTimers.h rename to nrf52/ServoTimers.h diff --git a/pin_io_functions.cpp b/pin_io_functions.cpp index f321fee..9c13366 100644 --- a/pin_io_functions.cpp +++ b/pin_io_functions.cpp @@ -250,12 +250,6 @@ bool writeAnalogue(uint8_t pin, uint16_t value, uint8_t profile, uint16_t durati } } -// void writePWM(uint8_t pin, uint16_t value) { -// if (value >= 0 && value <= 255) { -// analogWrite(pinMap[pin].physicalPin, value); -// } -// } - void processInputs() { for (uint8_t pin = 0; pin < numPins; pin++) { uint8_t pinByte = pin / 8; diff --git a/pin_io_functions.h b/pin_io_functions.h index ef37bb1..9d0942f 100644 --- a/pin_io_functions.h +++ b/pin_io_functions.h @@ -29,7 +29,6 @@ bool enableDigitalInput(uint8_t pin, bool pullup); bool writeDigitalOutput(uint8_t pin, bool state); bool enableAnalogue(uint8_t pin); bool writeAnalogue(uint8_t pin, uint16_t value, uint8_t profile, uint16_t duration); -// void writePWM(uint8_t pin, uint16_t value); void processInputs(); bool processOutputTest(bool testState); diff --git a/src/renesas/Servo.cpp b/renesas/Servo.cpp similarity index 100% rename from src/renesas/Servo.cpp rename to renesas/Servo.cpp diff --git a/src/renesas/ServoTimers.h b/renesas/ServoTimers.h similarity index 100% rename from src/renesas/ServoTimers.h rename to renesas/ServoTimers.h diff --git a/src/sam/Servo.cpp b/sam/Servo.cpp similarity index 100% rename from src/sam/Servo.cpp rename to sam/Servo.cpp diff --git a/src/sam/ServoTimers.h b/sam/ServoTimers.h similarity index 100% rename from src/sam/ServoTimers.h rename to sam/ServoTimers.h diff --git a/src/samd/Servo.cpp b/samd/Servo.cpp similarity index 100% rename from src/samd/Servo.cpp rename to samd/Servo.cpp diff --git a/src/samd/ServoTimers.h b/samd/ServoTimers.h similarity index 100% rename from src/samd/ServoTimers.h rename to samd/ServoTimers.h diff --git a/serial_functions.cpp b/serial_functions.cpp index 0ad5eb9..6aa4882 100644 --- a/serial_functions.cpp +++ b/serial_functions.cpp @@ -25,7 +25,7 @@ #include "display_functions.h" bool newSerialData = false; // Flag for new serial data being received -const byte numSerialChars = 10; // Max number of chars for serial input +const byte numSerialChars = 20; // Max number of chars for serial input char serialInputChars[numSerialChars]; // Char array for serial input /* @@ -71,13 +71,10 @@ void processSerialInput() { parameter = strtokIndex[0]; if (parameter == 'S') { strtokIndex = strtok(NULL," "); - USB_SERIAL.println(strtokIndex); vpin = strtoul(strtokIndex, NULL, 10); // get Vpin, this needs to be disconnected from CS strtokIndex = strtok(NULL, " "); - USB_SERIAL.println(strtokIndex); value = strtoul(strtokIndex, NULL, 10); // get value of the angle or dimming strtokIndex = strtok(NULL, " "); - USB_SERIAL.println(strtokIndex); profile = strtoul(strtokIndex, NULL, 10); // get the profile } } else { @@ -103,12 +100,7 @@ void processSerialInput() { } else if (parameter == 'P') { setPullupTesting(); } else if (parameter == 'S') { - USB_SERIAL.print(F("Test a servo vpin|value|profile: ")); - USB_SERIAL.print(vpin); - USB_SERIAL.print(F("|")); - USB_SERIAL.print(value); - USB_SERIAL.print(F("|")); - USB_SERIAL.println(profile); + testServo(vpin, value, profile); } else { serialCaseT(); } diff --git a/src/stm32f4/Servo.cpp b/stm32f4/Servo.cpp similarity index 100% rename from src/stm32f4/Servo.cpp rename to stm32f4/Servo.cpp diff --git a/src/stm32f4/ServoTimers.h b/stm32f4/ServoTimers.h similarity index 100% rename from src/stm32f4/ServoTimers.h rename to stm32f4/ServoTimers.h diff --git a/test_functions.cpp b/test_functions.cpp index c080f90..5b1fb97 100644 --- a/test_functions.cpp +++ b/test_functions.cpp @@ -131,16 +131,24 @@ void testPullup(bool enable) { } } -void testServo(bool enable) { - if (enable) { - USB_SERIAL.println(F("Servo/LED testing enabled, I2C connection disabled, diags enabled, reboot once testing complete")); +void testServo(uint8_t vpin, uint16_t value, uint8_t profile) { + if (firstVpin > 0) { + USB_SERIAL.println(F("EX-IOExpander has been connected and configured, please disconnect from EX-CommandStation and reboot")); + } else { + USB_SERIAL.print(F("Test move servo or dim LED - vpin|physicalPin|value|profile:")); + USB_SERIAL.print(vpin); + USB_SERIAL.print(F("|")); + USB_SERIAL.print(pinMap[vpin].physicalPin); + USB_SERIAL.print(F("|")); + USB_SERIAL.print(value); + USB_SERIAL.print(F("|")); + USB_SERIAL.println(profile); setupComplete = true; disableWire(); testAnalogue(false); testOutput(false); testInput(false); testPullup(false); - } else { - USB_SERIAL.println(F("Disable servo testing")); + writeAnalogue(vpin, value, profile, 0); } -} \ No newline at end of file +} diff --git a/test_functions.h b/test_functions.h index 823a6e6..6ba8619 100644 --- a/test_functions.h +++ b/test_functions.h @@ -27,5 +27,6 @@ void testAnalogue(bool enable); void testInput(bool enable); void testOutput(bool enable); void testPullup(bool enable); +void testServo(uint8_t vpin, uint16_t value, uint8_t profile); #endif \ No newline at end of file From 90ce252c4ceb5cfb84067bbf960d1afd215170f6 Mon Sep 17 00:00:00 2001 From: peteGSX <97784652+peteGSX@users.noreply.github.com> Date: Tue, 18 Jul 2023 09:13:25 +1000 Subject: [PATCH 04/16] Trying to get test function working --- pin_io_functions.cpp | 2 +- pin_io_functions.h | 2 +- servo_functions.cpp | 1 - test_functions.cpp | 8 +++----- version.h | 2 +- 5 files changed, 6 insertions(+), 9 deletions(-) diff --git a/pin_io_functions.cpp b/pin_io_functions.cpp index 9c13366..44c55d5 100644 --- a/pin_io_functions.cpp +++ b/pin_io_functions.cpp @@ -27,7 +27,7 @@ int digitalPinBytes = 0; // Used for configuring and sending/receiving digital int analoguePinBytes = 0; // Used for sending analogue 16 bit values byte* digitalPinStates; // Store digital pin states to send to device driver byte* analoguePinStates; // Store analogue pin states to send to device driver -unsigned long lastOutputTest = 0; +unsigned long lastOutputTest = 0; // Delay for output testing /* * Get the count of analogue and PWM capable pins diff --git a/pin_io_functions.h b/pin_io_functions.h index 9d0942f..402998f 100644 --- a/pin_io_functions.h +++ b/pin_io_functions.h @@ -28,7 +28,7 @@ void initialisePins(); bool enableDigitalInput(uint8_t pin, bool pullup); bool writeDigitalOutput(uint8_t pin, bool state); bool enableAnalogue(uint8_t pin); -bool writeAnalogue(uint8_t pin, uint16_t value, uint8_t profile, uint16_t duration); +bool writeAnalogue(uint8_t pin, uint16_t value, uint8_t profile=0, uint16_t duration=0); void processInputs(); bool processOutputTest(bool testState); diff --git a/servo_functions.cpp b/servo_functions.cpp index d6c95e4..c1c6228 100644 --- a/servo_functions.cpp +++ b/servo_functions.cpp @@ -76,7 +76,6 @@ void updatePosition(uint8_t pin) { } // Send servo command bitSet(digitalPinStates[pinByte], pinBit); - // writePWM(pin, s->currentPosition); writeServo(pin, s->currentPosition); } else if (s->stepNumber < s->numSteps + _catchupSteps) { bitSet(digitalPinStates[pinByte], pinBit); diff --git a/test_functions.cpp b/test_functions.cpp index 5b1fb97..4ba39fb 100644 --- a/test_functions.cpp +++ b/test_functions.cpp @@ -134,6 +134,8 @@ void testPullup(bool enable) { void testServo(uint8_t vpin, uint16_t value, uint8_t profile) { if (firstVpin > 0) { USB_SERIAL.println(F("EX-IOExpander has been connected and configured, please disconnect from EX-CommandStation and reboot")); + } else if (analogueTesting || inputTesting || outputTesting || pullupTesting) { + USB_SERIAL.println(F("Please disable all other testing first")); } else { USB_SERIAL.print(F("Test move servo or dim LED - vpin|physicalPin|value|profile:")); USB_SERIAL.print(vpin); @@ -145,10 +147,6 @@ void testServo(uint8_t vpin, uint16_t value, uint8_t profile) { USB_SERIAL.println(profile); setupComplete = true; disableWire(); - testAnalogue(false); - testOutput(false); - testInput(false); - testPullup(false); - writeAnalogue(vpin, value, profile, 0); + writeAnalogue(vpin, value, profile); } } diff --git a/version.h b/version.h index f83465c..658d6a4 100644 --- a/version.h +++ b/version.h @@ -2,7 +2,7 @@ #define VERSION_H // Version must only ever be numeric in order to be able to send it to the CommandStation -#define VERSION "0.0.22" +#define VERSION "0.0.23" // 0.0.22 includes: // - Add experimental support for Bluepill STM32F103C8 From acdb92f2791d28ced8f0087d4100accf1e440636 Mon Sep 17 00:00:00 2001 From: peteGSX <97784652+peteGSX@users.noreply.github.com> Date: Tue, 18 Jul 2023 13:00:12 +1000 Subject: [PATCH 05/16] Servo testing works --- test_functions.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/test_functions.cpp b/test_functions.cpp index 4ba39fb..79ffcd5 100644 --- a/test_functions.cpp +++ b/test_functions.cpp @@ -145,7 +145,9 @@ void testServo(uint8_t vpin, uint16_t value, uint8_t profile) { USB_SERIAL.print(value); USB_SERIAL.print(F("|")); USB_SERIAL.println(profile); - setupComplete = true; + if (!setupComplete) { + setupComplete = true; + } disableWire(); writeAnalogue(vpin, value, profile); } From 4d379d33f5ff4a519a6a4ffbab27596b52b1697a Mon Sep 17 00:00:00 2001 From: peteGSX Date: Wed, 26 Jul 2023 19:29:33 +1000 Subject: [PATCH 06/16] Start updates --- arduino_bluepill_f103c8.h | 4 ++-- defines.h | 9 ++++++++- display_functions.cpp | 25 +++++++++++-------------- globals.h | 1 + pin_io_functions.cpp | 35 ++++++++++++++++------------------- servo_functions.cpp | 7 ++++--- 6 files changed, 42 insertions(+), 39 deletions(-) diff --git a/arduino_bluepill_f103c8.h b/arduino_bluepill_f103c8.h index 86ec3d7..0e14245 100644 --- a/arduino_bluepill_f103c8.h +++ b/arduino_bluepill_f103c8.h @@ -29,8 +29,8 @@ pinDefinition pinMap[TOTAL_PINS] = { {PB9,DIO},{PB8,DIO},{PB5,DIOP},{PB4,DIOP},{PB3,DIOP},{PA15,DIOP}, {PA10,DIOP},{PA9,DIOP},{PA8,DIOP},{PB15,DIOP},{PB14,DIOP},{PB13,DIOP},{PB12,DIO}, }; -#define I2C_SDA PB9 -#define I2C_SCL PB8 +#define I2C_SDA PB7 +#define I2C_SCL PB6 pinName pinNameMap[TOTAL_PINS] = { {PC13,"PC13"},{PC14,"PC14"},{PC15,"PC15"},{PA0,"PA0"},{PA1,"PA1"},{PA2,"PA2"},{PA3,"PA3"},{PA4,"PA4"}, diff --git a/defines.h b/defines.h index 6a06f24..9cab1c2 100644 --- a/defines.h +++ b/defines.h @@ -33,6 +33,7 @@ #define TOTAL_PINS 18 #define HAS_SERVO_LIB #define HAS_DIMMER_LIB +#define NUM_SUPERPINS 16 #define HAS_EEPROM // Arduino Uno #elif defined(ARDUINO_AVR_UNO) @@ -40,6 +41,7 @@ #define TOTAL_PINS 16 #define HAS_SERVO_LIB #define HAS_DIMMER_LIB +#define NUM_SUPERPINS 16 #define HAS_EEPROM // Arduino Mega2560 #elif defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_AVR_MEGA) @@ -47,23 +49,28 @@ #define TOTAL_PINS 62 #define HAS_SERVO_LIB #define HAS_DIMMER_LIB +#define NUM_SUPERPINS 62 #define HAS_EEPROM #elif defined(ARDUINO_NUCLEO_F411RE) #define BOARD_TYPE F("Nucleo-F411RE") #define TOTAL_PINS 40 #define NUM_PWM_PINS 25 +#define NUM_SUPERPINS 40 #elif defined(ARDUINO_NUCLEO_F412ZG) #define BOARD_TYPE F("Nucleo-F412ZG") #define TOTAL_PINS 97 #define NUM_PWM_PINS 40 +#define NUM_SUPERPINS 97 #elif defined(ARDUINO_ARCH_SAMD) #define BOARD_TYPE F("Arduino Zero or Clone") #define TOTAL_PINS 27 #define NUM_PWM_PINS 12 +#define NUM_SUPERPINS 27 #elif defined(ARDUINO_BLUEPILL_F103C8) #define BOARD_TYPE F("BLUEPILL-STM32F103C8") #define TOTAL_PINS 28 #define NUM_PWM_PINS 19 +#define NUM_SUPERPINS 28 #else #define CPU_TYPE_ERROR #endif @@ -145,7 +152,7 @@ struct ServoData { #define SERVO_MEDIUM 0x02 // 1 second end-to-end #define SERVO_SLOW 0x03 // 2 seconds end-to-end #define SERVO_BOUNCE 0x04 // For semaphores/turnouts with a bit of bounce!! -#define USE_DIMMER 0x80 // Flag to use dimmer rather than servo (NoPowerOff in device driver) +#define USE_SUPERPIN 0x80 // Flag to use SuperPin for dimming rather than servo (NoPowerOff in device driver) ///////////////////////////////////////////////////////////////////////////////////// // Define the register hex values we need to act on or respond with diff --git a/display_functions.cpp b/display_functions.cpp index 3e04548..69798be 100644 --- a/display_functions.cpp +++ b/display_functions.cpp @@ -225,21 +225,18 @@ void startupDisplay() { } USB_SERIAL.print(F("Available at I2C address 0x")); USB_SERIAL.println(i2cAddress, HEX); - #if defined(HAS_SERVO_LIB) - USB_SERIAL.print(F("Servo library support for up to ")); - USB_SERIAL.print(MAX_SERVOS); - USB_SERIAL.println(F(" servos")); -#endif -#if defined(HAS_DIMMER_LIB) - USB_SERIAL.print(F("Dimmer library support for up to ")); - USB_SERIAL.print(MAX_DIMMERS); + if (useServoLib) { + USB_SERIAL.print(F("Servo library support for up to ")); + USB_SERIAL.print(MAX_SERVOS); + USB_SERIAL.println(F(" servos")); + } else { + USB_SERIAL.print(F("Use hardware PWM pins for up to ")); + USB_SERIAL.print(numPWMPins); + USB_SERIAL.println(F(" servos/LEDs")); + } + USB_SERIAL.print(F("SuperPin support to dim up to ")); + USB_SERIAL.print(NUM_SUPERPINS); USB_SERIAL.println(F(" LEDs")); -#endif -#if !defined(HAS_SERVO_LIB) - USB_SERIAL.print(F("Use hardware PWM pins for up to ")); - USB_SERIAL.print(numPWMPins); - USB_SERIAL.println(F(" servos/LEDs")); -#endif #if defined(DISABLE_I2C_PULLUPS) && defined(I2C_SDA) && defined(I2C_SCL) USB_SERIAL.print(F("Disabling I2C pullups on pins SDA|SCL: ")); USB_SERIAL.print(I2C_SDA); diff --git a/globals.h b/globals.h index 383f072..46733c6 100644 --- a/globals.h +++ b/globals.h @@ -52,6 +52,7 @@ extern bool analogueTesting; extern bool inputTesting; extern bool outputTesting; extern bool pullupTesting; +extern bool useServoLib; extern ServoData** servoDataArray; #if defined(HAS_SERVO_LIB) extern Servo servoMap[MAX_SERVOS]; diff --git a/pin_io_functions.cpp b/pin_io_functions.cpp index 44c55d5..21ab50d 100644 --- a/pin_io_functions.cpp +++ b/pin_io_functions.cpp @@ -56,11 +56,11 @@ void setupPinDetails() { */ void initialisePins() { for (uint8_t pin = 0; pin < numPins; pin++) { -#if defined(HAS_SERVO_LIB) - if (exioPins[pin].servoIndex != 255 && servoMap[exioPins[pin].servoIndex].attached()) { - servoMap[exioPins[pin].servoIndex].detach(); + if (useServoLib) { + if (exioPins[pin].servoIndex != 255 && servoMap[exioPins[pin].servoIndex].attached()) { + servoMap[exioPins[pin].servoIndex].detach(); + } } -#endif if (bitRead(pinMap[pin].capability, DIGITAL_INPUT) || bitRead(pinMap[pin].capability, ANALOGUE_INPUT)) { pinMode(pinMap[pin].physicalPin, INPUT); exioPins[pin].direction = 1; @@ -81,9 +81,9 @@ void initialisePins() { for (uint8_t pin = 0; pin < numPins; pin++) { servoDataArray[pin] = NULL; } -#if defined(HAS_SERVO_LIB) - nextServoObject = 0; -#endif + if (useServoLib) { + nextServoObject = 0; + } } /* @@ -185,11 +185,9 @@ bool enableAnalogue(uint8_t pin) { * Function to write PWM output to a pin */ bool writeAnalogue(uint8_t pin, uint16_t value, uint8_t profile, uint16_t duration) { -#if defined(HAS_SERVO_LIB) || defined(HAS_DIMMER_LIB) - if (bitRead(pinMap[pin].capability, DIGITAL_OUTPUT)) { -#else - if (bitRead(pinMap[pin].capability, PWM_OUTPUT)) { -#endif + bool useSuperPin = bitRead(profile, 7); // if bit 7 is set, we're using FADE, therefore use SuperPin + if (((useServoLib || useSuperPin) && bitRead(pinMap[pin].capability, DIGITAL_OUTPUT)) || + bitRead(pinMap[pin].capability, PWM_OUTPUT)) { if (exioPins[pin].enable && (exioPins[pin].direction || (exioPins[pin].mode != MODE_PWM && exioPins[pin].mode != MODE_PWM_LED))) { USB_SERIAL.print(F("ERROR! pin ")); @@ -197,13 +195,12 @@ bool writeAnalogue(uint8_t pin, uint16_t value, uint8_t profile, uint16_t durati USB_SERIAL.println(F(" already in use, cannot use as a PWM output pin")); return false; } else { - bool isLED = bitRead(profile, 7); -#if defined(HAS_SERVO_LIB) || defined(HAS_DIMMER_LIB) - if (!configureServo(pin, isLED)) return false; -#endif + if (useServoLib || useSuperPin) { + if (!configureServo(pin, useSuperPin)) return false; + } if (!exioPins[pin].enable) { exioPins[pin].enable = 1; - if (isLED) { + if (useSuperPin) { exioPins[pin].mode = MODE_PWM_LED; } else { exioPins[pin].mode = MODE_PWM; @@ -226,12 +223,12 @@ bool writeAnalogue(uint8_t pin, uint16_t value, uint8_t profile, uint16_t durati s->activePosition = 4095; s->inactivePosition = 0; s->currentPosition = value; - s->profile = SERVO_INSTANT | USE_DIMMER; // Use instant profile (but not this time) + s->profile = SERVO_INSTANT | USE_SUPERPIN; // Use instant profile (but not this time) } // Animated profile. Initiate the appropriate action. s->currentProfile = profile; - uint8_t profileValue = profile & ~USE_DIMMER; // Mask off 'don't-power-off' bit. + uint8_t profileValue = profile & ~USE_SUPERPIN; // Mask off 'don't-power-off' bit. s->numSteps = profileValue==SERVO_FAST ? 10 : // 0.5 seconds profileValue==SERVO_MEDIUM ? 20 : // 1.0 seconds profileValue==SERVO_SLOW ? 40 : // 2.0 seconds diff --git a/servo_functions.cpp b/servo_functions.cpp index c1c6228..78815a2 100644 --- a/servo_functions.cpp +++ b/servo_functions.cpp @@ -24,6 +24,9 @@ #if defined(HAS_SERVO_LIB) #include "Servo.h" uint8_t nextServoObject = 0; +bool useServoLib = 1; +#else +bool useServoLib = 0; #endif #if defined(HAS_DIMMER_LIB) #include "EXIODimmer.h" @@ -66,7 +69,7 @@ void updatePosition(uint8_t pin) { bitSet(digitalPinStates[pinByte], pinBit); // Animation in progress, reposition servo s->stepNumber++; - if ((s->currentProfile & ~USE_DIMMER) == SERVO_BOUNCE) { + if ((s->currentProfile & ~USE_SUPERPIN) == SERVO_BOUNCE) { // Retrieve step positions from array in flash uint8_t profileValue = bounceProfile[s->stepNumber]; s->currentPosition = map(profileValue, 0, 100, s->fromPosition, s->toPosition); @@ -88,7 +91,6 @@ void updatePosition(uint8_t pin) { } } -#if defined(HAS_SERVO_LIB) || defined(HAS_DIMMER_LIB) bool configureServo(uint8_t pin, bool isLED) { if (exioPins[pin].servoIndex == 255) { if (((!isLED && nextServoObject < MAX_SERVOS) || (isLED && nextDimmerObject < MAX_DIMMERS)) && bitRead(pinMap[pin].capability, DIGITAL_OUTPUT)) { @@ -114,7 +116,6 @@ bool configureServo(uint8_t pin, bool isLED) { } return true; } -#endif void writeServo(uint8_t pin, uint16_t value) { #if defined(HAS_SERVO_LIB) || defined(HAS_DIMMER_LIB) From 55be7c316b17a40a54c7baf392ea04030be5105c Mon Sep 17 00:00:00 2001 From: peteGSX Date: Thu, 27 Jul 2023 05:31:04 +1000 Subject: [PATCH 07/16] Little more progress --- servo_functions.cpp | 33 +++++++++++++++++---------------- servo_functions.h | 6 ++---- 2 files changed, 19 insertions(+), 20 deletions(-) diff --git a/servo_functions.cpp b/servo_functions.cpp index 78815a2..a6004e4 100644 --- a/servo_functions.cpp +++ b/servo_functions.cpp @@ -69,6 +69,7 @@ void updatePosition(uint8_t pin) { bitSet(digitalPinStates[pinByte], pinBit); // Animation in progress, reposition servo s->stepNumber++; + bool useSuperPin = s->currentProfile & USE_SUPERPIN; if ((s->currentProfile & ~USE_SUPERPIN) == SERVO_BOUNCE) { // Retrieve step positions from array in flash uint8_t profileValue = bounceProfile[s->stepNumber]; @@ -79,7 +80,7 @@ void updatePosition(uint8_t pin) { } // Send servo command bitSet(digitalPinStates[pinByte], pinBit); - writeServo(pin, s->currentPosition); + writeServo(pin, s->currentPosition, useSuperPin); } else if (s->stepNumber < s->numSteps + _catchupSteps) { bitSet(digitalPinStates[pinByte], pinBit); // We've finished animation, wait a little to allow servo to catch up @@ -91,10 +92,10 @@ void updatePosition(uint8_t pin) { } } -bool configureServo(uint8_t pin, bool isLED) { +bool configureServo(uint8_t pin, bool useSuperPin) { if (exioPins[pin].servoIndex == 255) { - if (((!isLED && nextServoObject < MAX_SERVOS) || (isLED && nextDimmerObject < MAX_DIMMERS)) && bitRead(pinMap[pin].capability, DIGITAL_OUTPUT)) { - if (isLED) { + if (((!useSuperPin && nextServoObject < MAX_SERVOS) || (useSuperPin && nextDimmerObject < MAX_DIMMERS)) && bitRead(pinMap[pin].capability, DIGITAL_OUTPUT)) { + if (useSuperPin) { exioPins[pin].servoIndex = nextDimmerObject; nextDimmerObject++; } else { @@ -105,7 +106,7 @@ bool configureServo(uint8_t pin, bool isLED) { return false; } } - if (isLED) { + if (useSuperPin) { if (!dimmerMap[exioPins[pin].servoIndex].attached()) { dimmerMap[exioPins[pin].servoIndex].attach(pinMap[pin].physicalPin); } @@ -117,16 +118,16 @@ bool configureServo(uint8_t pin, bool isLED) { return true; } -void writeServo(uint8_t pin, uint16_t value) { -#if defined(HAS_SERVO_LIB) || defined(HAS_DIMMER_LIB) - if (exioPins[pin].mode == MODE_PWM) { - servoMap[exioPins[pin].servoIndex].writeMicroseconds(value); - } else if (exioPins[pin].mode == MODE_PWM_LED) { - dimmerMap[exioPins[pin].servoIndex].write(value); - } -#else - if (value >= 0 && value <= 255) { - analogWrite(pinMap[pin].physicalPin, value); +void writeServo(uint8_t pin, uint16_t value, bool useSuperPin) { + if (useServoLib || useSuperPin) { + if (exioPins[pin].mode == MODE_PWM) { + servoMap[exioPins[pin].servoIndex].writeMicroseconds(value); + } else if (exioPins[pin].mode == MODE_PWM_LED) { + dimmerMap[exioPins[pin].servoIndex].write(value); + } + } else { + if (value >= 0 && value <= 255) { + analogWrite(pinMap[pin].physicalPin, value); + } } -#endif } diff --git a/servo_functions.h b/servo_functions.h index 8331aa8..3c5ca1f 100644 --- a/servo_functions.h +++ b/servo_functions.h @@ -37,9 +37,7 @@ extern uint8_t nextDimmerObject; void processServos(); void updatePosition(uint8_t pin); -void writeServo(uint8_t pin, uint16_t value); -#if defined(HAS_SERVO_LIB) || defined(HAS_DIMMER_LIB) -bool configureServo(uint8_t pin, bool isLED); -#endif +void writeServo(uint8_t pin, uint16_t value, bool useSuperPin); +bool configureServo(uint8_t pin, bool useSuperPin); #endif \ No newline at end of file From d52209e7efa4dfe9ef0c6b496106603ba5757de2 Mon Sep 17 00:00:00 2001 From: peteGSX Date: Fri, 28 Jul 2023 05:20:15 +1000 Subject: [PATCH 08/16] Update output --- test_functions.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test_functions.cpp b/test_functions.cpp index 79ffcd5..c035dd3 100644 --- a/test_functions.cpp +++ b/test_functions.cpp @@ -137,10 +137,11 @@ void testServo(uint8_t vpin, uint16_t value, uint8_t profile) { } else if (analogueTesting || inputTesting || outputTesting || pullupTesting) { USB_SERIAL.println(F("Please disable all other testing first")); } else { + String pinLabel = pinNameMap[vpin].pinLabel; USB_SERIAL.print(F("Test move servo or dim LED - vpin|physicalPin|value|profile:")); USB_SERIAL.print(vpin); USB_SERIAL.print(F("|")); - USB_SERIAL.print(pinMap[vpin].physicalPin); + USB_SERIAL.print(pinLabel); USB_SERIAL.print(F("|")); USB_SERIAL.print(value); USB_SERIAL.print(F("|")); From 6cff874d75071a7bd377addb242d94cb55185927 Mon Sep 17 00:00:00 2001 From: peteGSX <97784652+peteGSX@users.noreply.github.com> Date: Fri, 28 Jul 2023 13:21:40 +1000 Subject: [PATCH 09/16] Start adding SuperPin --- SuperPin.cpp | 89 ++++++++++++++++++++++++++++++++++++++++++++++++++++ SuperPin.h | 48 ++++++++++++++++++++++++++++ 2 files changed, 137 insertions(+) create mode 100644 SuperPin.cpp create mode 100644 SuperPin.h diff --git a/SuperPin.cpp b/SuperPin.cpp new file mode 100644 index 0000000..75a6eff --- /dev/null +++ b/SuperPin.cpp @@ -0,0 +1,89 @@ +/* + * © 2023, Chris Harlow. + * © 2023, Peter Cole. + * + * All rights reserved. + * + * This file is part of EX-IOExpander. + * + * This 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. + * + * It 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 CommandStation. If not, see . + */ + +#include "SuperPin.h" + +SuperPin* SuperPin::firstPin=NULL; + + +// create a superpin for you to set +// e.g. SuperPin p=new SuperPin(15); +// then set the pattern when required with p->setPattern(....) + +SuperPin::SuperPin(byte _pinId) { + next=firstPin; + firstPin=this; + pinId=_pinId; + onCount=0; + runningCount=255; + pinMode(_pinId, OUTPUT); + pinState=LOW; + digitalWrite(pinId,pinState); +} + +void SuperPin::setOn(byte _onCount) { + onCount=_onCount; + offCount = 255 - onCount; + runningCount=0; + pinState=LOW; +} + +void SuperPin::tick() { + if (runningCount) { + runningCount--; + return; + } + if (pinState) { + // pin is HIGH... switch to LOW unless locked + if (offCount==0) { + // pin is locked on + runningCount=onCount; + return; + } + runningCount=offCount; + pinState=LOW; + } + else { + // pin is LOW switch to HIGH unless locked + if (onCount==0) { + // pin is locked off + runningCount=offCount; + return; + } + runningCount=onCount; + pinState=HIGH; + } + digitalWrite(pinId,pinState); + runningCount--; +} + +void SuperPin::interrupt() { + for (SuperPin* p=firstPin; p; p=p->next) p->tick(); +} + +void SuperPin::start() { + + // OK... have to put some HW specific code here, taken from the CS + // to make the timer of your choice call interrupt() at the frequency + // of your choice. + // Timer1.attachInterrupt(interrupt,1); +} diff --git a/SuperPin.h b/SuperPin.h new file mode 100644 index 0000000..cc87079 --- /dev/null +++ b/SuperPin.h @@ -0,0 +1,48 @@ +/* + * © 2023, Chris Harlow. + * © 2023, Peter Cole. + * + * All rights reserved. + * + * This file is part of EX-IOExpander. + * + * This 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. + * + * It 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 CommandStation. If not, see . + */ + +#ifndef SuperPin_h +#define SuperPin_h + +#include + +// The variables all need to be marked volatile because they may be accessed by loop and interrupt code. +// The constructor and setpattern routines should really use nointerrupt() ... or there is a small risk of lost values. +// It would be worth creating a static setPattern(pinid, oncount, offcount) so the caller didn't have to remember the pointers. + +class SuperPin { + public: + static void start(); + SuperPin(byte _pinid); + void setOn(byte _onCount); + static void interrupt(); + + private: + // static void interrupt(); + void tick(); + static SuperPin* firstPin; + SuperPin* next; + volatile byte onCount, offCount, runningCount; + volatile bool pinState; + volatile byte pinId; +}; +#endif From 057be5fc5007863d6b2722e685c4b45ce2a9e657 Mon Sep 17 00:00:00 2001 From: peteGSX Date: Sat, 29 Jul 2023 19:22:56 +1000 Subject: [PATCH 10/16] Converting dimmer to SuperPin --- EXIODimmer.h | 57 ----------------- SuperPin.cpp | 132 +++++++++++++++++++++++---------------- SuperPin.h | 51 +++++++-------- arduino_avr_mega.h | 140 +++++++++++++++++++++--------------------- arduino_avr_nano.h | 40 ++++++------ arduino_avr_uno.h | 40 ++++++------ avr/EXIODimmer.cpp | 100 ------------------------------ defines.h | 17 +++-- display_functions.cpp | 2 +- globals.h | 10 +-- servo_functions.cpp | 18 +++--- servo_functions.h | 4 +- 12 files changed, 235 insertions(+), 376 deletions(-) delete mode 100644 EXIODimmer.h delete mode 100644 avr/EXIODimmer.cpp diff --git a/EXIODimmer.h b/EXIODimmer.h deleted file mode 100644 index 637c541..0000000 --- a/EXIODimmer.h +++ /dev/null @@ -1,57 +0,0 @@ -/* - * © 2023, Peter Cole. All rights reserved. - * - * This file is part of EX-IOExpander. - * - * This 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. - * - * It 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 CommandStation. If not, see . - */ - -#ifndef EXIO_DIMMER_H -#define EXIO_DIMMER_H - -#include - -#if !defined(ARDUINO_ARCH_AVR) -#error "This library only works with AVR." -#endif - -#if defined(ARDUINO_AVR_NANO) || defined(ARDUINO_AVR_PRO) || defined(ARDUINO_AVR_UNO) -#define MAX_DIMMERS 16 -#elif defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_AVR_MEGA) -#define MAX_DIMMERS 62 -#endif -#define INVALID_DIMMER 255 - -#define MIN_ON 0 -#define MAX_ON 255 - -typedef struct { - uint8_t physicalPin; - bool isActive; - uint8_t onValue; -} dimmerDefinition; - -class EXIODimmer { - public: - EXIODimmer(); - uint8_t attach(uint8_t pin); - bool attached(); - void detach(); - void write(uint8_t value); - - private: - uint8_t dimmerIndex; -}; - -#endif \ No newline at end of file diff --git a/SuperPin.cpp b/SuperPin.cpp index 75a6eff..16b01e8 100644 --- a/SuperPin.cpp +++ b/SuperPin.cpp @@ -1,8 +1,5 @@ /* - * © 2023, Chris Harlow. - * © 2023, Peter Cole. - * - * All rights reserved. + * © 2023, Peter Cole. All rights reserved. * * This file is part of EX-IOExpander. * @@ -20,70 +17,99 @@ * along with CommandStation. If not, see . */ +#include #include "SuperPin.h" -SuperPin* SuperPin::firstPin=NULL; +/* + Variables +*/ +static superPinDefinition superPins[MAX_SUPERPINS]; +uint8_t superPinCount = 0; +static volatile uint8_t counter = 0; +/* + Static functions +*/ +static inline void handle_interrupts() { + for (uint8_t i = 0; i < MAX_SUPERPINS; i++) { + if (superPins[i].isActive) { + if (counter < superPins[i].onValue) { + digitalWrite(superPins[i].physicalPin, HIGH); + } else { + digitalWrite(superPins[i].physicalPin, LOW); + } + } + } + counter++; +} -// create a superpin for you to set -// e.g. SuperPin p=new SuperPin(15); -// then set the pattern when required with p->setPattern(....) - -SuperPin::SuperPin(byte _pinId) { - next=firstPin; - firstPin=this; - pinId=_pinId; - onCount=0; - runningCount=255; - pinMode(_pinId, OUTPUT); - pinState=LOW; - digitalWrite(pinId,pinState); +SIGNAL (TIMER2_COMPA_vect) { + handle_interrupts(); } -void SuperPin::setOn(byte _onCount) { - onCount=_onCount; - offCount = 255 - onCount; - runningCount=0; - pinState=LOW; +static void initISR() { + TCCR2A = 0x00; + TCCR2B = (1<superPinIndex = superPinCount++; + } else { + this->superPinIndex = INVALID_SUPERPIN; } - else { - // pin is LOW switch to HIGH unless locked - if (onCount==0) { - // pin is locked off - runningCount=offCount; - return; +} + +uint8_t SuperPin::attach(uint8_t pin) { + if (this->superPinIndex < MAX_SUPERPINS) { + if (isTimerActive(this->superPinIndex) == false) { + initISR(); + superPins[this->superPinIndex].isActive = true; + superPins[this->superPinIndex].physicalPin = pin; + pinMode(superPins[this->superPinIndex].physicalPin, OUTPUT); } - runningCount=onCount; - pinState=HIGH; } - digitalWrite(pinId,pinState); - runningCount--; + return this->superPinIndex; } -void SuperPin::interrupt() { - for (SuperPin* p=firstPin; p; p=p->next) p->tick(); +bool SuperPin::attached() { + if (superPins[this->superPinIndex].isActive) { + return true; + } else { + return false; + } } -void SuperPin::start() { +void SuperPin::detach() { + superPins[this->superPinIndex].isActive = false; +} - // OK... have to put some HW specific code here, taken from the CS - // to make the timer of your choice call interrupt() at the frequency - // of your choice. - // Timer1.attachInterrupt(interrupt,1); +void SuperPin::write(uint8_t value) { + uint8_t channel = this->superPinIndex; + if (channel < MAX_SUPERPINS) { + if (value < MIN_ON) { + value = MIN_ON; + } else if (value > MAX_ON) { + value = MAX_ON; + } + uint8_t oldSREG = SREG; + cli(); + superPins[channel].onValue = value; + SREG = oldSREG; + } } diff --git a/SuperPin.h b/SuperPin.h index cc87079..e592890 100644 --- a/SuperPin.h +++ b/SuperPin.h @@ -1,8 +1,5 @@ /* - * © 2023, Chris Harlow. - * © 2023, Peter Cole. - * - * All rights reserved. + * © 2023, Peter Cole. All rights reserved. * * This file is part of EX-IOExpander. * @@ -20,29 +17,33 @@ * along with CommandStation. If not, see . */ -#ifndef SuperPin_h -#define SuperPin_h +#ifndef SUPERPIN_H +#define SUPERPIN_H #include +#include "defines.h" + +#define INVALID_SUPERPIN 255 + +#define MIN_ON 0 +#define MAX_ON 255 -// The variables all need to be marked volatile because they may be accessed by loop and interrupt code. -// The constructor and setpattern routines should really use nointerrupt() ... or there is a small risk of lost values. -// It would be worth creating a static setPattern(pinid, oncount, offcount) so the caller didn't have to remember the pointers. - -class SuperPin { - public: - static void start(); - SuperPin(byte _pinid); - void setOn(byte _onCount); - static void interrupt(); - - private: - // static void interrupt(); - void tick(); - static SuperPin* firstPin; - SuperPin* next; - volatile byte onCount, offCount, runningCount; - volatile bool pinState; - volatile byte pinId; +typedef struct { + uint8_t physicalPin; + bool isActive; + uint8_t onValue; +} superPinDefinition; + +class SuperPin { + public: + SuperPin(); + uint8_t attach(uint8_t pin); + bool attached(); + void detach(); + void write(uint8_t value); + + private: + uint8_t superPinIndex; }; + #endif diff --git a/arduino_avr_mega.h b/arduino_avr_mega.h index b3c3ae2..db120b5 100644 --- a/arduino_avr_mega.h +++ b/arduino_avr_mega.h @@ -100,77 +100,77 @@ Servo servoMap[MAX_SERVOS] = { servo37,servo38,servo39,servo40,servo41,servo42,servo43,servo44,servo45,servo46,servo47,servo48, }; -// Dimmer support here -EXIODimmer dimmer1; -EXIODimmer dimmer2; -EXIODimmer dimmer3; -EXIODimmer dimmer4; -EXIODimmer dimmer5; -EXIODimmer dimmer6; -EXIODimmer dimmer7; -EXIODimmer dimmer8; -EXIODimmer dimmer9; -EXIODimmer dimmer10; -EXIODimmer dimmer11; -EXIODimmer dimmer12; -EXIODimmer dimmer13; -EXIODimmer dimmer14; -EXIODimmer dimmer15; -EXIODimmer dimmer16; -EXIODimmer dimmer17; -EXIODimmer dimmer18; -EXIODimmer dimmer19; -EXIODimmer dimmer20; -EXIODimmer dimmer21; -EXIODimmer dimmer22; -EXIODimmer dimmer23; -EXIODimmer dimmer24; -EXIODimmer dimmer25; -EXIODimmer dimmer26; -EXIODimmer dimmer27; -EXIODimmer dimmer28; -EXIODimmer dimmer29; -EXIODimmer dimmer30; -EXIODimmer dimmer31; -EXIODimmer dimmer32; -EXIODimmer dimmer33; -EXIODimmer dimmer34; -EXIODimmer dimmer35; -EXIODimmer dimmer36; -EXIODimmer dimmer37; -EXIODimmer dimmer38; -EXIODimmer dimmer39; -EXIODimmer dimmer40; -EXIODimmer dimmer41; -EXIODimmer dimmer42; -EXIODimmer dimmer43; -EXIODimmer dimmer44; -EXIODimmer dimmer45; -EXIODimmer dimmer46; -EXIODimmer dimmer47; -EXIODimmer dimmer48; -EXIODimmer dimmer49; -EXIODimmer dimmer50; -EXIODimmer dimmer51; -EXIODimmer dimmer52; -EXIODimmer dimmer53; -EXIODimmer dimmer54; -EXIODimmer dimmer55; -EXIODimmer dimmer56; -EXIODimmer dimmer57; -EXIODimmer dimmer58; -EXIODimmer dimmer59; -EXIODimmer dimmer60; -EXIODimmer dimmer61; -EXIODimmer dimmer62; +// SuperPin support here +SuperPin sPin1; +SuperPin sPin2; +SuperPin sPin3; +SuperPin sPin4; +SuperPin sPin5; +SuperPin sPin6; +SuperPin sPin7; +SuperPin sPin8; +SuperPin sPin9; +SuperPin sPin10; +SuperPin sPin11; +SuperPin sPin12; +SuperPin sPin13; +SuperPin sPin14; +SuperPin sPin15; +SuperPin sPin16; +SuperPin sPin17; +SuperPin sPin18; +SuperPin sPin19; +SuperPin sPin20; +SuperPin sPin21; +SuperPin sPin22; +SuperPin sPin23; +SuperPin sPin24; +SuperPin sPin25; +SuperPin sPin26; +SuperPin sPin27; +SuperPin sPin28; +SuperPin sPin29; +SuperPin sPin30; +SuperPin sPin31; +SuperPin sPin32; +SuperPin sPin33; +SuperPin sPin34; +SuperPin sPin35; +SuperPin sPin36; +SuperPin sPin37; +SuperPin sPin38; +SuperPin sPin39; +SuperPin sPin40; +SuperPin sPin41; +SuperPin sPin42; +SuperPin sPin43; +SuperPin sPin44; +SuperPin sPin45; +SuperPin sPin46; +SuperPin sPin47; +SuperPin sPin48; +SuperPin sPin49; +SuperPin sPin50; +SuperPin sPin51; +SuperPin sPin52; +SuperPin sPin53; +SuperPin sPin54; +SuperPin sPin55; +SuperPin sPin56; +SuperPin sPin57; +SuperPin sPin58; +SuperPin sPin59; +SuperPin sPin60; +SuperPin sPin61; +SuperPin sPin62; -EXIODimmer dimmerMap[MAX_DIMMERS] = { - dimmer1,dimmer2,dimmer3,dimmer4,dimmer5,dimmer6,dimmer7,dimmer8,dimmer9,dimmer10,dimmer11,dimmer12, - dimmer13,dimmer14,dimmer15,dimmer16,dimmer17,dimmer18,dimmer19,dimmer20,dimmer21,dimmer22,dimmer23,dimmer24, - dimmer25,dimmer26,dimmer27,dimmer28,dimmer29,dimmer30,dimmer31,dimmer32,dimmer33,dimmer34,dimmer35,dimmer36, - dimmer37,dimmer38,dimmer39,dimmer40,dimmer41,dimmer42,dimmer43,dimmer44,dimmer45,dimmer46,dimmer47,dimmer48, - dimmer49,dimmer50,dimmer51,dimmer52,dimmer53,dimmer54,dimmer55,dimmer56,dimmer57,dimmer58,dimmer59,dimmer60, - dimmer61,dimmer62, +SuperPin superPinMap[MAX_SUPERPINS] = { + sPin1,sPin2,sPin3,sPin4,sPin5,sPin6,sPin7,sPin8,sPin9,sPin10,sPin11,sPin12, + sPin13,sPin14,sPin15,sPin16,sPin17,sPin18,sPin19,sPin20,sPin21,sPin22,sPin23,sPin24, + sPin25,sPin26,sPin27,sPin28,sPin29,sPin30,sPin31,sPin32,sPin33,sPin34,sPin35,sPin36, + sPin37,sPin38,sPin39,sPin40,sPin41,sPin42,sPin43,sPin44,sPin45,sPin46,sPin47,sPin48, + sPin49,sPin50,sPin51,sPin52,sPin53,sPin54,sPin55,sPin56,sPin57,sPin58,sPin59,sPin60, + sPin61,sPin62, }; #endif \ No newline at end of file diff --git a/arduino_avr_nano.h b/arduino_avr_nano.h index 9f412ca..ee77d45 100644 --- a/arduino_avr_nano.h +++ b/arduino_avr_nano.h @@ -55,27 +55,27 @@ Servo servoMap[MAX_SERVOS] = { servo1, servo2, servo3, servo4, servo5, servo6, servo7, servo8, servo9, servo10, servo11, servo12, }; -// Dimmer support here -EXIODimmer dimmer1; -EXIODimmer dimmer2; -EXIODimmer dimmer3; -EXIODimmer dimmer4; -EXIODimmer dimmer5; -EXIODimmer dimmer6; -EXIODimmer dimmer7; -EXIODimmer dimmer8; -EXIODimmer dimmer9; -EXIODimmer dimmer10; -EXIODimmer dimmer11; -EXIODimmer dimmer12; -EXIODimmer dimmer13; -EXIODimmer dimmer14; -EXIODimmer dimmer15; -EXIODimmer dimmer16; +// SuperPin support here +SuperPin sPin1; +SuperPin sPin2; +SuperPin sPin3; +SuperPin sPin4; +SuperPin sPin5; +SuperPin sPin6; +SuperPin sPin7; +SuperPin sPin8; +SuperPin sPin9; +SuperPin sPin10; +SuperPin sPin11; +SuperPin sPin12; +SuperPin sPin13; +SuperPin sPin14; +SuperPin sPin15; +SuperPin sPin16; -EXIODimmer dimmerMap[MAX_DIMMERS] = { - dimmer1,dimmer2,dimmer3,dimmer4,dimmer5,dimmer6,dimmer7,dimmer8, - dimmer9,dimmer10,dimmer11,dimmer12,dimmer13,dimmer14,dimmer15,dimmer16, +SuperPin superPinMap[MAX_SUPERPINS] = { + sPin1,sPin2,sPin3,sPin4,sPin5,sPin6,sPin7,sPin8, + sPin9,sPin10,sPin11,sPin12,sPin13,sPin14,sPin15,sPin16, }; #endif \ No newline at end of file diff --git a/arduino_avr_uno.h b/arduino_avr_uno.h index 3296e2e..84026cb 100644 --- a/arduino_avr_uno.h +++ b/arduino_avr_uno.h @@ -55,27 +55,27 @@ Servo servoMap[MAX_SERVOS] = { servo1, servo2, servo3, servo4, servo5, servo6, servo7, servo8, servo9, servo10, servo11, servo12, }; -// Dimmer support here -EXIODimmer dimmer1; -EXIODimmer dimmer2; -EXIODimmer dimmer3; -EXIODimmer dimmer4; -EXIODimmer dimmer5; -EXIODimmer dimmer6; -EXIODimmer dimmer7; -EXIODimmer dimmer8; -EXIODimmer dimmer9; -EXIODimmer dimmer10; -EXIODimmer dimmer11; -EXIODimmer dimmer12; -EXIODimmer dimmer13; -EXIODimmer dimmer14; -EXIODimmer dimmer15; -EXIODimmer dimmer16; +// SuperPin support here +SuperPin sPin1; +SuperPin sPin2; +SuperPin sPin3; +SuperPin sPin4; +SuperPin sPin5; +SuperPin sPin6; +SuperPin sPin7; +SuperPin sPin8; +SuperPin sPin9; +SuperPin sPin10; +SuperPin sPin11; +SuperPin sPin12; +SuperPin sPin13; +SuperPin sPin14; +SuperPin sPin15; +SuperPin sPin16; -EXIODimmer dimmerMap[MAX_DIMMERS] = { - dimmer1,dimmer2,dimmer3,dimmer4,dimmer5,dimmer6,dimmer7,dimmer8, - dimmer9,dimmer10,dimmer11,dimmer12,dimmer13,dimmer14,dimmer15,dimmer16, +SuperPin superPinMap[MAX_SUPERPINS] = { + sPin1,sPin2,sPin3,sPin4,sPin5,sPin6,sPin7,sPin8, + sPin9,sPin10,sPin11,sPin12,sPin13,sPin14,sPin15,sPin16, }; #endif \ No newline at end of file diff --git a/avr/EXIODimmer.cpp b/avr/EXIODimmer.cpp deleted file mode 100644 index 2b57093..0000000 --- a/avr/EXIODimmer.cpp +++ /dev/null @@ -1,100 +0,0 @@ -#if defined(ARDUINO_ARCH_AVR) - -#include -#include "EXIODimmer.h" - -/* - Variables -*/ -static dimmerDefinition dimmers[MAX_DIMMERS]; -uint8_t dimmerCount = 0; -static volatile uint8_t counter = 0; - -/* - Static functions -*/ -static inline void handle_interrupts() { - for (uint8_t i = 0; i < MAX_DIMMERS; i++) { - if (dimmers[i].isActive) { - if (counter < dimmers[i].onValue) { - digitalWrite(dimmers[i].physicalPin, HIGH); - } else { - digitalWrite(dimmers[i].physicalPin, LOW); - } - } - } - counter++; -} - -SIGNAL (TIMER2_COMPA_vect) { - handle_interrupts(); -} - -static void initISR() { - TCCR2A = 0x00; - TCCR2B = (1<dimmerIndex = dimmerCount++; - } else { - this->dimmerIndex = INVALID_DIMMER; - } -} - -uint8_t EXIODimmer::attach(uint8_t pin) { - if (this->dimmerIndex < MAX_DIMMERS) { - if (isTimerActive(this->dimmerIndex) == false) { - initISR(); - dimmers[this->dimmerIndex].isActive = true; - dimmers[this->dimmerIndex].physicalPin = pin; - pinMode(dimmers[this->dimmerIndex].physicalPin, OUTPUT); - } - } - return this->dimmerIndex; -} - -bool EXIODimmer::attached() { - if (dimmers[this->dimmerIndex].isActive) { - return true; - } else { - return false; - } -} - -void EXIODimmer::detach() { - dimmers[this->dimmerIndex].isActive = false; -} - -void EXIODimmer::write(uint8_t value) { - uint8_t channel = this->dimmerIndex; - if (channel < MAX_DIMMERS) { - if (value < MIN_ON) { - value = MIN_ON; - } else if (value > MAX_ON) { - value = MAX_ON; - } - uint8_t oldSREG = SREG; - cli(); - dimmers[channel].onValue = value; - SREG = oldSREG; - } -} - -#endif \ No newline at end of file diff --git a/defines.h b/defines.h index 9cab1c2..00a5387 100644 --- a/defines.h +++ b/defines.h @@ -32,45 +32,42 @@ #endif #define TOTAL_PINS 18 #define HAS_SERVO_LIB -#define HAS_DIMMER_LIB -#define NUM_SUPERPINS 16 +#define MAX_SUPERPINS 16 #define HAS_EEPROM // Arduino Uno #elif defined(ARDUINO_AVR_UNO) #define BOARD_TYPE F("Uno") #define TOTAL_PINS 16 #define HAS_SERVO_LIB -#define HAS_DIMMER_LIB -#define NUM_SUPERPINS 16 +#define MAX_SUPERPINS 16 #define HAS_EEPROM // Arduino Mega2560 #elif defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_AVR_MEGA) #define BOARD_TYPE F("Mega") #define TOTAL_PINS 62 #define HAS_SERVO_LIB -#define HAS_DIMMER_LIB -#define NUM_SUPERPINS 62 +#define MAX_SUPERPINS 62 #define HAS_EEPROM #elif defined(ARDUINO_NUCLEO_F411RE) #define BOARD_TYPE F("Nucleo-F411RE") #define TOTAL_PINS 40 #define NUM_PWM_PINS 25 -#define NUM_SUPERPINS 40 +#define MAX_SUPERPINS 40 #elif defined(ARDUINO_NUCLEO_F412ZG) #define BOARD_TYPE F("Nucleo-F412ZG") #define TOTAL_PINS 97 #define NUM_PWM_PINS 40 -#define NUM_SUPERPINS 97 +#define MAX_SUPERPINS 97 #elif defined(ARDUINO_ARCH_SAMD) #define BOARD_TYPE F("Arduino Zero or Clone") #define TOTAL_PINS 27 #define NUM_PWM_PINS 12 -#define NUM_SUPERPINS 27 +#define MAX_SUPERPINS 27 #elif defined(ARDUINO_BLUEPILL_F103C8) #define BOARD_TYPE F("BLUEPILL-STM32F103C8") #define TOTAL_PINS 28 #define NUM_PWM_PINS 19 -#define NUM_SUPERPINS 28 +#define MAX_SUPERPINS 28 #else #define CPU_TYPE_ERROR #endif diff --git a/display_functions.cpp b/display_functions.cpp index 69798be..653335e 100644 --- a/display_functions.cpp +++ b/display_functions.cpp @@ -235,7 +235,7 @@ void startupDisplay() { USB_SERIAL.println(F(" servos/LEDs")); } USB_SERIAL.print(F("SuperPin support to dim up to ")); - USB_SERIAL.print(NUM_SUPERPINS); + USB_SERIAL.print(MAX_SUPERPINS); USB_SERIAL.println(F(" LEDs")); #if defined(DISABLE_I2C_PULLUPS) && defined(I2C_SDA) && defined(I2C_SCL) USB_SERIAL.print(F("Disabling I2C pullups on pins SDA|SCL: ")); diff --git a/globals.h b/globals.h index 46733c6..df5c9d7 100644 --- a/globals.h +++ b/globals.h @@ -26,9 +26,7 @@ #if defined(HAS_SERVO_LIB) #include "Servo.h" #endif -#if defined(HAS_DIMMER_LIB) -#include "EXIODimmer.h" -#endif +#include "SuperPin.h" extern pinDefinition pinMap[TOTAL_PINS]; extern pinName pinNameMap[TOTAL_PINS]; @@ -57,8 +55,6 @@ extern ServoData** servoDataArray; #if defined(HAS_SERVO_LIB) extern Servo servoMap[MAX_SERVOS]; #endif -#if defined(HAS_DIMMER_LIB) -extern EXIODimmer dimmerMap[MAX_DIMMERS]; -#endif +extern SuperPin superPinMap[MAX_SUPERPINS]; -#endif \ No newline at end of file +#endif diff --git a/servo_functions.cpp b/servo_functions.cpp index a6004e4..e35c282 100644 --- a/servo_functions.cpp +++ b/servo_functions.cpp @@ -28,10 +28,8 @@ bool useServoLib = 1; #else bool useServoLib = 0; #endif -#if defined(HAS_DIMMER_LIB) -#include "EXIODimmer.h" -uint8_t nextDimmerObject = 0; -#endif +#include "SuperPin.h" +uint8_t nextSuperPinObject = 0; const unsigned int refreshInterval = 50; unsigned long lastRefresh = 0; @@ -94,10 +92,10 @@ void updatePosition(uint8_t pin) { bool configureServo(uint8_t pin, bool useSuperPin) { if (exioPins[pin].servoIndex == 255) { - if (((!useSuperPin && nextServoObject < MAX_SERVOS) || (useSuperPin && nextDimmerObject < MAX_DIMMERS)) && bitRead(pinMap[pin].capability, DIGITAL_OUTPUT)) { + if (((!useSuperPin && nextServoObject < MAX_SERVOS) || (useSuperPin && nextSuperPinObject < MAX_SUPERPINS)) && bitRead(pinMap[pin].capability, DIGITAL_OUTPUT)) { if (useSuperPin) { - exioPins[pin].servoIndex = nextDimmerObject; - nextDimmerObject++; + exioPins[pin].servoIndex = nextSuperPinObject; + nextSuperPinObject++; } else { exioPins[pin].servoIndex = nextServoObject; nextServoObject++; @@ -107,8 +105,8 @@ bool configureServo(uint8_t pin, bool useSuperPin) { } } if (useSuperPin) { - if (!dimmerMap[exioPins[pin].servoIndex].attached()) { - dimmerMap[exioPins[pin].servoIndex].attach(pinMap[pin].physicalPin); + if (!superPinMap[exioPins[pin].servoIndex].attached()) { + superPinMap[exioPins[pin].servoIndex].attach(pinMap[pin].physicalPin); } } else { if (!servoMap[exioPins[pin].servoIndex].attached()) { @@ -123,7 +121,7 @@ void writeServo(uint8_t pin, uint16_t value, bool useSuperPin) { if (exioPins[pin].mode == MODE_PWM) { servoMap[exioPins[pin].servoIndex].writeMicroseconds(value); } else if (exioPins[pin].mode == MODE_PWM_LED) { - dimmerMap[exioPins[pin].servoIndex].write(value); + superPinMap[exioPins[pin].servoIndex].write(value); } } else { if (value >= 0 && value <= 255) { diff --git a/servo_functions.h b/servo_functions.h index 3c5ca1f..3955c08 100644 --- a/servo_functions.h +++ b/servo_functions.h @@ -31,9 +31,7 @@ const uint8_t bounceProfile[30] = #if defined(HAS_SERVO_LIB) extern uint8_t nextServoObject; #endif -#if defined(HAS_DIMMER_LIB) -extern uint8_t nextDimmerObject; -#endif +extern uint8_t nextSuperPinObject; void processServos(); void updatePosition(uint8_t pin); From 0e5b350da59181e47c814b28a7154193193b06a6 Mon Sep 17 00:00:00 2001 From: peteGSX Date: Sun, 30 Jul 2023 19:18:19 +1000 Subject: [PATCH 11/16] Update SuperPin (not working) --- EX-IOExpander.ino | 1 + SuperPin.cpp | 103 +++++++++++++++++++++++++++++++++++++++++++++- SuperPin.h | 20 ++++++++- 3 files changed, 122 insertions(+), 2 deletions(-) diff --git a/EX-IOExpander.ino b/EX-IOExpander.ino index 0b1bf54..021283c 100644 --- a/EX-IOExpander.ino +++ b/EX-IOExpander.ino @@ -153,6 +153,7 @@ void loop() { processInputs(); outputTestState = processOutputTest(outputTestState); processServos(); + SuperPin::loop(); } if (diag) { displayPins(); diff --git a/SuperPin.cpp b/SuperPin.cpp index 16b01e8..8965191 100644 --- a/SuperPin.cpp +++ b/SuperPin.cpp @@ -1,5 +1,5 @@ /* - * © 2023, Peter Cole. All rights reserved. + * © 2023, Chris Harlow and Peter Cole. All rights reserved. * * This file is part of EX-IOExpander. * @@ -20,16 +20,114 @@ #include #include "SuperPin.h" +SuperPin* volatile SuperPin::firstPin=NULL; + + +// create a superpin for you to set +// e.g. SuperPin p=new SuperPin(15); +// then set the pattern when required with p->setPattern(....) + +SuperPin::SuperPin(byte _pinId) { + pinId=_pinId; + onCount=0; + offCount=255; + runningCount=255; + pinMode(_pinId, OUTPUT); + pinState=LOW; + digitalWrite(pinId,pinState); + + // chain in the new pin + noInterrupts(); + next=firstPin; + firstPin=this; + interrupts(); +} + +// Call this to set a pattern of on/off +// setPattern(25,100) low frequency PWM 20% (25 on, 100 off) +// setPattern(1,4) high frequency PWM 20% (1 on, 4 off) +// and so on... + +void SuperPin::setPattern(byte _onCount, byte _offCount) { + noInterrupts(); + onCount=_onCount; + offCount=_offCount; + runningCount=0; + pinState=LOW; + interrupts(); +} + +// Set a pin to be HIGH or LOW +void SuperPin::set(byte _pinId, bool _high) { + if (_high) setPattern(_pinId,255,0); + else setPattern(_pinId,0,255); +} + +// Set a pin to be a PWM pattern +void SuperPin::setPattern(byte _pinId, byte _onCount, byte _offCount) { + for ( SuperPin* volatile p=firstPin; p; p=p->next){ + if (p->pinId==_pinId) { + p->setPattern(_onCount, _offCount); + return; + } + } + (new SuperPin(_pinId))->setPattern(_onCount,_offCount); +} + + +void SuperPin::tick() { + if (runningCount) { + runningCount--; + return; + } + if (pinState) { + // pin is HIGH... switch to LOW unless locked + if (offCount==0) { + // pin is locked on + runningCount=onCount; + return; + } + runningCount=offCount; + pinState=LOW; + } + else { + // pin is LOW switch to HIGH unless locked + if (onCount==0) { + // pin is locked off + runningCount=offCount; + return; + } + runningCount=onCount; + pinState=HIGH; + } + digitalWrite(pinId,pinState); // repace with something faster ! + runningCount--; +} + + +// EITHER - call this loop() function at a suitable frequency from your +// sketch loop() +// OR use a timer of your choice call loop() at the frequency +// of your choice. +// e.g. Timer1.attachInterrupt(SuperPin::loop,freq); + +void SuperPin::loop() { + for (SuperPin* volatile p=firstPin; p; p=p->next) p->tick(); +} + /* Variables */ +/* static superPinDefinition superPins[MAX_SUPERPINS]; uint8_t superPinCount = 0; static volatile uint8_t counter = 0; +*/ /* Static functions */ +/* static inline void handle_interrupts() { for (uint8_t i = 0; i < MAX_SUPERPINS; i++) { if (superPins[i].isActive) { @@ -63,10 +161,12 @@ static bool isTimerActive(uint8_t channel) { return false; } } +*/ /* Constructor and functions */ +/* SuperPin::SuperPin() { if (superPinCount < MAX_SUPERPINS) { this->superPinIndex = superPinCount++; @@ -113,3 +213,4 @@ void SuperPin::write(uint8_t value) { SREG = oldSREG; } } +*/ diff --git a/SuperPin.h b/SuperPin.h index e592890..26263a1 100644 --- a/SuperPin.h +++ b/SuperPin.h @@ -1,5 +1,5 @@ /* - * © 2023, Peter Cole. All rights reserved. + * © 2023, Chris Harlow and Peter Cole. All rights reserved. * * This file is part of EX-IOExpander. * @@ -23,6 +23,23 @@ #include #include "defines.h" +class SuperPin { + public: + static void setPattern(byte pinId, byte _onCount, byte _offCount); + static void set(byte pinId, bool high); + static void loop(); + + private: + SuperPin(byte _pinid); + void setPattern(byte _onCount, byte _offCount); + void tick(); + static SuperPin* volatile firstPin; + SuperPin* volatile next; + volatile byte pinId, onCount, offCount, runningCount; + volatile bool pinState; +}; + +/* #define INVALID_SUPERPIN 255 #define MIN_ON 0 @@ -45,5 +62,6 @@ class SuperPin { private: uint8_t superPinIndex; }; +*/ #endif From 50ce2cc3954002569af11fdd3335c91d1cb826b4 Mon Sep 17 00:00:00 2001 From: peteGSX <97784652+peteGSX@users.noreply.github.com> Date: Mon, 31 Jul 2023 08:21:19 +1000 Subject: [PATCH 12/16] SuperPin working --- EX-IOExpander.ino | 2 +- SuperPin.cpp | 104 ++------------------------------------------ SuperPin.h | 25 ----------- arduino_avr_mega.h | 75 +------------------------------- arduino_avr_nano.h | 25 +---------- arduino_avr_uno.h | 25 +---------- servo_functions.cpp | 23 ++++++---- servo_functions.h | 3 +- 8 files changed, 23 insertions(+), 259 deletions(-) diff --git a/EX-IOExpander.ino b/EX-IOExpander.ino index 021283c..84736a6 100644 --- a/EX-IOExpander.ino +++ b/EX-IOExpander.ino @@ -160,4 +160,4 @@ void loop() { } processSerialInput(); processDisplayOutput(); -} \ No newline at end of file +} diff --git a/SuperPin.cpp b/SuperPin.cpp index 8965191..29653a2 100644 --- a/SuperPin.cpp +++ b/SuperPin.cpp @@ -53,7 +53,9 @@ void SuperPin::setPattern(byte _onCount, byte _offCount) { onCount=_onCount; offCount=_offCount; runningCount=0; - pinState=LOW; + // pinState=LOW; + pinState=_offCount?LOW:HIGH; + digitalWrite(pinId,pinState); interrupts(); } @@ -114,103 +116,3 @@ void SuperPin::tick() { void SuperPin::loop() { for (SuperPin* volatile p=firstPin; p; p=p->next) p->tick(); } - -/* - Variables -*/ -/* -static superPinDefinition superPins[MAX_SUPERPINS]; -uint8_t superPinCount = 0; -static volatile uint8_t counter = 0; -*/ - -/* - Static functions -*/ -/* -static inline void handle_interrupts() { - for (uint8_t i = 0; i < MAX_SUPERPINS; i++) { - if (superPins[i].isActive) { - if (counter < superPins[i].onValue) { - digitalWrite(superPins[i].physicalPin, HIGH); - } else { - digitalWrite(superPins[i].physicalPin, LOW); - } - } - } - counter++; -} - -SIGNAL (TIMER2_COMPA_vect) { - handle_interrupts(); -} - -static void initISR() { - TCCR2A = 0x00; - TCCR2B = (1<superPinIndex = superPinCount++; - } else { - this->superPinIndex = INVALID_SUPERPIN; - } -} - -uint8_t SuperPin::attach(uint8_t pin) { - if (this->superPinIndex < MAX_SUPERPINS) { - if (isTimerActive(this->superPinIndex) == false) { - initISR(); - superPins[this->superPinIndex].isActive = true; - superPins[this->superPinIndex].physicalPin = pin; - pinMode(superPins[this->superPinIndex].physicalPin, OUTPUT); - } - } - return this->superPinIndex; -} - -bool SuperPin::attached() { - if (superPins[this->superPinIndex].isActive) { - return true; - } else { - return false; - } -} - -void SuperPin::detach() { - superPins[this->superPinIndex].isActive = false; -} - -void SuperPin::write(uint8_t value) { - uint8_t channel = this->superPinIndex; - if (channel < MAX_SUPERPINS) { - if (value < MIN_ON) { - value = MIN_ON; - } else if (value > MAX_ON) { - value = MAX_ON; - } - uint8_t oldSREG = SREG; - cli(); - superPins[channel].onValue = value; - SREG = oldSREG; - } -} -*/ diff --git a/SuperPin.h b/SuperPin.h index 26263a1..62a4824 100644 --- a/SuperPin.h +++ b/SuperPin.h @@ -39,29 +39,4 @@ class SuperPin { volatile bool pinState; }; -/* -#define INVALID_SUPERPIN 255 - -#define MIN_ON 0 -#define MAX_ON 255 - -typedef struct { - uint8_t physicalPin; - bool isActive; - uint8_t onValue; -} superPinDefinition; - -class SuperPin { - public: - SuperPin(); - uint8_t attach(uint8_t pin); - bool attached(); - void detach(); - void write(uint8_t value); - - private: - uint8_t superPinIndex; -}; -*/ - #endif diff --git a/arduino_avr_mega.h b/arduino_avr_mega.h index db120b5..9f73850 100644 --- a/arduino_avr_mega.h +++ b/arduino_avr_mega.h @@ -100,77 +100,4 @@ Servo servoMap[MAX_SERVOS] = { servo37,servo38,servo39,servo40,servo41,servo42,servo43,servo44,servo45,servo46,servo47,servo48, }; -// SuperPin support here -SuperPin sPin1; -SuperPin sPin2; -SuperPin sPin3; -SuperPin sPin4; -SuperPin sPin5; -SuperPin sPin6; -SuperPin sPin7; -SuperPin sPin8; -SuperPin sPin9; -SuperPin sPin10; -SuperPin sPin11; -SuperPin sPin12; -SuperPin sPin13; -SuperPin sPin14; -SuperPin sPin15; -SuperPin sPin16; -SuperPin sPin17; -SuperPin sPin18; -SuperPin sPin19; -SuperPin sPin20; -SuperPin sPin21; -SuperPin sPin22; -SuperPin sPin23; -SuperPin sPin24; -SuperPin sPin25; -SuperPin sPin26; -SuperPin sPin27; -SuperPin sPin28; -SuperPin sPin29; -SuperPin sPin30; -SuperPin sPin31; -SuperPin sPin32; -SuperPin sPin33; -SuperPin sPin34; -SuperPin sPin35; -SuperPin sPin36; -SuperPin sPin37; -SuperPin sPin38; -SuperPin sPin39; -SuperPin sPin40; -SuperPin sPin41; -SuperPin sPin42; -SuperPin sPin43; -SuperPin sPin44; -SuperPin sPin45; -SuperPin sPin46; -SuperPin sPin47; -SuperPin sPin48; -SuperPin sPin49; -SuperPin sPin50; -SuperPin sPin51; -SuperPin sPin52; -SuperPin sPin53; -SuperPin sPin54; -SuperPin sPin55; -SuperPin sPin56; -SuperPin sPin57; -SuperPin sPin58; -SuperPin sPin59; -SuperPin sPin60; -SuperPin sPin61; -SuperPin sPin62; - -SuperPin superPinMap[MAX_SUPERPINS] = { - sPin1,sPin2,sPin3,sPin4,sPin5,sPin6,sPin7,sPin8,sPin9,sPin10,sPin11,sPin12, - sPin13,sPin14,sPin15,sPin16,sPin17,sPin18,sPin19,sPin20,sPin21,sPin22,sPin23,sPin24, - sPin25,sPin26,sPin27,sPin28,sPin29,sPin30,sPin31,sPin32,sPin33,sPin34,sPin35,sPin36, - sPin37,sPin38,sPin39,sPin40,sPin41,sPin42,sPin43,sPin44,sPin45,sPin46,sPin47,sPin48, - sPin49,sPin50,sPin51,sPin52,sPin53,sPin54,sPin55,sPin56,sPin57,sPin58,sPin59,sPin60, - sPin61,sPin62, -}; - -#endif \ No newline at end of file +#endif diff --git a/arduino_avr_nano.h b/arduino_avr_nano.h index ee77d45..7d885bb 100644 --- a/arduino_avr_nano.h +++ b/arduino_avr_nano.h @@ -55,27 +55,4 @@ Servo servoMap[MAX_SERVOS] = { servo1, servo2, servo3, servo4, servo5, servo6, servo7, servo8, servo9, servo10, servo11, servo12, }; -// SuperPin support here -SuperPin sPin1; -SuperPin sPin2; -SuperPin sPin3; -SuperPin sPin4; -SuperPin sPin5; -SuperPin sPin6; -SuperPin sPin7; -SuperPin sPin8; -SuperPin sPin9; -SuperPin sPin10; -SuperPin sPin11; -SuperPin sPin12; -SuperPin sPin13; -SuperPin sPin14; -SuperPin sPin15; -SuperPin sPin16; - -SuperPin superPinMap[MAX_SUPERPINS] = { - sPin1,sPin2,sPin3,sPin4,sPin5,sPin6,sPin7,sPin8, - sPin9,sPin10,sPin11,sPin12,sPin13,sPin14,sPin15,sPin16, -}; - -#endif \ No newline at end of file +#endif diff --git a/arduino_avr_uno.h b/arduino_avr_uno.h index 84026cb..f6998ba 100644 --- a/arduino_avr_uno.h +++ b/arduino_avr_uno.h @@ -55,27 +55,4 @@ Servo servoMap[MAX_SERVOS] = { servo1, servo2, servo3, servo4, servo5, servo6, servo7, servo8, servo9, servo10, servo11, servo12, }; -// SuperPin support here -SuperPin sPin1; -SuperPin sPin2; -SuperPin sPin3; -SuperPin sPin4; -SuperPin sPin5; -SuperPin sPin6; -SuperPin sPin7; -SuperPin sPin8; -SuperPin sPin9; -SuperPin sPin10; -SuperPin sPin11; -SuperPin sPin12; -SuperPin sPin13; -SuperPin sPin14; -SuperPin sPin15; -SuperPin sPin16; - -SuperPin superPinMap[MAX_SUPERPINS] = { - sPin1,sPin2,sPin3,sPin4,sPin5,sPin6,sPin7,sPin8, - sPin9,sPin10,sPin11,sPin12,sPin13,sPin14,sPin15,sPin16, -}; - -#endif \ No newline at end of file +#endif diff --git a/servo_functions.cpp b/servo_functions.cpp index e35c282..3bade78 100644 --- a/servo_functions.cpp +++ b/servo_functions.cpp @@ -30,6 +30,7 @@ bool useServoLib = 0; #endif #include "SuperPin.h" uint8_t nextSuperPinObject = 0; +const uint8_t superPinMax = 255; const unsigned int refreshInterval = 50; unsigned long lastRefresh = 0; @@ -104,15 +105,10 @@ bool configureServo(uint8_t pin, bool useSuperPin) { return false; } } - if (useSuperPin) { - if (!superPinMap[exioPins[pin].servoIndex].attached()) { - superPinMap[exioPins[pin].servoIndex].attach(pinMap[pin].physicalPin); - } - } else { - if (!servoMap[exioPins[pin].servoIndex].attached()) { - servoMap[exioPins[pin].servoIndex].attach(pinMap[pin].physicalPin); - } + if (!useSuperPin && !servoMap[exioPins[pin].servoIndex].attached()) { + servoMap[exioPins[pin].servoIndex].attach(pinMap[pin].physicalPin); } + // } return true; } @@ -121,7 +117,7 @@ void writeServo(uint8_t pin, uint16_t value, bool useSuperPin) { if (exioPins[pin].mode == MODE_PWM) { servoMap[exioPins[pin].servoIndex].writeMicroseconds(value); } else if (exioPins[pin].mode == MODE_PWM_LED) { - superPinMap[exioPins[pin].servoIndex].write(value); + setSuperPin(pin, value); } } else { if (value >= 0 && value <= 255) { @@ -129,3 +125,12 @@ void writeServo(uint8_t pin, uint16_t value, bool useSuperPin) { } } } + +void setSuperPin(uint8_t pin, uint16_t value) { + uint8_t physicalPin = pinMap[pin].physicalPin; + if (value > superPinMax) { + value = superPinMax; + } + uint8_t off = superPinMax - value; + SuperPin::setPattern(physicalPin, value, off); +} diff --git a/servo_functions.h b/servo_functions.h index 3955c08..5a0acb5 100644 --- a/servo_functions.h +++ b/servo_functions.h @@ -35,7 +35,8 @@ extern uint8_t nextSuperPinObject; void processServos(); void updatePosition(uint8_t pin); -void writeServo(uint8_t pin, uint16_t value, bool useSuperPin); bool configureServo(uint8_t pin, bool useSuperPin); +void writeServo(uint8_t pin, uint16_t value, bool useSuperPin); +void setSuperPin(uint8_t pin, uint16_t value); #endif \ No newline at end of file From 675680c7eb514690c5e2d7ea132d131058e3472b Mon Sep 17 00:00:00 2001 From: peteGSX <97784652+peteGSX@users.noreply.github.com> Date: Mon, 31 Jul 2023 14:55:02 +1000 Subject: [PATCH 13/16] Working --- display_functions.cpp | 18 +++++++++--------- globals.h | 1 - pin_io_functions.cpp | 20 ++++++++++++-------- servo_functions.cpp | 38 ++++++++++++++++++++------------------ version.h | 4 ++++ 5 files changed, 45 insertions(+), 36 deletions(-) diff --git a/display_functions.cpp b/display_functions.cpp index 653335e..67c759e 100644 --- a/display_functions.cpp +++ b/display_functions.cpp @@ -225,15 +225,15 @@ void startupDisplay() { } USB_SERIAL.print(F("Available at I2C address 0x")); USB_SERIAL.println(i2cAddress, HEX); - if (useServoLib) { - USB_SERIAL.print(F("Servo library support for up to ")); - USB_SERIAL.print(MAX_SERVOS); - USB_SERIAL.println(F(" servos")); - } else { - USB_SERIAL.print(F("Use hardware PWM pins for up to ")); - USB_SERIAL.print(numPWMPins); - USB_SERIAL.println(F(" servos/LEDs")); - } +#if defined(HAS_SERVO_LIB) + USB_SERIAL.print(F("Servo library support for up to ")); + USB_SERIAL.print(MAX_SERVOS); + USB_SERIAL.println(F(" servos")); +#else + USB_SERIAL.print(F("Use hardware PWM pins for up to ")); + USB_SERIAL.print(numPWMPins); + USB_SERIAL.println(F(" servos/LEDs")); +#endif USB_SERIAL.print(F("SuperPin support to dim up to ")); USB_SERIAL.print(MAX_SUPERPINS); USB_SERIAL.println(F(" LEDs")); diff --git a/globals.h b/globals.h index df5c9d7..d731d28 100644 --- a/globals.h +++ b/globals.h @@ -50,7 +50,6 @@ extern bool analogueTesting; extern bool inputTesting; extern bool outputTesting; extern bool pullupTesting; -extern bool useServoLib; extern ServoData** servoDataArray; #if defined(HAS_SERVO_LIB) extern Servo servoMap[MAX_SERVOS]; diff --git a/pin_io_functions.cpp b/pin_io_functions.cpp index 21ab50d..0a8d3e2 100644 --- a/pin_io_functions.cpp +++ b/pin_io_functions.cpp @@ -56,11 +56,11 @@ void setupPinDetails() { */ void initialisePins() { for (uint8_t pin = 0; pin < numPins; pin++) { - if (useServoLib) { - if (exioPins[pin].servoIndex != 255 && servoMap[exioPins[pin].servoIndex].attached()) { - servoMap[exioPins[pin].servoIndex].detach(); - } +#if defined(HAS_SERVO_LIB) + if (exioPins[pin].servoIndex != 255 && servoMap[exioPins[pin].servoIndex].attached()) { + servoMap[exioPins[pin].servoIndex].detach(); } +#endif if (bitRead(pinMap[pin].capability, DIGITAL_INPUT) || bitRead(pinMap[pin].capability, ANALOGUE_INPUT)) { pinMode(pinMap[pin].physicalPin, INPUT); exioPins[pin].direction = 1; @@ -81,9 +81,9 @@ void initialisePins() { for (uint8_t pin = 0; pin < numPins; pin++) { servoDataArray[pin] = NULL; } - if (useServoLib) { - nextServoObject = 0; - } +#if defined(HAS_SERVO_LIB) + nextServoObject = 0; +#endif } /* @@ -185,6 +185,10 @@ bool enableAnalogue(uint8_t pin) { * Function to write PWM output to a pin */ bool writeAnalogue(uint8_t pin, uint16_t value, uint8_t profile, uint16_t duration) { + bool useServoLib = false; +#if defined(HAS_SERVO_LIB) + useServoLib = true; +#endif bool useSuperPin = bitRead(profile, 7); // if bit 7 is set, we're using FADE, therefore use SuperPin if (((useServoLib || useSuperPin) && bitRead(pinMap[pin].capability, DIGITAL_OUTPUT)) || bitRead(pinMap[pin].capability, PWM_OUTPUT)) { @@ -241,7 +245,7 @@ bool writeAnalogue(uint8_t pin, uint16_t value, uint8_t profile, uint16_t durati } } else { USB_SERIAL.print(F("ERROR! Pin ")); - USB_SERIAL.print(pinMap[pin].physicalPin); + USB_SERIAL.print(pinNameMap[pin].pinLabel); USB_SERIAL.println(F(" not capable of PWM output")); return false; } diff --git a/servo_functions.cpp b/servo_functions.cpp index 3bade78..41ef66b 100644 --- a/servo_functions.cpp +++ b/servo_functions.cpp @@ -24,9 +24,6 @@ #if defined(HAS_SERVO_LIB) #include "Servo.h" uint8_t nextServoObject = 0; -bool useServoLib = 1; -#else -bool useServoLib = 0; #endif #include "SuperPin.h" uint8_t nextSuperPinObject = 0; @@ -93,32 +90,37 @@ void updatePosition(uint8_t pin) { bool configureServo(uint8_t pin, bool useSuperPin) { if (exioPins[pin].servoIndex == 255) { - if (((!useSuperPin && nextServoObject < MAX_SERVOS) || (useSuperPin && nextSuperPinObject < MAX_SUPERPINS)) && bitRead(pinMap[pin].capability, DIGITAL_OUTPUT)) { - if (useSuperPin) { - exioPins[pin].servoIndex = nextSuperPinObject; - nextSuperPinObject++; - } else { - exioPins[pin].servoIndex = nextServoObject; - nextServoObject++; - } + if (useSuperPin && nextSuperPinObject < MAX_SUPERPINS && bitRead(pinMap[pin].capability, DIGITAL_OUTPUT)) { + exioPins[pin].servoIndex = nextSuperPinObject; + nextSuperPinObject++; +#if defined(HAS_SERVO_LIB) + } else if (nextServoObject < MAX_SERVOS && bitRead(pinMap[pin].capability, DIGITAL_OUTPUT)) { + exioPins[pin].servoIndex = nextServoObject; + nextServoObject++; +#endif } else { return false; } } +#if defined(HAS_SERVO_LIB) if (!useSuperPin && !servoMap[exioPins[pin].servoIndex].attached()) { servoMap[exioPins[pin].servoIndex].attach(pinMap[pin].physicalPin); } - // } +#endif return true; } void writeServo(uint8_t pin, uint16_t value, bool useSuperPin) { - if (useServoLib || useSuperPin) { - if (exioPins[pin].mode == MODE_PWM) { - servoMap[exioPins[pin].servoIndex].writeMicroseconds(value); - } else if (exioPins[pin].mode == MODE_PWM_LED) { - setSuperPin(pin, value); - } + bool useServoLib = false; +#if defined(HAS_SERVO_LIB) + useServoLib = true; +#endif + if (useServoLib && exioPins[pin].mode == MODE_PWM) { +#if defined(HAS_SERVO_LIB) + servoMap[exioPins[pin].servoIndex].writeMicroseconds(value); +#endif + } else if (useSuperPin && exioPins[pin].mode == MODE_PWM_LED) { + setSuperPin(pin, value); } else { if (value >= 0 && value <= 255) { analogWrite(pinMap[pin].physicalPin, value); diff --git a/version.h b/version.h index 658d6a4..2f3bfc4 100644 --- a/version.h +++ b/version.h @@ -4,6 +4,10 @@ // Version must only ever be numeric in order to be able to send it to the CommandStation #define VERSION "0.0.23" +// 0.0.23 includes: +// - Add use of Servo library for controlling servos for supported platforms +// - Add SuperPin class for neat dimming of LEDs without needing hardware PWM pins +// - Adjust pin maps for F411RE and F412ZG for more logical Vpin to physical pin mapping // 0.0.22 includes: // - Add experimental support for Bluepill STM32F103C8 // - Enhance command to show board type, I2C address as well as Vpin map From 685070573b8313b62581ea8b65d8df2f2b5ff92a Mon Sep 17 00:00:00 2001 From: peteGSX Date: Mon, 31 Jul 2023 19:03:41 +1000 Subject: [PATCH 14/16] Update F411RE pin mapping --- arduino_nucleo_f411re.h | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/arduino_nucleo_f411re.h b/arduino_nucleo_f411re.h index b1bd273..bca4402 100644 --- a/arduino_nucleo_f411re.h +++ b/arduino_nucleo_f411re.h @@ -24,22 +24,20 @@ #include "globals.h" pinDefinition pinMap[TOTAL_PINS] = { - {PC10,DIO},{PC11,DIO},{PC12,DIO},{PD2,DIO},{PA15,DIOP},{PB7,DIOP},{PC15,DIO}, // CN7 - {PA0,AIDIOP},{PA1,AIDIOP},{PA4,AIDIO},{PB0,AIDIOP},{PC2,AIDIO},{PC1,AIDIO},{PC3,AIDIO},{PC0,AIDIO}, // CN7 - {PC9,DIOP},{PC8,DIOP},{PC6,DIOP},{PC5,AIDIO},{PA5,AIDIOP},{PA12,DIO},{PA6,AIDIOP},{PA11,DIOP},{PA7,DIOP}, // CN10 - {PB12,DIO},{PB6,DIOP},{PC7,DIOP},{PA9,DIOP},{PB2,DIO},{PA8,DIOP},{PB1,AIDIOP},{PB10,DIOP},{PB15,DIOP}, // CN10 - {PB4,DIOP},{PB14,DIOP},{PB5,DIOP},{PB13,DIOP},{PB3,DIOP},{PA10,DIOP},{PC4,AIDIO}, // CN10 + {PC10,DIO},{PC12,DIO},{PA15,DIOP},{PB7,DIOP},{PC15,DIO},{PC2,AIDIO},{PC3,AIDIO}, // CN7 outer pins + {PC11,DIO},{PD2,DIO}, {PA0,AIDIOP},{PA1,AIDIOP},{PA4,AIDIO},{PB0,AIDIOP},{PC1,AIDIO},{PC0,AIDIO}, // CN7 inner pins + {PC9,DIOP},{PA5,AIDIOP},{PA6,AIDIOP},{PA7,DIOP},{PB6,DIOP},{PC7,DIOP},{PA9,DIOP},{PA8,DIOP},{PB10,DIOP},{PB4,DIOP},{PB5,DIOP},{PB3,DIOP},{PA10,DIOP}, // CN10 inner pins + {PC8,DIOP},{PC6,DIOP},{PC5,AIDIO},{PA12,DIO},{PA11,DIOP},{PB12,DIO},{PB2,DIO},{PB1,AIDIOP},{PB15,DIOP},{PB14,DIOP},{PB13,DIOP},{PC4,AIDIO}, // CN10 outer pins }; #define I2C_SDA PB9 #define I2C_SCL PB8 pinName pinNameMap[TOTAL_PINS] = { - {PC10,"PC10"},{PC11,"PC11"},{PC12,"PC12"},{PD2,"PD2"},{PA15,"PA15"},{PB7,"PB7"},{PC15,"PC15"}, // CN7 - {PA0,"PA0"},{PA1,"PA1"},{PA4,"PA4"},{PB0,"PB0"},{PC2,"PC2"},{PC1,"PC1"},{PC3,"PC3"},{PC0,"PC0"}, // CN7 - {PC9,"PC9"},{PC8,"PC8"},{PC6,"PC6"},{PC5,"PC5"},{PA5,"PA5"},{PA12,"PA12"},{PA6,"PA6"},{PA11,"PA11"},{PA7,"PA7"}, // CN10 - {PB12,"PB12"},{PB6,"PB6"},{PC7,"PC7"},{PA9,"PA9"},{PB2,"PB2"},{PA8,"PA8"},{PB1,"PB1"},{PB10,"PB10"},{PB15,"PB15"}, // CN10 - {PB4,"PB4"},{PB14,"PB14"},{PB5,"PB5"},{PB13,"PB13"},{PB3,"PB3"},{PA10,"PA10"},{PC4,"PC4"}, // CN10 + {PC10,"PC10"},{PC12,"PC12"},{PA15,"PA15"},{PB7,"PB7"},{PC15,"PC15"},{PC2,"PC2"},{PC3,"PC3"}, // CN7 outer pins + {PC11,"PC11"},{PD2,"PD2"},{PA0,"PA0"},{PA1,"PA1"},{PA4,"PA4"},{PB0,"PB0"},{PC1,"PC1"},{PC0,"PC0"}, // CN7 inner pins + {PC9,"PC9"},{PA5,"PA5"},{PA6,"PA6"},{PA7,"PA7"},{PB6,"PB6"},{PC7,"PC7"},{PA9,"PA9"},{PA8,"PA8"},{PB10,"PB10"},{PB4,"PB4"},{PB5,"PB5"},{PB3,"PB3"},{PA10,"PA10"}, // CN10 inner pins + {PC8,"PC8"},{PC6,"PC6"},{PC5,"PC5"},{PA12,"PA12"},{PA11,"PA11"},{PB12,"PB12"},{PB2,"PB2"},{PB1,"PB1"},{PB15,"PB15"},{PB14,"PB14"},{PB13,"PB13"},{PC4,"PC4"}, // CN10 outer pins }; /* From 7704058b2496228bf79d4d782288189f92e462c3 Mon Sep 17 00:00:00 2001 From: peteGSX Date: Mon, 31 Jul 2023 19:48:03 +1000 Subject: [PATCH 15/16] Update F412ZG pin map --- arduino_nucleo_f412zg.h | 38 ++++++++++++++++---------------------- 1 file changed, 16 insertions(+), 22 deletions(-) diff --git a/arduino_nucleo_f412zg.h b/arduino_nucleo_f412zg.h index 4b2d9ca..6fb02bd 100644 --- a/arduino_nucleo_f412zg.h +++ b/arduino_nucleo_f412zg.h @@ -24,34 +24,28 @@ #include "globals.h" pinDefinition pinMap[TOTAL_PINS] = { - {PC10,DIO},{PC11,DIO},{PC12,DIO},{PD2,DIO},{PF6,DIOP},{PF7,DIOP},{PA15,DIOP},{PB7,DIOP},{PC13,DIO}, // CN11 - {PA0,AIDIOP},{PA1,AIDIOP},{PA4,AIDIO},{PB0,AIDIOP},{PC2,AIDIO},{PC1,AIDIO},{PC3,AIDIO},{PC0,AIDIO}, // CN11 - {PD4,DIO},{PD3,DIO},{PD5,DIO},{PG2,DIO},{PD6,DIO},{PG3,DIO},{PD7,DIO},{PE2,DIO},{PE3,DIO},{PE4,DIO}, // CN11 - {PE5,DIOP},{PF1,DIO},{PF2,DIO},{PF0,DIO},{PF8,DIOP},{PD1,DIO},{PF9,DIOP},{PD0,DIO},{PG1,DIO},{PG0,DIO}, // CN11 - {PE1,DIO},{PE6,DIOP},{PG9,DIO},{PG15,DIO},{PG12,DIO},{PG10,DIO},{PG13,DIO},{PG11,DIO}, // CN11 - {PC9,DIOP},{PC8,DIOP},{PC6,DIOP},{PC5,AIDIO},{PA5,AIDIOP},{PA6,AIDIOP},{PA7,AIDIOP},{PB12,DIO},{PB6,DIOP}, // CN12 - {PB11,DIOP},{PC7,DIOP},{PB2,DIO},{PB1,AIDIOP},{PB10,DIOP},{PB15,DIOP},{PB4,DIOP},{PB14,DIOP},{PB5,DIOP}, // CN12 - {PB13,DIOP},{PB3,DIOP},{PC4,AIDIO},{PA2,AIDIOP},{PF5,DIO},{PA3,AIDIOP},{PF4,DIO},{PE8,DIOP},{PD13,DIOP}, // CN12 - {PF10,DIO},{PD12,DIOP},{PE7,DIO},{PD11,DIO},{PD14,DIOP},{PE10,DIOP},{PD15,DIOP},{PE12,DIOP},{PF14,DIO}, // CN12 - {PE14,DIOP},{PE9,DIOP},{PE15,DIO},{PE13,DIOP},{PE11,DIOP},{PF13,DIO},{PF3,DIO},{PF12,DIO},{PF15,DIO}, // CN12 - {PG14,DIO},{PF11,DIO},{PE0,DIO},{PD10,DIO},{PG8,DIO},{PG5,DIO},{PG4,DIO}, // CN12 + {PC10,DIO},{PC12,DIO},{PF6,DIOP},{PF7,DIOP},{PA15,DIOP},{PB7,DIOP},{PC13,DIO},{PC2,AIDIO},{PC3,AIDIO},{PD4,DIO},{PD5,DIO},{PD6,DIO},{PD7,DIO},{PE3,DIO}, + {PF1,DIO},{PF0,DIO},{PD1,DIO},{PD0,DIO},{PG0,DIO},{PE1,DIO},{PG9,DIO},{PG12,DIO}, // CN11 outer pins + {PC11,DIO},{PD2,DIO},{PA0,AIDIOP},{PA1,AIDIOP},{PA4,AIDIO},{PB0,AIDIOP},{PC1,AIDIO},{PC0,AIDIO},{PD3,DIO},{PG2,DIO},{PG3,DIO},{PE2,DIO},{PE4,DIO}, + {PE5,DIOP},{PF2,DIO},{PF8,DIOP},{PF9,DIOP},{PG1,DIO},{PE6,DIOP},{PG15,DIO},{PG10,DIO},{PG13,DIO},{PG11,DIO}, //CN11 inner pins + {PC9,DIOP},{PA5,AIDIOP},{PA6,AIDIOP},{PA7,AIDIOP},{PB6,DIOP},{PC7,DIOP},{PB10,DIOP},{PB4,DIOP},{PB5,DIOP},{PB3,DIOP},{PA2,AIDIOP},{PA3,AIDIOP},{PD13,DIOP}, + {PD12,DIOP},{PD11,DIO},{PE10,DIOP},{PE12,DIOP},{PE14,DIOP},{PE15,DIO},{PE13,DIOP},{PF13,DIO},{PF12,DIO},{PG14,DIO},{PD10,DIO},{PG4,DIO}, // CN12 inner pins + {PC8,DIOP},{PC6,DIOP},{PC5,AIDIO},{PB12,DIO},{PB11,DIOP},{PB2,DIO},{PB1,AIDIOP},{PB15,DIOP},{PB14,DIOP},{PB13,DIOP},{PC4,AIDIO},{PF5,DIO},{PF4,DIO},{PE8,DIOP}, + {PF10,DIO},{PE7,DIO},{PD14,DIOP},{PD15,DIOP},{PF14,DIO},{PE9,DIOP},{PE11,DIOP},{PF3,DIO},{PF15,DIO},{PF11,DIO},{PE0,DIO},{PG8,DIO},{PG5,DIO}, // CN12 outer pins }; #define I2C_SDA PB9 #define I2C_SCL PB8 pinName pinNameMap[TOTAL_PINS] = { - {PC10,"PC10"},{PC11,"PC11"},{PC12,"PC12"},{PD2,"PD2"},{PF6,"PF6"},{PF7,"PF7"},{PA15,"PA15"},{PB7,"PB7"}, // CN11 - {PC13,"PC13"},{PA0,"PA0"},{PA1,"PA1"},{PA4,"PA4"},{PB0,"PB0"},{PC2,"PC2"},{PC1,"PC1"},{PC3,"PC3"},{PC0,"PC0"}, // CN11 - {PD4,"PD4"},{PD3,"PD3"},{PD5,"PD5"},{PG2,"PG2"},{PD6,"PD6"},{PG3,"PG3"},{PD7,"PD7"},{PE2,"PE2"},{PE3,"PE3"},{PE4,"PE4"}, // CN11 - {PE5,"PE5"},{PF1,"PF1"},{PF2,"PF2"},{PF0,"PF0"},{PF8,"PF8"},{PD1,"PD1"},{PF9,"PF9"},{PD0,"PD0"},{PG1,"PG1"},{PG0,"PG0"}, // CN11 - {PE1,"PE1"},{PE6,"PE6"},{PG9,"PG9"},{PG15,"PG15"},{PG12,"PG12"},{PG10,"PG10"},{PG13,"PG13"},{PG11,"PG11"}, // CN11 - {PC9,"PC9"},{PC8,"PC8"},{PC6,"PC6"},{PC5,"PC5"},{PA5,"PA5"},{PA6,"PA6"},{PA7,"PA7"},{PB12,"PB12"},{PB6,"PB6"}, // CN12 - {PB11,"PB11"},{PC7,"PC7"},{PB2,"PB2"},{PB1,"PB1"},{PB10,"PB10"},{PB15,"PB15"},{PB4,"PB4"},{PB14,"PB14"},{PB5,"PB5"}, // CN12 - {PB13,"PB13"},{PB3,"PB3"},{PC4,"PC4"},{PA2,"PA2"},{PF5,"PF5"},{PA3,"PA3"},{PF4,"PF4"},{PE8,"PE8"},{PD13,"PD13"}, // CN12 - {PF10,"PF10"},{PD12,"PD12"},{PE7,"PE7"},{PD11,"PD11"},{PD14,"PD14"},{PE10,"PE10"},{PD15,"PD15"},{PE12,"PE12"},{PF14,"PF14"}, // CN12 - {PE14,"PE14"},{PE9,"PE9"},{PE15,"PE15"},{PE13,"PE13"},{PE11,"PE11"},{PF13,"PF13"},{PF3,"PF3"},{PF12,"PF12"},{PF15,"PF15"}, // CN12 - {PG14,"PG14"},{PF11,"PF11"},{PE0,"PE0"},{PD10,"PD10"},{PG8,"PG8"},{PG5,"PG5"},{PG4,"PG4"}, // CN12 + {PC10,"PC10"},{PC12,"PC12"},{PF6,"PF6"},{PF7,"PF7"},{PA15,"PA15"},{PB7,"PB7"},{PC13,"PC13"},{PC2,"PC2"},{PC3,"PC3"},{PD4,"PD4"},{PD5,"PD5"},{PD6,"PD6"},{PD7,"PD7"},{PE3,"PE3"}, + {PF1,"PF1"},{PF0,"PF0"},{PD1,"PD1"},{PD0,"PD0"},{PG0,"PG0"},{PE1,"PE1"},{PG9,"PG9"},{PG12,"PG12"}, // CN11 outer pins + {PC11,"PC11"},{PD2,"PD2"},{PA0,"PA0"},{PA1,"PA1"},{PA4,"PA4"},{PB0,"PB0"},{PC1,"PC1"},{PC0,"PC0"},{PD3,"PD3"},{PG2,"PG2"},{PG3,"PG3"},{PE2,"PE2"},{PE4,"PE4"}, + {PE5,"PE5"},{PF2,"PF2"},{PF8,"PF8"},{PF9,"PF9"},{PG1,"PG1"},{PE6,"PE6"},{PG15,"PG15"},{PG10,"PG10"},{PG13,"PG13"},{PG11,"PG11"}, // CN11 inner pins + {PC9,"PC9"},{PA5,"PA5"},{PA6,"PA6"},{PA7,"PA7"},{PB6,"PB6"},{PC7,"PC7"},{PB10,"PB10"},{PB4,"PB4"},{PB5,"PB5"},{PB3,"PB3"},{PA2,"PA2"},{PA3,"PA3"},{PD13,"PD13"}, + {PD12,"PD12"},{PD11,"PD11"},{PE10,"PE10"},{PE12,"PE12"},{PE14,"PE14"},{PE15,"PE15"},{PE13,"PE13"},{PF13,"PF13"},{PF12,"PF12"},{PG14,"PG14"},{PD10,"PD10"},{PG4,"PG4"}, // CN12 inner pins + {PC8,"PC8"},{PC6,"PC6"},{PC5,"PC5"},{PB12,"PB12"},{PB11,"PB11"},{PB2,"PB2"},{PB1,"PB1"},{PB15,"PB15"},{PB14,"PB14"},{PB13,"PB13"},{PC4,"PC4"},{PF5,"PF5"},{PF4,"PF4"},{PE8,"PE8"}, + {PF10,"PF10"},{PE7,"PE7"},{PD14,"PD14"},{PD15,"PD15"},{PF14,"PF14"},{PE9,"PE9"},{PE11,"PE11"},{PF3,"PF3"},{PF15,"PF15"},{PF11,"PF11"},{PE0,"PE0"},{PG8,"PG8"},{PG5,"PG5"}, // CN12 outer pins }; /* From b67e8af9f50ceb537f1a894e363ee51b3d22a084 Mon Sep 17 00:00:00 2001 From: peteGSX <97784652+peteGSX@users.noreply.github.com> Date: Tue, 1 Aug 2023 11:15:53 +1000 Subject: [PATCH 16/16] Add fast digital writes for AVR --- SuperPin.cpp | 128 ++++++++++++++++++++++++------------------- SuperPin.h | 27 ++++----- defines.h | 3 + pin_io_functions.cpp | 2 +- version.h | 1 + 5 files changed, 92 insertions(+), 69 deletions(-) diff --git a/SuperPin.cpp b/SuperPin.cpp index 29653a2..e0c287b 100644 --- a/SuperPin.cpp +++ b/SuperPin.cpp @@ -28,19 +28,19 @@ SuperPin* volatile SuperPin::firstPin=NULL; // then set the pattern when required with p->setPattern(....) SuperPin::SuperPin(byte _pinId) { - pinId=_pinId; - onCount=0; - offCount=255; - runningCount=255; - pinMode(_pinId, OUTPUT); - pinState=LOW; - digitalWrite(pinId,pinState); - - // chain in the new pin - noInterrupts(); - next=firstPin; - firstPin=this; - interrupts(); + pinId=_pinId; + onCount=0; + offCount=255; + runningCount=255; + pinMode(_pinId, OUTPUT); + pinState=LOW; + digitalWrite(pinId,pinState); + + // chain in the new pin + noInterrupts(); + next=firstPin; + firstPin=this; + interrupts(); } // Call this to set a pattern of on/off @@ -49,61 +49,61 @@ SuperPin::SuperPin(byte _pinId) { // and so on... void SuperPin::setPattern(byte _onCount, byte _offCount) { - noInterrupts(); - onCount=_onCount; - offCount=_offCount; - runningCount=0; - // pinState=LOW; - pinState=_offCount?LOW:HIGH; - digitalWrite(pinId,pinState); - interrupts(); + noInterrupts(); + onCount=_onCount; + offCount=_offCount; + runningCount=0; + // pinState=LOW; + pinState=_offCount?LOW:HIGH; + fastDigitalWrite(pinId, pinState); + interrupts(); } // Set a pin to be HIGH or LOW void SuperPin::set(byte _pinId, bool _high) { - if (_high) setPattern(_pinId,255,0); - else setPattern(_pinId,0,255); + if (_high) setPattern(_pinId,255,0); + else setPattern(_pinId,0,255); } // Set a pin to be a PWM pattern void SuperPin::setPattern(byte _pinId, byte _onCount, byte _offCount) { - for ( SuperPin* volatile p=firstPin; p; p=p->next){ - if (p->pinId==_pinId) { - p->setPattern(_onCount, _offCount); - return; - } - } - (new SuperPin(_pinId))->setPattern(_onCount,_offCount); + for ( SuperPin* volatile p=firstPin; p; p=p->next){ + if (p->pinId==_pinId) { + p->setPattern(_onCount, _offCount); + return; + } + } + (new SuperPin(_pinId))->setPattern(_onCount,_offCount); } void SuperPin::tick() { - if (runningCount) { - runningCount--; - return; - } - if (pinState) { - // pin is HIGH... switch to LOW unless locked - if (offCount==0) { - // pin is locked on - runningCount=onCount; - return; - } - runningCount=offCount; - pinState=LOW; + if (runningCount) { + runningCount--; + return; + } + if (pinState) { + // pin is HIGH... switch to LOW unless locked + if (offCount==0) { + // pin is locked on + runningCount=onCount; + return; } - else { - // pin is LOW switch to HIGH unless locked - if (onCount==0) { - // pin is locked off - runningCount=offCount; - return; - } - runningCount=onCount; - pinState=HIGH; + runningCount=offCount; + pinState=LOW; + } + else { + // pin is LOW switch to HIGH unless locked + if (onCount==0) { + // pin is locked off + runningCount=offCount; + return; } - digitalWrite(pinId,pinState); // repace with something faster ! - runningCount--; + runningCount=onCount; + pinState=HIGH; + } + fastDigitalWrite(pinId, pinState); + runningCount--; } @@ -114,5 +114,23 @@ void SuperPin::tick() { // e.g. Timer1.attachInterrupt(SuperPin::loop,freq); void SuperPin::loop() { - for (SuperPin* volatile p=firstPin; p; p=p->next) p->tick(); + for (SuperPin* volatile p=firstPin; p; p=p->next) p->tick(); +} + +// Fast digital writes stolen from EX-CommandStation +void SuperPin::fastDigitalWrite(uint8_t pin, bool state) { +#ifdef USE_FAST_WRITES + if (pin >= NUM_DIGITAL_PINS) return; + uint8_t mask = digitalPinToBitMask(pin); + uint8_t port = digitalPinToPort(pin); + volatile uint8_t *outPortAdr = portOutputRegister(port); + noInterrupts(); + if (state) + *outPortAdr |= mask; + else + *outPortAdr &= ~mask; + interrupts(); +#else + digitalWrite(pin, state); +#endif } diff --git a/SuperPin.h b/SuperPin.h index 62a4824..92e2bb3 100644 --- a/SuperPin.h +++ b/SuperPin.h @@ -24,19 +24,20 @@ #include "defines.h" class SuperPin { - public: - static void setPattern(byte pinId, byte _onCount, byte _offCount); - static void set(byte pinId, bool high); - static void loop(); - - private: - SuperPin(byte _pinid); - void setPattern(byte _onCount, byte _offCount); - void tick(); - static SuperPin* volatile firstPin; - SuperPin* volatile next; - volatile byte pinId, onCount, offCount, runningCount; - volatile bool pinState; + public: + static void setPattern(byte pinId, byte _onCount, byte _offCount); + static void set(byte pinId, bool high); + static void loop(); + + private: + SuperPin(byte _pinid); + void setPattern(byte _onCount, byte _offCount); + void tick(); + static SuperPin* volatile firstPin; + SuperPin* volatile next; + volatile byte pinId, onCount, offCount, runningCount; + volatile bool pinState; + static void fastDigitalWrite(uint8_t pin, bool state); }; #endif diff --git a/defines.h b/defines.h index 00a5387..3fbc1d3 100644 --- a/defines.h +++ b/defines.h @@ -34,6 +34,7 @@ #define HAS_SERVO_LIB #define MAX_SUPERPINS 16 #define HAS_EEPROM +#define USE_FAST_WRITES // Arduino Uno #elif defined(ARDUINO_AVR_UNO) #define BOARD_TYPE F("Uno") @@ -41,6 +42,7 @@ #define HAS_SERVO_LIB #define MAX_SUPERPINS 16 #define HAS_EEPROM +#define USE_FAST_WRITES // Arduino Mega2560 #elif defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_AVR_MEGA) #define BOARD_TYPE F("Mega") @@ -48,6 +50,7 @@ #define HAS_SERVO_LIB #define MAX_SUPERPINS 62 #define HAS_EEPROM +#define USE_FAST_WRITES #elif defined(ARDUINO_NUCLEO_F411RE) #define BOARD_TYPE F("Nucleo-F411RE") #define TOTAL_PINS 40 diff --git a/pin_io_functions.cpp b/pin_io_functions.cpp index 0a8d3e2..56ba2cd 100644 --- a/pin_io_functions.cpp +++ b/pin_io_functions.cpp @@ -310,4 +310,4 @@ bool processOutputTest(bool testState) { } } return testState; -} \ No newline at end of file +} diff --git a/version.h b/version.h index 2f3bfc4..3df9346 100644 --- a/version.h +++ b/version.h @@ -8,6 +8,7 @@ // - Add use of Servo library for controlling servos for supported platforms // - Add SuperPin class for neat dimming of LEDs without needing hardware PWM pins // - Adjust pin maps for F411RE and F412ZG for more logical Vpin to physical pin mapping +// - Enable fast digital writes for SuperPin on AVR platforms // 0.0.22 includes: // - Add experimental support for Bluepill STM32F103C8 // - Enhance command to show board type, I2C address as well as Vpin map