├── docs └── index.md ├── lib └── .DS_Store ├── src ├── PID_v1.o ├── Encoder.cpp ├── .gitattributes ├── PID_v1.d ├── WProgram.h ├── .gitignore ├── PinStates.h ├── utility │ ├── direct_pin_read.h │ ├── interrupt_config.h │ └── interrupt_pins.h ├── Event.h ├── RunningAverage.h ├── Event.cpp ├── PID_v1.h ├── wiring.h ├── RunningAverage.cpp ├── PID_v1.cpp ├── motor_control_and_plot.ino └── Encoder.h ├── examples └── slider │ ├── src │ ├── PID_v1.o │ ├── PID_v1.d │ ├── WProgram.h │ ├── PID_v1.h │ ├── wiring.h │ ├── PID_v1.cpp │ └── motor.ino │ ├── platformio.ini │ └── lib │ └── readme.txt ├── platformio.ini ├── platformio.ini~ ├── serial_n_plot.py └── README.md /docs/index.md: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /lib/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/owhite/Arduino-PID-control/HEAD/lib/.DS_Store -------------------------------------------------------------------------------- /src/PID_v1.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/owhite/Arduino-PID-control/HEAD/src/PID_v1.o -------------------------------------------------------------------------------- /examples/slider/src/PID_v1.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/owhite/Arduino-PID-control/HEAD/examples/slider/src/PID_v1.o -------------------------------------------------------------------------------- /src/Encoder.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "Encoder.h" 3 | 4 | // Yes, all the code is in the header file, to provide the user 5 | // configure options with #define (before they include it), and 6 | // to facilitate some crafty optimizations! 7 | 8 | Encoder_internal_state_t * Encoder::interruptArgs[]; 9 | 10 | 11 | -------------------------------------------------------------------------------- /examples/slider/platformio.ini: -------------------------------------------------------------------------------- 1 | # 2 | # Simple and base environment 3 | 4 | [env:autogen_uno] 5 | platform = atmelavr 6 | framework = arduino 7 | board = uno 8 | 9 | 10 | # Automatic targets - enable auto-uploading 11 | targets = upload 12 | 13 | [platformio] 14 | lib_dir = ~/Documents/Arduino/libraries 15 | -------------------------------------------------------------------------------- /src/.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | 7 | # Standard to msysgit 8 | *.doc diff=astextplain 9 | *.DOC diff=astextplain 10 | *.docx diff=astextplain 11 | *.DOCX diff=astextplain 12 | *.dot diff=astextplain 13 | *.DOT diff=astextplain 14 | *.pdf diff=astextplain 15 | *.PDF diff=astextplain 16 | *.rtf diff=astextplain 17 | *.RTF diff=astextplain 18 | -------------------------------------------------------------------------------- /platformio.ini: -------------------------------------------------------------------------------- 1 | # 2 | # Project Configuration File 3 | # 4 | # A detailed documentation with the EXAMPLES is located here: 5 | # http://docs.platformio.org/en/latest/projectconf.html 6 | # 7 | 8 | # A sign `#` at the beginning of the line indicates a comment 9 | # Comment lines are ignored. 10 | 11 | # Simple and base environment 12 | # [env:mybaseenv] 13 | # platform = %INSTALLED_PLATFORM_NAME_HERE% 14 | # framework = 15 | # board = 16 | # 17 | # Automatic targets - enable auto-uploading 18 | # targets = upload 19 | 20 | [env:teensy31] 21 | platform = teensy 22 | framework = arduino 23 | board = teensy31 24 | targets = upload 25 | -------------------------------------------------------------------------------- /platformio.ini~: -------------------------------------------------------------------------------- 1 | # 2 | # Project Configuration File 3 | # 4 | # A detailed documentation with the EXAMPLES is located here: 5 | # http://docs.platformio.org/en/latest/projectconf.html 6 | # 7 | 8 | # A sign `#` at the beginning of the line indicates a comment 9 | # Comment lines are ignored. 10 | 11 | # Simple and base environment 12 | # [env:mybaseenv] 13 | # platform = %INSTALLED_PLATFORM_NAME_HERE% 14 | # framework = 15 | # board = 16 | # 17 | # Automatic targets - enable auto-uploading 18 | # targets = upload 19 | 20 | [env:teensy31] 21 | platform = teensy 22 | framework = arduino 23 | board = teensy31 24 | targets = upload 25 | -------------------------------------------------------------------------------- /src/PID_v1.d: -------------------------------------------------------------------------------- 1 | .pioenvs/uno/src/PID_v1.o: .pioenvs/uno/src/PID_v1.cpp \ 2 | .pioenvs/uno/FrameworkArduino/Arduino.h \ 3 | .pioenvs/uno/FrameworkArduino/binary.h \ 4 | .pioenvs/uno/FrameworkArduino/WCharacter.h \ 5 | .pioenvs/uno/FrameworkArduino/WString.h \ 6 | .pioenvs/uno/FrameworkArduino/HardwareSerial.h \ 7 | .pioenvs/uno/FrameworkArduino/Stream.h \ 8 | .pioenvs/uno/FrameworkArduino/Print.h \ 9 | .pioenvs/uno/FrameworkArduino/Printable.h \ 10 | .pioenvs/uno/FrameworkArduino/USBAPI.h \ 11 | .pioenvs/uno/FrameworkArduino/Arduino.h \ 12 | .pioenvs/uno/FrameworkArduinoVariant/pins_arduino.h \ 13 | .pioenvs/uno/src/PID_v1.h 14 | -------------------------------------------------------------------------------- /examples/slider/src/PID_v1.d: -------------------------------------------------------------------------------- 1 | .pioenvs/uno/src/PID_v1.o: .pioenvs/uno/src/PID_v1.cpp \ 2 | .pioenvs/uno/FrameworkArduino/Arduino.h \ 3 | .pioenvs/uno/FrameworkArduino/binary.h \ 4 | .pioenvs/uno/FrameworkArduino/WCharacter.h \ 5 | .pioenvs/uno/FrameworkArduino/WString.h \ 6 | .pioenvs/uno/FrameworkArduino/HardwareSerial.h \ 7 | .pioenvs/uno/FrameworkArduino/Stream.h \ 8 | .pioenvs/uno/FrameworkArduino/Print.h \ 9 | .pioenvs/uno/FrameworkArduino/Printable.h \ 10 | .pioenvs/uno/FrameworkArduino/USBAPI.h \ 11 | .pioenvs/uno/FrameworkArduino/Arduino.h \ 12 | .pioenvs/uno/FrameworkArduinoVariant/pins_arduino.h \ 13 | .pioenvs/uno/src/PID_v1.h 14 | -------------------------------------------------------------------------------- /src/WProgram.h: -------------------------------------------------------------------------------- 1 | #ifndef WProgram_h 2 | #define WProgram_h 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "wiring.h" 11 | 12 | #ifdef __cplusplus 13 | #include "HardwareSerial.h" 14 | 15 | uint16_t makeWord(uint16_t w); 16 | uint16_t makeWord(byte h, byte l); 17 | 18 | #define word(...) makeWord(__VA_ARGS__) 19 | 20 | unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout = 1000000L); 21 | 22 | // WMath prototypes 23 | long random(long); 24 | long random(long, long); 25 | void randomSeed(unsigned int); 26 | long map(long, long, long, long, long); 27 | #endif 28 | 29 | #endif -------------------------------------------------------------------------------- /examples/slider/src/WProgram.h: -------------------------------------------------------------------------------- 1 | #ifndef WProgram_h 2 | #define WProgram_h 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "wiring.h" 11 | 12 | #ifdef __cplusplus 13 | #include "HardwareSerial.h" 14 | 15 | uint16_t makeWord(uint16_t w); 16 | uint16_t makeWord(byte h, byte l); 17 | 18 | #define word(...) makeWord(__VA_ARGS__) 19 | 20 | unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout = 1000000L); 21 | 22 | // WMath prototypes 23 | long random(long); 24 | long random(long, long); 25 | void randomSeed(unsigned int); 26 | long map(long, long, long, long, long); 27 | #endif 28 | 29 | #endif -------------------------------------------------------------------------------- /src/.gitignore: -------------------------------------------------------------------------------- 1 | # Windows image file caches 2 | Thumbs.db 3 | ehthumbs.db 4 | 5 | # Folder config file 6 | Desktop.ini 7 | 8 | # Recycle Bin used on file shares 9 | $RECYCLE.BIN/ 10 | 11 | # Windows Installer files 12 | *.cab 13 | *.msi 14 | *.msm 15 | *.msp 16 | 17 | # Windows shortcuts 18 | *.lnk 19 | 20 | # ========================= 21 | # Operating System Files 22 | # ========================= 23 | 24 | # OSX 25 | # ========================= 26 | 27 | .DS_Store 28 | .AppleDouble 29 | .LSOverride 30 | 31 | # Thumbnails 32 | ._* 33 | 34 | # Files that might appear on external disk 35 | .Spotlight-V100 36 | .Trashes 37 | 38 | # Directories potentially created on remote AFP share 39 | .AppleDB 40 | .AppleDesktop 41 | Network Trash Folder 42 | Temporary Items 43 | .apdisk 44 | -------------------------------------------------------------------------------- /src/PinStates.h: -------------------------------------------------------------------------------- 1 | // http://masteringarduino.blogspot.com/2013/10/fastest-and-smallest-digitalread-and.html 2 | 3 | #define portOfPin(P)\ 4 | (((P)>=0&&(P)<8)?&PORTD:(((P)>7&&(P)<14)?&PORTB:&PORTC)) 5 | #define ddrOfPin(P)\ 6 | (((P)>=0&&(P)<8)?&DDRD:(((P)>7&&(P)<14)?&DDRB:&DDRC)) 7 | #define pinOfPin(P)\ 8 | (((P)>=0&&(P)<8)?&PIND:(((P)>7&&(P)<14)?&PINB:&PINC)) 9 | #define pinIndex(P)((uint8_t)(P>13?P-14:P&7)) 10 | #define pinMask(P)((uint8_t)(1<0) 18 | #define isLow(P)((*(pinOfPin(P))& pinMask(P))==0) 19 | #define digitalState(P)((uint8_t)isHigh(P)) 20 | 21 | -------------------------------------------------------------------------------- /examples/slider/lib/readme.txt: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for the project specific (private) libraries. 3 | PlatformIO will compile them to static libraries and link to executable file. 4 | 5 | The source code of each library should be placed in separate directory, like 6 | "lib/private_lib/[here are source files]". 7 | 8 | For example, see how can be organized `Foo` and `Bar` libraries: 9 | 10 | |--lib 11 | | |--Bar 12 | | | |--docs 13 | | | |--examples 14 | | | |--src 15 | | | |- Bar.c 16 | | | |- Bar.h 17 | | |--Foo 18 | | | |- Foo.c 19 | | | |- Foo.h 20 | | |- readme.txt --> THIS FILE 21 | |- platformio.ini 22 | |--src 23 | |- main.c 24 | 25 | Then in `src/main.c` you should use: 26 | 27 | #include 28 | #include 29 | 30 | // rest H/C/CPP code 31 | 32 | PlatformIO will find your libraries automatically, configure preprocessor's 33 | include paths and build them. 34 | 35 | See additional options for PlatformIO Library Dependency Finder `lib_*`: 36 | 37 | http://docs.platformio.org/en/stable/projectconf.html#lib-install 38 | 39 | -------------------------------------------------------------------------------- /src/utility/direct_pin_read.h: -------------------------------------------------------------------------------- 1 | #ifndef direct_pin_read_h_ 2 | #define direct_pin_read_h_ 3 | 4 | #if defined(__AVR__) || defined(__MK20DX128__) || defined(__MK20DX256__) 5 | 6 | #define IO_REG_TYPE uint8_t 7 | #define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin))) 8 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 9 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 10 | 11 | #elif defined(__SAM3X8E__) 12 | 13 | #define IO_REG_TYPE uint32_t 14 | #define PIN_TO_BASEREG(pin) (portInputRegister(digitalPinToPort(pin))) 15 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 16 | #define DIRECT_PIN_READ(base, mask) (((*(base)) & (mask)) ? 1 : 0) 17 | 18 | #elif defined(__PIC32MX__) 19 | 20 | #define IO_REG_TYPE uint32_t 21 | #define PIN_TO_BASEREG(pin) (portModeRegister(digitalPinToPort(pin))) 22 | #define PIN_TO_BITMASK(pin) (digitalPinToBitMask(pin)) 23 | #define DIRECT_PIN_READ(base, mask) (((*(base+4)) & (mask)) ? 1 : 0) 24 | 25 | #endif 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /src/Event.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This program is free software; you can redistribute it and/or modify 3 | * it under the terms of the GNU General Public License as published by 4 | * the Free Software Foundation; either version 2 of the License, or 5 | * (at your option) any later version. 6 | * 7 | * This program is distributed in the hope that it will be useful, 8 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 9 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 10 | * GNU General Public License for more details. 11 | * 12 | * You should have received a copy of the GNU General Public License 13 | * along with this program; if not, write to the Free Software 14 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, 15 | * MA 02110-1301, USA. 16 | */ 17 | 18 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * 19 | Code by Simon Monk 20 | http://www.simonmonk.org 21 | * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 22 | 23 | #ifndef Event_h 24 | #define Event_h 25 | 26 | #include 27 | 28 | #define EVENT_NONE 0 29 | #define EVENT_EVERY 1 30 | #define EVENT_OSCILLATE 2 31 | 32 | class Event 33 | { 34 | 35 | public: 36 | Event(void); 37 | void update(void); 38 | void update(unsigned long now); 39 | int8_t eventType; 40 | unsigned long period; 41 | int repeatCount; 42 | uint8_t pin; 43 | uint8_t pinState; 44 | void (*callback)(void); 45 | unsigned long lastEventTime; 46 | int count; 47 | }; 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /src/RunningAverage.h: -------------------------------------------------------------------------------- 1 | // 2 | // FILE: RunningAverage.h 3 | // AUTHOR: Rob dot Tillaart at gmail dot com 4 | // VERSION: 0.2.11 5 | // DATE: 2015-sep-04 6 | // PURPOSE: RunningAverage library for Arduino 7 | // URL: http://arduino.cc/playground/Main/RunningAverage 8 | // HISTORY: See RunningAverage.cpp 9 | // 10 | // Released to the public domain 11 | // 12 | // backwards compatibility 13 | // clr() clear() 14 | // add(x) addValue(x) 15 | // avg() getAverage() 16 | 17 | #ifndef RunningAverage_h 18 | #define RunningAverage_h 19 | 20 | #define RUNNINGAVERAGE_LIB_VERSION "0.2.11" 21 | 22 | #include "Arduino.h" 23 | 24 | class RunningAverage 25 | { 26 | public: 27 | RunningAverage(void); 28 | explicit RunningAverage(const uint8_t); 29 | ~RunningAverage(); 30 | 31 | void clear(); 32 | void addValue(const double); 33 | void fillValue(const double, const uint8_t); 34 | 35 | double getAverage() const; // does iterate over all elements. 36 | double getFastAverage() const; // reuses previous values. 37 | 38 | // returns min/max added to the data-set since last clear 39 | double getMin() const { return _min; }; 40 | double getMax() const { return _max; }; 41 | 42 | // returns min/max from the values in the internal buffer 43 | double GetMinInBuffer() const; 44 | double GetMaxInBuffer() const; 45 | 46 | double getElement(uint8_t idx) const; 47 | 48 | uint8_t getSize() const { return _size; } 49 | uint8_t getCount() const { return _cnt; } 50 | 51 | protected: 52 | uint8_t _size; 53 | uint8_t _cnt; 54 | uint8_t _idx; 55 | double _sum; 56 | double * _ar; 57 | double _min; 58 | double _max; 59 | }; 60 | 61 | #endif 62 | // END OF FILE -------------------------------------------------------------------------------- /src/Event.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This program is free software; you can redistribute it and/or modify 3 | * it under the terms of the GNU General Public License as published by 4 | * the Free Software Foundation; either version 2 of the License, or 5 | * (at your option) any later version. 6 | * 7 | * This program is distributed in the hope that it will be useful, 8 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 9 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 10 | * GNU General Public License for more details. 11 | * 12 | * You should have received a copy of the GNU General Public License 13 | * along with this program; if not, write to the Free Software 14 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, 15 | * MA 02110-1301, USA. 16 | */ 17 | 18 | /* * * * * * * * * * * * * * * * * * * * * * * * * * * * 19 | Code by Simon Monk 20 | http://www.simonmonk.org 21 | * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ 22 | 23 | // For Arduino 1.0 and earlier 24 | #if defined(ARDUINO) && ARDUINO >= 100 25 | #include "Arduino.h" 26 | #else 27 | #include "WProgram.h" 28 | #endif 29 | 30 | #include "Event.h" 31 | 32 | Event::Event(void) 33 | { 34 | eventType = EVENT_NONE; 35 | } 36 | 37 | void Event::update(void) 38 | { 39 | unsigned long now = millis(); 40 | update(now); 41 | } 42 | 43 | void Event::update(unsigned long now) 44 | { 45 | if (now - lastEventTime >= period) 46 | { 47 | switch (eventType) 48 | { 49 | case EVENT_EVERY: 50 | (*callback)(); 51 | break; 52 | 53 | case EVENT_OSCILLATE: 54 | pinState = ! pinState; 55 | digitalWrite(pin, pinState); 56 | break; 57 | } 58 | lastEventTime = now; 59 | count++; 60 | } 61 | if (repeatCount > -1 && count >= repeatCount) 62 | { 63 | eventType = EVENT_NONE; 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /src/utility/interrupt_config.h: -------------------------------------------------------------------------------- 1 | #if defined(__AVR__) 2 | 3 | #include 4 | #include 5 | 6 | #define attachInterrupt(num, func, mode) enableInterrupt(num) 7 | #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 8 | #define SCRAMBLE_INT_ORDER(num) ((num < 4) ? num + 2 : ((num < 6) ? num - 4 : num)) 9 | #define DESCRAMBLE_INT_ORDER(num) ((num < 2) ? num + 4 : ((num < 6) ? num - 2 : num)) 10 | #else 11 | #define SCRAMBLE_INT_ORDER(num) (num) 12 | #define DESCRAMBLE_INT_ORDER(num) (num) 13 | #endif 14 | 15 | static void enableInterrupt(uint8_t num) 16 | { 17 | switch (DESCRAMBLE_INT_ORDER(num)) { 18 | #if defined(EICRA) && defined(EIMSK) 19 | case 0: 20 | EICRA = (EICRA & 0xFC) | 0x01; 21 | EIMSK |= 0x01; 22 | return; 23 | case 1: 24 | EICRA = (EICRA & 0xF3) | 0x04; 25 | EIMSK |= 0x02; 26 | return; 27 | case 2: 28 | EICRA = (EICRA & 0xCF) | 0x10; 29 | EIMSK |= 0x04; 30 | return; 31 | case 3: 32 | EICRA = (EICRA & 0x3F) | 0x40; 33 | EIMSK |= 0x08; 34 | return; 35 | #elif defined(MCUCR) && defined(GICR) 36 | case 0: 37 | MCUCR = (MCUCR & ~((1 << ISC00) | (1 << ISC01))) | (mode << ISC00); 38 | GICR |= (1 << INT0); 39 | return; 40 | case 1: 41 | MCUCR = (MCUCR & ~((1 << ISC10) | (1 << ISC11))) | (mode << ISC10); 42 | GICR |= (1 << INT1); 43 | return; 44 | #elif defined(MCUCR) && defined(GIMSK) 45 | case 0: 46 | MCUCR = (MCUCR & ~((1 << ISC00) | (1 << ISC01))) | (mode << ISC00); 47 | GIMSK |= (1 << INT0); 48 | return; 49 | case 1: 50 | MCUCR = (MCUCR & ~((1 << ISC10) | (1 << ISC11))) | (mode << ISC10); 51 | GIMSK |= (1 << INT1); 52 | return; 53 | #endif 54 | #if defined(EICRB) && defined(EIMSK) 55 | case 4: 56 | EICRB = (EICRB & 0xFC) | 0x01; 57 | EIMSK |= 0x10; 58 | return; 59 | case 5: 60 | EICRB = (EICRB & 0xF3) | 0x04; 61 | EIMSK |= 0x20; 62 | return; 63 | case 6: 64 | EICRB = (EICRB & 0xCF) | 0x10; 65 | EIMSK |= 0x40; 66 | return; 67 | case 7: 68 | EICRB = (EICRB & 0x3F) | 0x40; 69 | EIMSK |= 0x80; 70 | return; 71 | #endif 72 | } 73 | } 74 | 75 | #elif defined(__PIC32MX__) 76 | 77 | #ifdef ENCODER_OPTIMIZE_INTERRUPTS 78 | #undef ENCODER_OPTIMIZE_INTERRUPTS 79 | #endif 80 | 81 | #else 82 | 83 | #ifdef ENCODER_OPTIMIZE_INTERRUPTS 84 | #undef ENCODER_OPTIMIZE_INTERRUPTS 85 | #endif 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /src/PID_v1.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_v1_h 2 | #define PID_v1_h 3 | #define LIBRARY_VERSION 1.1.1 4 | 5 | class PID 6 | { 7 | 8 | 9 | public: 10 | 11 | //Constants used in some of the functions below 12 | #define AUTOMATIC 1 13 | #define MANUAL 0 14 | #define DIRECT 0 15 | #define REVERSE 1 16 | 17 | //commonly used functions ************************************************************************** 18 | PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and 19 | double, double, double, int); // Setpoint. Initial tuning parameters are also set here 20 | 21 | void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0) 22 | 23 | bool Compute(); // * performs the PID calculation. it should be 24 | // called every time loop() cycles. ON/OFF and 25 | // calculation frequency can be set using SetMode 26 | // SetSampleTime respectively 27 | 28 | void SetOutputLimits(double, double); //clamps the output to a specific range. 0-255 by default, but 29 | //it's likely the user will want to change this depending on 30 | //the application 31 | 32 | 33 | 34 | //available but not commonly used functions ******************************************************** 35 | void SetTunings(double, double, // * While most users will set the tunings once in the 36 | double); // constructor, this function gives the user the option 37 | // of changing tunings during runtime for Adaptive control 38 | void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT 39 | // means the output will increase when error is positive. REVERSE 40 | // means the opposite. it's very unlikely that this will be needed 41 | // once it is set in the constructor. 42 | void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which 43 | // the PID calculation is performed. default is 100 44 | 45 | 46 | 47 | //Display functions **************************************************************** 48 | double GetKp(); // These functions query the pid for interal values. 49 | double GetKi(); // they were created mainly for the pid front-end, 50 | double GetKd(); // where it's important to know what is actually 51 | int GetMode(); // inside the PID. 52 | int GetDirection(); // 53 | 54 | private: 55 | void Initialize(); 56 | 57 | double dispKp; // * we'll hold on to the tuning parameters in user-entered 58 | double dispKi; // format for display purposes 59 | double dispKd; // 60 | 61 | double kp; // * (P)roportional Tuning Parameter 62 | double ki; // * (I)ntegral Tuning Parameter 63 | double kd; // * (D)erivative Tuning Parameter 64 | 65 | int controllerDirection; 66 | 67 | double *myInput; // * Pointers to the Input, Output, and Setpoint variables 68 | double *myOutput; // This creates a hard link between the variables and the 69 | double *mySetpoint; // PID, freeing the user from having to constantly tell us 70 | // what these values are. with pointers we'll just know. 71 | 72 | unsigned long lastTime; 73 | double ITerm, lastInput; 74 | 75 | unsigned long SampleTime; 76 | double outMin, outMax; 77 | bool inAuto; 78 | }; 79 | #endif 80 | 81 | -------------------------------------------------------------------------------- /examples/slider/src/PID_v1.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_v1_h 2 | #define PID_v1_h 3 | #define LIBRARY_VERSION 1.1.1 4 | 5 | class PID 6 | { 7 | 8 | 9 | public: 10 | 11 | //Constants used in some of the functions below 12 | #define AUTOMATIC 1 13 | #define MANUAL 0 14 | #define DIRECT 0 15 | #define REVERSE 1 16 | 17 | //commonly used functions ************************************************************************** 18 | PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and 19 | double, double, double, int); // Setpoint. Initial tuning parameters are also set here 20 | 21 | void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0) 22 | 23 | bool Compute(); // * performs the PID calculation. it should be 24 | // called every time loop() cycles. ON/OFF and 25 | // calculation frequency can be set using SetMode 26 | // SetSampleTime respectively 27 | 28 | void SetOutputLimits(double, double); //clamps the output to a specific range. 0-255 by default, but 29 | //it's likely the user will want to change this depending on 30 | //the application 31 | 32 | 33 | 34 | //available but not commonly used functions ******************************************************** 35 | void SetTunings(double, double, // * While most users will set the tunings once in the 36 | double); // constructor, this function gives the user the option 37 | // of changing tunings during runtime for Adaptive control 38 | void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT 39 | // means the output will increase when error is positive. REVERSE 40 | // means the opposite. it's very unlikely that this will be needed 41 | // once it is set in the constructor. 42 | void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which 43 | // the PID calculation is performed. default is 100 44 | 45 | 46 | 47 | //Display functions **************************************************************** 48 | double GetKp(); // These functions query the pid for interal values. 49 | double GetKi(); // they were created mainly for the pid front-end, 50 | double GetKd(); // where it's important to know what is actually 51 | int GetMode(); // inside the PID. 52 | int GetDirection(); // 53 | 54 | private: 55 | void Initialize(); 56 | 57 | double dispKp; // * we'll hold on to the tuning parameters in user-entered 58 | double dispKi; // format for display purposes 59 | double dispKd; // 60 | 61 | double kp; // * (P)roportional Tuning Parameter 62 | double ki; // * (I)ntegral Tuning Parameter 63 | double kd; // * (D)erivative Tuning Parameter 64 | 65 | int controllerDirection; 66 | 67 | double *myInput; // * Pointers to the Input, Output, and Setpoint variables 68 | double *myOutput; // This creates a hard link between the variables and the 69 | double *mySetpoint; // PID, freeing the user from having to constantly tell us 70 | // what these values are. with pointers we'll just know. 71 | 72 | unsigned long lastTime; 73 | double ITerm, lastInput; 74 | 75 | unsigned long SampleTime; 76 | double outMin, outMax; 77 | bool inAuto; 78 | }; 79 | #endif 80 | 81 | -------------------------------------------------------------------------------- /src/wiring.h: -------------------------------------------------------------------------------- 1 | /* 2 | wiring.h - Partial implementation of the Wiring API for the ATmega8. 3 | Part of Arduino - http://www.arduino.cc/ 4 | 5 | Copyright (c) 2005-2006 David A. Mellis 6 | 7 | This library is free software; you can redistribute it and/or 8 | modify it under the terms of the GNU Lesser General Public 9 | License as published by the Free Software Foundation; either 10 | version 2.1 of the License, or (at your option) any later version. 11 | 12 | This library is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | Lesser General Public License for more details. 16 | 17 | You should have received a copy of the GNU Lesser General 18 | Public License along with this library; if not, write to the 19 | Free Software Foundation, Inc., 59 Temple Place, Suite 330, 20 | Boston, MA 02111-1307 USA 21 | 22 | $Id$ 23 | */ 24 | 25 | #ifndef Wiring_h 26 | #define Wiring_h 27 | 28 | #include 29 | #include 30 | #include "binary.h" 31 | 32 | #ifdef __cplusplus 33 | extern "C"{ 34 | #endif 35 | 36 | #define HIGH 0x1 37 | #define LOW 0x0 38 | 39 | #define INPUT 0x0 40 | #define OUTPUT 0x1 41 | 42 | #define true 0x1 43 | #define false 0x0 44 | 45 | #define PI 3.1415926535897932384626433832795 46 | #define HALF_PI 1.5707963267948966192313216916398 47 | #define TWO_PI 6.283185307179586476925286766559 48 | #define DEG_TO_RAD 0.017453292519943295769236907684886 49 | #define RAD_TO_DEG 57.295779513082320876798154814105 50 | 51 | #define SERIAL 0x0 52 | #define DISPLAY 0x1 53 | 54 | #define LSBFIRST 0 55 | #define MSBFIRST 1 56 | 57 | #define CHANGE 1 58 | #define FALLING 2 59 | #define RISING 3 60 | 61 | #if defined(__AVR_ATmega1280__) 62 | #define INTERNAL1V1 2 63 | #define INTERNAL2V56 3 64 | #else 65 | #define INTERNAL 3 66 | #endif 67 | #define DEFAULT 1 68 | #define EXTERNAL 0 69 | 70 | // undefine stdlib's abs if encountered 71 | #ifdef abs 72 | #undef abs 73 | #endif 74 | 75 | #define min(a,b) ((a)<(b)?(a):(b)) 76 | #define max(a,b) ((a)>(b)?(a):(b)) 77 | #define abs(x) ((x)>0?(x):-(x)) 78 | #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 79 | #define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) 80 | #define radians(deg) ((deg)*DEG_TO_RAD) 81 | #define degrees(rad) ((rad)*RAD_TO_DEG) 82 | #define sq(x) ((x)*(x)) 83 | 84 | #define interrupts() sei() 85 | #define noInterrupts() cli() 86 | 87 | #define clockCyclesPerMicrosecond() ( F_CPU / 1000000L ) 88 | #define clockCyclesToMicroseconds(a) ( (a) / clockCyclesPerMicrosecond() ) 89 | #define microsecondsToClockCycles(a) ( (a) * clockCyclesPerMicrosecond() ) 90 | 91 | #define lowByte(w) ((uint8_t) ((w) & 0xff)) 92 | #define highByte(w) ((uint8_t) ((w) >> 8)) 93 | 94 | #define bitRead(value, bit) (((value) >> (bit)) & 0x01) 95 | #define bitSet(value, bit) ((value) |= (1UL << (bit))) 96 | #define bitClear(value, bit) ((value) &= ~(1UL << (bit))) 97 | #define bitWrite(value, bit, bitvalue) (bitvalue ? bitSet(value, bit) : bitClear(value, bit)) 98 | 99 | typedef unsigned int word; 100 | 101 | #define bit(b) (1UL << (b)) 102 | 103 | typedef uint8_t boolean; 104 | typedef uint8_t byte; 105 | 106 | void init(void); 107 | 108 | void pinMode(uint8_t, uint8_t); 109 | void digitalWrite(uint8_t, uint8_t); 110 | int digitalRead(uint8_t); 111 | int analogRead(uint8_t); 112 | void analogReference(uint8_t mode); 113 | void analogWrite(uint8_t, int); 114 | 115 | unsigned long millis(void); 116 | unsigned long micros(void); 117 | void delay(unsigned long); 118 | void delayMicroseconds(unsigned int us); 119 | unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout); 120 | 121 | void shiftOut(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder, uint8_t val); 122 | uint8_t shiftIn(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder); 123 | 124 | void attachInterrupt(uint8_t, void (*)(void), int mode); 125 | void detachInterrupt(uint8_t); 126 | 127 | void setup(void); 128 | void loop(void); 129 | 130 | #ifdef __cplusplus 131 | } // extern "C" 132 | #endif 133 | 134 | #endif 135 | -------------------------------------------------------------------------------- /examples/slider/src/wiring.h: -------------------------------------------------------------------------------- 1 | /* 2 | wiring.h - Partial implementation of the Wiring API for the ATmega8. 3 | Part of Arduino - http://www.arduino.cc/ 4 | 5 | Copyright (c) 2005-2006 David A. Mellis 6 | 7 | This library is free software; you can redistribute it and/or 8 | modify it under the terms of the GNU Lesser General Public 9 | License as published by the Free Software Foundation; either 10 | version 2.1 of the License, or (at your option) any later version. 11 | 12 | This library is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | Lesser General Public License for more details. 16 | 17 | You should have received a copy of the GNU Lesser General 18 | Public License along with this library; if not, write to the 19 | Free Software Foundation, Inc., 59 Temple Place, Suite 330, 20 | Boston, MA 02111-1307 USA 21 | 22 | $Id$ 23 | */ 24 | 25 | #ifndef Wiring_h 26 | #define Wiring_h 27 | 28 | #include 29 | #include 30 | #include "binary.h" 31 | 32 | #ifdef __cplusplus 33 | extern "C"{ 34 | #endif 35 | 36 | #define HIGH 0x1 37 | #define LOW 0x0 38 | 39 | #define INPUT 0x0 40 | #define OUTPUT 0x1 41 | 42 | #define true 0x1 43 | #define false 0x0 44 | 45 | #define PI 3.1415926535897932384626433832795 46 | #define HALF_PI 1.5707963267948966192313216916398 47 | #define TWO_PI 6.283185307179586476925286766559 48 | #define DEG_TO_RAD 0.017453292519943295769236907684886 49 | #define RAD_TO_DEG 57.295779513082320876798154814105 50 | 51 | #define SERIAL 0x0 52 | #define DISPLAY 0x1 53 | 54 | #define LSBFIRST 0 55 | #define MSBFIRST 1 56 | 57 | #define CHANGE 1 58 | #define FALLING 2 59 | #define RISING 3 60 | 61 | #if defined(__AVR_ATmega1280__) 62 | #define INTERNAL1V1 2 63 | #define INTERNAL2V56 3 64 | #else 65 | #define INTERNAL 3 66 | #endif 67 | #define DEFAULT 1 68 | #define EXTERNAL 0 69 | 70 | // undefine stdlib's abs if encountered 71 | #ifdef abs 72 | #undef abs 73 | #endif 74 | 75 | #define min(a,b) ((a)<(b)?(a):(b)) 76 | #define max(a,b) ((a)>(b)?(a):(b)) 77 | #define abs(x) ((x)>0?(x):-(x)) 78 | #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 79 | #define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) 80 | #define radians(deg) ((deg)*DEG_TO_RAD) 81 | #define degrees(rad) ((rad)*RAD_TO_DEG) 82 | #define sq(x) ((x)*(x)) 83 | 84 | #define interrupts() sei() 85 | #define noInterrupts() cli() 86 | 87 | #define clockCyclesPerMicrosecond() ( F_CPU / 1000000L ) 88 | #define clockCyclesToMicroseconds(a) ( (a) / clockCyclesPerMicrosecond() ) 89 | #define microsecondsToClockCycles(a) ( (a) * clockCyclesPerMicrosecond() ) 90 | 91 | #define lowByte(w) ((uint8_t) ((w) & 0xff)) 92 | #define highByte(w) ((uint8_t) ((w) >> 8)) 93 | 94 | #define bitRead(value, bit) (((value) >> (bit)) & 0x01) 95 | #define bitSet(value, bit) ((value) |= (1UL << (bit))) 96 | #define bitClear(value, bit) ((value) &= ~(1UL << (bit))) 97 | #define bitWrite(value, bit, bitvalue) (bitvalue ? bitSet(value, bit) : bitClear(value, bit)) 98 | 99 | typedef unsigned int word; 100 | 101 | #define bit(b) (1UL << (b)) 102 | 103 | typedef uint8_t boolean; 104 | typedef uint8_t byte; 105 | 106 | void init(void); 107 | 108 | void pinMode(uint8_t, uint8_t); 109 | void digitalWrite(uint8_t, uint8_t); 110 | int digitalRead(uint8_t); 111 | int analogRead(uint8_t); 112 | void analogReference(uint8_t mode); 113 | void analogWrite(uint8_t, int); 114 | 115 | unsigned long millis(void); 116 | unsigned long micros(void); 117 | void delay(unsigned long); 118 | void delayMicroseconds(unsigned int us); 119 | unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout); 120 | 121 | void shiftOut(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder, uint8_t val); 122 | uint8_t shiftIn(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder); 123 | 124 | void attachInterrupt(uint8_t, void (*)(void), int mode); 125 | void detachInterrupt(uint8_t); 126 | 127 | void setup(void); 128 | void loop(void); 129 | 130 | #ifdef __cplusplus 131 | } // extern "C" 132 | #endif 133 | 134 | #endif 135 | -------------------------------------------------------------------------------- /src/RunningAverage.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // FILE: RunningAverage.cpp 3 | // AUTHOR: Rob Tillaart 4 | // VERSION: 0.2.11 5 | // DATE: 2015-July-10 6 | // PURPOSE: RunningAverage library for Arduino 7 | // 8 | // The library stores N individual values in a circular buffer, 9 | // to calculate the running average. 10 | // 11 | // HISTORY: 12 | // 0.1.00 - 2011-01-30 initial version 13 | // 0.1.01 - 2011-02-28 fixed missing destructor in .h 14 | // 0.2.00 - 2012-??-?? Yuval Naveh added trimValue (found on web) 15 | // http://stromputer.googlecode.com/svn-history/r74/trunk/Arduino/Libraries/RunningAverage/RunningAverage.cpp 16 | // 0.2.01 - 2012-11-21 refactored 17 | // 0.2.02 - 2012-12-30 refactored trimValue -> fillValue 18 | // 0.2.03 - 2013-11-31 getElement 19 | // 0.2.04 - 2014-07-03 added memory protection 20 | // 0.2.05 - 2014-12-16 changed float -> double 21 | // 0.2.06 - 2015-03-07 all size uint8_t 22 | // 0.2.07 - 2015-03-16 added getMin() and getMax() functions (Eric Mulder) 23 | // 0.2.08 - 2015-04-10 refactored getMin() and getMax() implementation 24 | // 0.2.09 - 2015-07-12 refactor const + constructor 25 | // 0.2.10 - 2015-09-01 added getFastAverage() and refactored getAverage() 26 | // http://forum.arduino.cc/index.php?topic=50473 27 | // 0.2.11 - 2015-09-04 added getMaxInBuffer() getMinInBuffer() request (Antoon) 28 | // 29 | // Released to the public domain 30 | // 31 | 32 | #include "RunningAverage.h" 33 | #include 34 | 35 | RunningAverage::RunningAverage(const uint8_t size) 36 | { 37 | _size = size; 38 | _ar = (double*) malloc(_size * sizeof(double)); 39 | if (_ar == NULL) _size = 0; 40 | clear(); 41 | } 42 | 43 | RunningAverage::~RunningAverage() 44 | { 45 | if (_ar != NULL) free(_ar); 46 | } 47 | 48 | // resets all counters 49 | void RunningAverage::clear() 50 | { 51 | _cnt = 0; 52 | _idx = 0; 53 | _sum = 0.0; 54 | _min = NAN; 55 | _max = NAN; 56 | for (uint8_t i = 0; i < _size; i++) 57 | { 58 | _ar[i] = 0.0; // keeps addValue simpler 59 | } 60 | } 61 | 62 | // adds a new value to the data-set 63 | void RunningAverage::addValue(const double value) 64 | { 65 | if (_ar == NULL) return; // allocation error 66 | _sum -= _ar[_idx]; 67 | _ar[_idx] = value; 68 | _sum += _ar[_idx]; 69 | _idx++; 70 | if (_idx == _size) _idx = 0; // faster than % 71 | 72 | // handle min max 73 | if (_cnt == 0) _min = _max = value; 74 | else if (value < _min) _min = value; 75 | else if (value > _max) _max = value; 76 | 77 | // update count as last otherwise if( _cnt == 0) above will fail 78 | if (_cnt < _size) _cnt++; 79 | } 80 | 81 | // returns the average of the data-set added sofar 82 | double RunningAverage::getAverage() const 83 | { 84 | if (_cnt == 0) return NAN; 85 | double sum = 0; 86 | for (uint8_t i = 0; i < _cnt; i++) 87 | { 88 | sum += _ar[i]; 89 | } 90 | return sum / _cnt; 91 | } 92 | 93 | double RunningAverage::getFastAverage() const 94 | { 95 | if (_cnt == 0) return NAN; 96 | return _sum / _cnt; 97 | } 98 | 99 | // returns the max value in the buffer 100 | double RunningAverage::GetMinInBuffer() const 101 | { 102 | if (_cnt == 0) return NAN; 103 | double min = _ar[0]; 104 | for (uint8_t i = 1; i < _cnt; i++) 105 | { 106 | if (min > _ar[i]) min = _ar[i]; 107 | } 108 | return min; 109 | } 110 | 111 | double RunningAverage::GetMaxInBuffer() const 112 | { 113 | if (_cnt == 0) return NAN; 114 | double max = _ar[0]; 115 | for (uint8_t i = 1; i < _cnt; i++) 116 | { 117 | if (max < _ar[i]) max = _ar[i]; 118 | } 119 | return max; 120 | } 121 | 122 | // returns the value of an element if exist, NAN otherwise 123 | double RunningAverage::getElement(uint8_t idx) const 124 | { 125 | if (idx >=_cnt ) return NAN; 126 | return _ar[idx]; 127 | } 128 | 129 | // fill the average with a value 130 | // the param number determines how often value is added (weight) 131 | // number should preferably be between 1 and size 132 | void RunningAverage::fillValue(const double value, const uint8_t number) 133 | { 134 | clear(); // TODO conditional? if (clr) clear(); 135 | 136 | for (uint8_t i = 0; i < number; i++) 137 | { 138 | addValue(value); 139 | } 140 | } 141 | // END OF FILE -------------------------------------------------------------------------------- /src/utility/interrupt_pins.h: -------------------------------------------------------------------------------- 1 | // interrupt pins for known boards 2 | 3 | // Teensy (and maybe others) define these automatically 4 | #if !defined(CORE_NUM_INTERRUPT) 5 | 6 | // Wiring boards 7 | #if defined(WIRING) 8 | #define CORE_NUM_INTERRUPT NUM_EXTERNAL_INTERRUPTS 9 | #if NUM_EXTERNAL_INTERRUPTS > 0 10 | #define CORE_INT0_PIN EI0 11 | #endif 12 | #if NUM_EXTERNAL_INTERRUPTS > 1 13 | #define CORE_INT1_PIN EI1 14 | #endif 15 | #if NUM_EXTERNAL_INTERRUPTS > 2 16 | #define CORE_INT2_PIN EI2 17 | #endif 18 | #if NUM_EXTERNAL_INTERRUPTS > 3 19 | #define CORE_INT3_PIN EI3 20 | #endif 21 | #if NUM_EXTERNAL_INTERRUPTS > 4 22 | #define CORE_INT4_PIN EI4 23 | #endif 24 | #if NUM_EXTERNAL_INTERRUPTS > 5 25 | #define CORE_INT5_PIN EI5 26 | #endif 27 | #if NUM_EXTERNAL_INTERRUPTS > 6 28 | #define CORE_INT6_PIN EI6 29 | #endif 30 | #if NUM_EXTERNAL_INTERRUPTS > 7 31 | #define CORE_INT7_PIN EI7 32 | #endif 33 | 34 | // Arduino Uno, Duemilanove, Diecimila, LilyPad, Mini, Fio, etc... 35 | #elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) 36 | #define CORE_NUM_INTERRUPT 2 37 | #define CORE_INT0_PIN 2 38 | #define CORE_INT1_PIN 3 39 | 40 | // Arduino Mega 41 | #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) 42 | #define CORE_NUM_INTERRUPT 6 43 | #define CORE_INT0_PIN 2 44 | #define CORE_INT1_PIN 3 45 | #define CORE_INT2_PIN 21 46 | #define CORE_INT3_PIN 20 47 | #define CORE_INT4_PIN 19 48 | #define CORE_INT5_PIN 18 49 | 50 | // Arduino Leonardo (untested) 51 | #elif defined(__AVR_ATmega32U4__) && !defined(CORE_TEENSY) 52 | #define CORE_NUM_INTERRUPT 4 53 | #define CORE_INT0_PIN 3 54 | #define CORE_INT1_PIN 2 55 | #define CORE_INT2_PIN 0 56 | #define CORE_INT3_PIN 1 57 | 58 | // Sanguino (untested) 59 | #elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) 60 | #define CORE_NUM_INTERRUPT 3 61 | #define CORE_INT0_PIN 10 62 | #define CORE_INT1_PIN 11 63 | #define CORE_INT2_PIN 2 64 | 65 | // Chipkit Uno32 - attachInterrupt may not support CHANGE option 66 | #elif defined(__PIC32MX__) && defined(_BOARD_UNO_) 67 | #define CORE_NUM_INTERRUPT 5 68 | #define CORE_INT0_PIN 38 69 | #define CORE_INT1_PIN 2 70 | #define CORE_INT2_PIN 7 71 | #define CORE_INT3_PIN 8 72 | #define CORE_INT4_PIN 35 73 | 74 | // Chipkit Uno32 - attachInterrupt may not support CHANGE option 75 | #elif defined(__PIC32MX__) && defined(_BOARD_MEGA_) 76 | #define CORE_NUM_INTERRUPT 5 77 | #define CORE_INT0_PIN 3 78 | #define CORE_INT1_PIN 2 79 | #define CORE_INT2_PIN 7 80 | #define CORE_INT3_PIN 21 81 | #define CORE_INT4_PIN 20 82 | 83 | // http://hlt.media.mit.edu/?p=1229 84 | #elif defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__) 85 | #define CORE_NUM_INTERRUPT 1 86 | #define CORE_INT0_PIN 2 87 | 88 | // Arduino Due (untested) 89 | #elif defined(__SAM3X8E__) 90 | #define CORE_NUM_INTERRUPT 54 91 | #define CORE_INT0_PIN 0 92 | #define CORE_INT1_PIN 1 93 | #define CORE_INT2_PIN 2 94 | #define CORE_INT3_PIN 3 95 | #define CORE_INT4_PIN 4 96 | #define CORE_INT5_PIN 5 97 | #define CORE_INT6_PIN 6 98 | #define CORE_INT7_PIN 7 99 | #define CORE_INT8_PIN 8 100 | #define CORE_INT9_PIN 9 101 | #define CORE_INT10_PIN 10 102 | #define CORE_INT11_PIN 11 103 | #define CORE_INT12_PIN 12 104 | #define CORE_INT13_PIN 13 105 | #define CORE_INT14_PIN 14 106 | #define CORE_INT15_PIN 15 107 | #define CORE_INT16_PIN 16 108 | #define CORE_INT17_PIN 17 109 | #define CORE_INT18_PIN 18 110 | #define CORE_INT19_PIN 19 111 | #define CORE_INT20_PIN 20 112 | #define CORE_INT21_PIN 21 113 | #define CORE_INT22_PIN 22 114 | #define CORE_INT23_PIN 23 115 | #define CORE_INT24_PIN 24 116 | #define CORE_INT25_PIN 25 117 | #define CORE_INT26_PIN 26 118 | #define CORE_INT27_PIN 27 119 | #define CORE_INT28_PIN 28 120 | #define CORE_INT29_PIN 29 121 | #define CORE_INT30_PIN 30 122 | #define CORE_INT31_PIN 31 123 | #define CORE_INT32_PIN 32 124 | #define CORE_INT33_PIN 33 125 | #define CORE_INT34_PIN 34 126 | #define CORE_INT35_PIN 35 127 | #define CORE_INT36_PIN 36 128 | #define CORE_INT37_PIN 37 129 | #define CORE_INT38_PIN 38 130 | #define CORE_INT39_PIN 39 131 | #define CORE_INT40_PIN 40 132 | #define CORE_INT41_PIN 41 133 | #define CORE_INT42_PIN 42 134 | #define CORE_INT43_PIN 43 135 | #define CORE_INT44_PIN 44 136 | #define CORE_INT45_PIN 45 137 | #define CORE_INT46_PIN 46 138 | #define CORE_INT47_PIN 47 139 | #define CORE_INT48_PIN 48 140 | #define CORE_INT49_PIN 49 141 | #define CORE_INT50_PIN 50 142 | #define CORE_INT51_PIN 51 143 | #define CORE_INT52_PIN 52 144 | #define CORE_INT53_PIN 53 145 | 146 | #endif 147 | #endif 148 | 149 | #if !defined(CORE_NUM_INTERRUPT) 150 | #error "Interrupts are unknown for this board, please add to this code" 151 | #endif 152 | #if CORE_NUM_INTERRUPT <= 0 153 | #error "Encoder requires interrupt pins, but this board does not have any :(" 154 | #error "You could try defining ENCODER_DO_NOT_USE_INTERRUPTS as a kludge." 155 | #endif 156 | 157 | -------------------------------------------------------------------------------- /serial_n_plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import time 4 | import sys 5 | import glob 6 | import serial 7 | import signal 8 | import re 9 | import math 10 | import matplotlib.pyplot as plt 11 | 12 | class handle_device: 13 | def __init__(self, device, baud_rate): 14 | port = '/dev/tty.usbmodem2335471' 15 | self.port_found = 0 16 | ser = serial.Serial(port, baud_rate, timeout=5) 17 | time.sleep(2) 18 | ser.flush() 19 | cmd = 'probe_device\n' 20 | ser.write(cmd) 21 | time.sleep(.3) 22 | buffer = '' 23 | while ser.inWaiting() > 0: 24 | buffer += ser.read(1) 25 | buffer = buffer.rstrip() 26 | if (buffer == device): 27 | self.port_found = 1 28 | self.serial = ser 29 | self.port_name = port 30 | else: 31 | ser.close() 32 | 33 | def handle_plot(self, text): 34 | x1 = [] # each loop number 35 | y1 = [] # decay results 36 | x2 = [] # each loop number 37 | y2 = [] # decay results 38 | 39 | max = 0; 40 | min = 10000; 41 | for line in text.split('\n'): 42 | str = line.split(' ') 43 | if (str[0] == 'positions'): 44 | t = 0 45 | for s in str[1:]: 46 | x1.append(t) 47 | y2.append(int(s)) 48 | if (int(s) > max): max = int(s) 49 | if (int(s) < min): min = int(s) 50 | t += 1 51 | 52 | if (str[0] == 'times'): 53 | t = 0 54 | for s in str[1:]: 55 | x2.append(t) 56 | y2.append(int(s)) 57 | t += 1 58 | 59 | if (str[0] == 'target'): 60 | target = float(str[1].rstrip()) 61 | if (str[0] == 'start'): 62 | start = str[1] 63 | if (str[0] == 'position'): 64 | position = float(str[1].rstrip()) 65 | if (str[0] == 'kP'): 66 | kP = float(str[1].rstrip()) 67 | if (str[0] == 'kI'): 68 | kI = float(str[1].rstrip()) 69 | if (str[0] == 'kD'): 70 | kD = float(str[1].rstrip()) 71 | 72 | if (max < target): max = target 73 | height = max - min 74 | bump = height * 0.05 75 | axes = plt.gca() 76 | axes.set_ylim([min - bump, max + bump]) 77 | PID_str = ("kP = %s kI = %s kD = %s") % (kP, kI, kD) 78 | 79 | plt.text((t / 2), min + bump, PID_str, 80 | horizontalalignment='left', 81 | verticalalignment='bottom') 82 | 83 | plt.plot(x1, y1, label=('position = %s' % int(position))) 84 | plt.plot([0, t], [target, target], label=('target = %s' % int(target))) 85 | plt.legend(loc=1); 86 | plt.show() 87 | 88 | 89 | 90 | 91 | class Decay: 92 | def __init__(self): 93 | # Using radioactive decay for acceleration 94 | # where the count of atoms is used as the length of delay between 95 | # each loop moving a motor. Decay of the atoms over the loops 96 | # speeds up the motor 97 | self.accelTime = 600 # Number of loops to arrive at velocity 98 | self.startDelay = 40000 # Number of atoms at t = 0, starting velocity 99 | self.finalDelay = 100 # Number of atoms to decay to, ending velocity 100 | self.decayConstant = .01 101 | 102 | # find final decay point using the above parameters 103 | N = float(self.startDelay) 104 | L = self.decayConstant 105 | t = 0 106 | while t < self.accelTime: 107 | N = N - (L * N) 108 | t += 1 109 | self.endPoint = N 110 | 111 | # The issue is the decay of our atoms may be much larger or smaller 112 | # than what we want. But presumably we love the rate. So 113 | # Scale our final end point to our desired endpoint 114 | # do this by getting ranges 115 | self.leftSpan = self.startDelay - self.endPoint 116 | self.rightSpan = self.startDelay - self.finalDelay 117 | 118 | def calcDecay(self, N): 119 | # this is the loss of atoms 120 | N = N - (d.decayConstant * N) 121 | # now scale our current value to our desired endpoint 122 | s = float(N - self.endPoint) / float(self.leftSpan) 123 | return (N, self.finalDelay + (s * self.rightSpan)) 124 | 125 | if __name__ == "__main__": 126 | 127 | l = handle_device('PID_device', 115200) 128 | if l.port_found: 129 | port = l.port_name 130 | print "found: " , port 131 | ser = l.serial 132 | plot_toggle = True 133 | while True: 134 | buffer = [] 135 | try: 136 | for message in iter(lambda:raw_input("Enter Message:"),""): 137 | if (message == 'quit'): 138 | print message 139 | ser.close() 140 | sys.exit() 141 | msg_with_newline = message+"\n" 142 | print "sending", msg_with_newline.rstrip() 143 | ser.write(msg_with_newline) 144 | buffer = '' 145 | time.sleep(1) 146 | while ser.inWaiting() > 0: 147 | buffer += ser.read(1) 148 | if (re.match(r'plot', buffer)): 149 | l.handle_plot(buffer) 150 | else: 151 | print ("%s %s") % (port, buffer) 152 | 153 | except KeyboardInterrupt: 154 | print "Bye" 155 | ser.close() 156 | sys.exit() 157 | 158 | -------------------------------------------------------------------------------- /src/PID_v1.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************************** 2 | * Arduino PID Library - Version 1.1.1 3 | * by Brett Beauregard brettbeauregard.com 4 | * 5 | * This Library is licensed under a GPLv3 License 6 | **********************************************************************************************/ 7 | 8 | #if ARDUINO >= 100 9 | #include "Arduino.h" 10 | #else 11 | #include "WProgram.h" 12 | #endif 13 | 14 | #include "PID_v1.h" 15 | 16 | /*Constructor (...)********************************************************* 17 | * The parameters specified here are those for for which we can't set up 18 | * reliable defaults, so we need to have the user set them. 19 | ***************************************************************************/ 20 | PID::PID(double* Input, double* Output, double* Setpoint, 21 | double Kp, double Ki, double Kd, int ControllerDirection) 22 | { 23 | 24 | myOutput = Output; 25 | myInput = Input; 26 | mySetpoint = Setpoint; 27 | inAuto = false; 28 | 29 | PID::SetOutputLimits(0, 255); //default output limit corresponds to 30 | //the arduino pwm limits 31 | 32 | SampleTime = 100; //default Controller Sample Time is 0.1 seconds 33 | 34 | PID::SetControllerDirection(ControllerDirection); 35 | PID::SetTunings(Kp, Ki, Kd); 36 | 37 | lastTime = millis()-SampleTime; 38 | } 39 | 40 | 41 | /* Compute() ********************************************************************** 42 | * This, as they say, is where the magic happens. this function should be called 43 | * every time "void loop()" executes. the function will decide for itself whether a new 44 | * pid Output needs to be computed. returns true when the output is computed, 45 | * false when nothing has been done. 46 | **********************************************************************************/ 47 | bool PID::Compute() 48 | { 49 | if(!inAuto) return false; 50 | unsigned long now = millis(); 51 | unsigned long timeChange = (now - lastTime); 52 | if(timeChange>=SampleTime) 53 | { 54 | /*Compute all the working error variables*/ 55 | double input = *myInput; 56 | double error = *mySetpoint - input; 57 | ITerm+= (ki * error); 58 | if(ITerm > outMax) ITerm= outMax; 59 | else if(ITerm < outMin) ITerm= outMin; 60 | double dInput = (input - lastInput); 61 | 62 | /*Compute PID Output*/ 63 | double output = kp * error + ITerm- kd * dInput; 64 | 65 | if(output > outMax) output = outMax; 66 | else if(output < outMin) output = outMin; 67 | *myOutput = output; 68 | 69 | /*Remember some variables for next time*/ 70 | lastInput = input; 71 | lastTime = now; 72 | return true; 73 | } 74 | else return false; 75 | } 76 | 77 | 78 | /* SetTunings(...)************************************************************* 79 | * This function allows the controller's dynamic performance to be adjusted. 80 | * it's called automatically from the constructor, but tunings can also 81 | * be adjusted on the fly during normal operation 82 | ******************************************************************************/ 83 | void PID::SetTunings(double Kp, double Ki, double Kd) 84 | { 85 | if (Kp<0 || Ki<0 || Kd<0) return; 86 | 87 | dispKp = Kp; dispKi = Ki; dispKd = Kd; 88 | 89 | double SampleTimeInSec = ((double)SampleTime)/1000; 90 | kp = Kp; 91 | ki = Ki * SampleTimeInSec; 92 | kd = Kd / SampleTimeInSec; 93 | 94 | if(controllerDirection ==REVERSE) 95 | { 96 | kp = (0 - kp); 97 | ki = (0 - ki); 98 | kd = (0 - kd); 99 | } 100 | } 101 | 102 | /* SetSampleTime(...) ********************************************************* 103 | * sets the period, in Milliseconds, at which the calculation is performed 104 | ******************************************************************************/ 105 | void PID::SetSampleTime(int NewSampleTime) 106 | { 107 | if (NewSampleTime > 0) 108 | { 109 | double ratio = (double)NewSampleTime 110 | / (double)SampleTime; 111 | ki *= ratio; 112 | kd /= ratio; 113 | SampleTime = (unsigned long)NewSampleTime; 114 | } 115 | } 116 | 117 | /* SetOutputLimits(...)**************************************************** 118 | * This function will be used far more often than SetInputLimits. while 119 | * the input to the controller will generally be in the 0-1023 range (which is 120 | * the default already,) the output will be a little different. maybe they'll 121 | * be doing a time window and will need 0-8000 or something. or maybe they'll 122 | * want to clamp it from 0-125. who knows. at any rate, that can all be done 123 | * here. 124 | **************************************************************************/ 125 | void PID::SetOutputLimits(double Min, double Max) 126 | { 127 | if(Min >= Max) return; 128 | outMin = Min; 129 | outMax = Max; 130 | 131 | if(inAuto) 132 | { 133 | if(*myOutput > outMax) *myOutput = outMax; 134 | else if(*myOutput < outMin) *myOutput = outMin; 135 | 136 | if(ITerm > outMax) ITerm= outMax; 137 | else if(ITerm < outMin) ITerm= outMin; 138 | } 139 | } 140 | 141 | /* SetMode(...)**************************************************************** 142 | * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) 143 | * when the transition from manual to auto occurs, the controller is 144 | * automatically initialized 145 | ******************************************************************************/ 146 | void PID::SetMode(int Mode) 147 | { 148 | bool newAuto = (Mode == AUTOMATIC); 149 | if(newAuto == !inAuto) 150 | { /*we just went from manual to auto*/ 151 | PID::Initialize(); 152 | } 153 | inAuto = newAuto; 154 | } 155 | 156 | /* Initialize()**************************************************************** 157 | * does all the things that need to happen to ensure a bumpless transfer 158 | * from manual to automatic mode. 159 | ******************************************************************************/ 160 | void PID::Initialize() 161 | { 162 | ITerm = *myOutput; 163 | lastInput = *myInput; 164 | if(ITerm > outMax) ITerm = outMax; 165 | else if(ITerm < outMin) ITerm = outMin; 166 | } 167 | 168 | /* SetControllerDirection(...)************************************************* 169 | * The PID will either be connected to a DIRECT acting process (+Output leads 170 | * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to 171 | * know which one, because otherwise we may increase the output when we should 172 | * be decreasing. This is called from the constructor. 173 | ******************************************************************************/ 174 | void PID::SetControllerDirection(int Direction) 175 | { 176 | if(inAuto && Direction !=controllerDirection) 177 | { 178 | kp = (0 - kp); 179 | ki = (0 - ki); 180 | kd = (0 - kd); 181 | } 182 | controllerDirection = Direction; 183 | } 184 | 185 | /* Status Funcions************************************************************* 186 | * Just because you set the Kp=-1 doesn't mean it actually happened. these 187 | * functions query the internal state of the PID. they're here for display 188 | * purposes. this are the functions the PID Front-end uses for example 189 | ******************************************************************************/ 190 | double PID::GetKp(){ return dispKp; } 191 | double PID::GetKi(){ return dispKi;} 192 | double PID::GetKd(){ return dispKd;} 193 | int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} 194 | int PID::GetDirection(){ return controllerDirection;} 195 | 196 | -------------------------------------------------------------------------------- /examples/slider/src/PID_v1.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************************** 2 | * Arduino PID Library - Version 1.1.1 3 | * by Brett Beauregard brettbeauregard.com 4 | * 5 | * This Library is licensed under a GPLv3 License 6 | **********************************************************************************************/ 7 | 8 | #if ARDUINO >= 100 9 | #include "Arduino.h" 10 | #else 11 | #include "WProgram.h" 12 | #endif 13 | 14 | #include "PID_v1.h" 15 | 16 | /*Constructor (...)********************************************************* 17 | * The parameters specified here are those for for which we can't set up 18 | * reliable defaults, so we need to have the user set them. 19 | ***************************************************************************/ 20 | PID::PID(double* Input, double* Output, double* Setpoint, 21 | double Kp, double Ki, double Kd, int ControllerDirection) 22 | { 23 | 24 | myOutput = Output; 25 | myInput = Input; 26 | mySetpoint = Setpoint; 27 | inAuto = false; 28 | 29 | PID::SetOutputLimits(0, 255); //default output limit corresponds to 30 | //the arduino pwm limits 31 | 32 | SampleTime = 100; //default Controller Sample Time is 0.1 seconds 33 | 34 | PID::SetControllerDirection(ControllerDirection); 35 | PID::SetTunings(Kp, Ki, Kd); 36 | 37 | lastTime = millis()-SampleTime; 38 | } 39 | 40 | 41 | /* Compute() ********************************************************************** 42 | * This, as they say, is where the magic happens. this function should be called 43 | * every time "void loop()" executes. the function will decide for itself whether a new 44 | * pid Output needs to be computed. returns true when the output is computed, 45 | * false when nothing has been done. 46 | **********************************************************************************/ 47 | bool PID::Compute() 48 | { 49 | if(!inAuto) return false; 50 | unsigned long now = millis(); 51 | unsigned long timeChange = (now - lastTime); 52 | if(timeChange>=SampleTime) 53 | { 54 | /*Compute all the working error variables*/ 55 | double input = *myInput; 56 | double error = *mySetpoint - input; 57 | ITerm+= (ki * error); 58 | if(ITerm > outMax) ITerm= outMax; 59 | else if(ITerm < outMin) ITerm= outMin; 60 | double dInput = (input - lastInput); 61 | 62 | /*Compute PID Output*/ 63 | double output = kp * error + ITerm- kd * dInput; 64 | 65 | if(output > outMax) output = outMax; 66 | else if(output < outMin) output = outMin; 67 | *myOutput = output; 68 | 69 | /*Remember some variables for next time*/ 70 | lastInput = input; 71 | lastTime = now; 72 | return true; 73 | } 74 | else return false; 75 | } 76 | 77 | 78 | /* SetTunings(...)************************************************************* 79 | * This function allows the controller's dynamic performance to be adjusted. 80 | * it's called automatically from the constructor, but tunings can also 81 | * be adjusted on the fly during normal operation 82 | ******************************************************************************/ 83 | void PID::SetTunings(double Kp, double Ki, double Kd) 84 | { 85 | if (Kp<0 || Ki<0 || Kd<0) return; 86 | 87 | dispKp = Kp; dispKi = Ki; dispKd = Kd; 88 | 89 | double SampleTimeInSec = ((double)SampleTime)/1000; 90 | kp = Kp; 91 | ki = Ki * SampleTimeInSec; 92 | kd = Kd / SampleTimeInSec; 93 | 94 | if(controllerDirection ==REVERSE) 95 | { 96 | kp = (0 - kp); 97 | ki = (0 - ki); 98 | kd = (0 - kd); 99 | } 100 | } 101 | 102 | /* SetSampleTime(...) ********************************************************* 103 | * sets the period, in Milliseconds, at which the calculation is performed 104 | ******************************************************************************/ 105 | void PID::SetSampleTime(int NewSampleTime) 106 | { 107 | if (NewSampleTime > 0) 108 | { 109 | double ratio = (double)NewSampleTime 110 | / (double)SampleTime; 111 | ki *= ratio; 112 | kd /= ratio; 113 | SampleTime = (unsigned long)NewSampleTime; 114 | } 115 | } 116 | 117 | /* SetOutputLimits(...)**************************************************** 118 | * This function will be used far more often than SetInputLimits. while 119 | * the input to the controller will generally be in the 0-1023 range (which is 120 | * the default already,) the output will be a little different. maybe they'll 121 | * be doing a time window and will need 0-8000 or something. or maybe they'll 122 | * want to clamp it from 0-125. who knows. at any rate, that can all be done 123 | * here. 124 | **************************************************************************/ 125 | void PID::SetOutputLimits(double Min, double Max) 126 | { 127 | if(Min >= Max) return; 128 | outMin = Min; 129 | outMax = Max; 130 | 131 | if(inAuto) 132 | { 133 | if(*myOutput > outMax) *myOutput = outMax; 134 | else if(*myOutput < outMin) *myOutput = outMin; 135 | 136 | if(ITerm > outMax) ITerm= outMax; 137 | else if(ITerm < outMin) ITerm= outMin; 138 | } 139 | } 140 | 141 | /* SetMode(...)**************************************************************** 142 | * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) 143 | * when the transition from manual to auto occurs, the controller is 144 | * automatically initialized 145 | ******************************************************************************/ 146 | void PID::SetMode(int Mode) 147 | { 148 | bool newAuto = (Mode == AUTOMATIC); 149 | if(newAuto == !inAuto) 150 | { /*we just went from manual to auto*/ 151 | PID::Initialize(); 152 | } 153 | inAuto = newAuto; 154 | } 155 | 156 | /* Initialize()**************************************************************** 157 | * does all the things that need to happen to ensure a bumpless transfer 158 | * from manual to automatic mode. 159 | ******************************************************************************/ 160 | void PID::Initialize() 161 | { 162 | ITerm = *myOutput; 163 | lastInput = *myInput; 164 | if(ITerm > outMax) ITerm = outMax; 165 | else if(ITerm < outMin) ITerm = outMin; 166 | } 167 | 168 | /* SetControllerDirection(...)************************************************* 169 | * The PID will either be connected to a DIRECT acting process (+Output leads 170 | * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to 171 | * know which one, because otherwise we may increase the output when we should 172 | * be decreasing. This is called from the constructor. 173 | ******************************************************************************/ 174 | void PID::SetControllerDirection(int Direction) 175 | { 176 | if(inAuto && Direction !=controllerDirection) 177 | { 178 | kp = (0 - kp); 179 | ki = (0 - ki); 180 | kd = (0 - kd); 181 | } 182 | controllerDirection = Direction; 183 | } 184 | 185 | /* Status Funcions************************************************************* 186 | * Just because you set the Kp=-1 doesn't mean it actually happened. these 187 | * functions query the internal state of the PID. they're here for display 188 | * purposes. this are the functions the PID Front-end uses for example 189 | ******************************************************************************/ 190 | double PID::GetKp(){ return dispKp; } 191 | double PID::GetKi(){ return dispKi;} 192 | double PID::GetKd(){ return dispKd;} 193 | int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} 194 | int PID::GetDirection(){ return controllerDirection;} 195 | 196 | -------------------------------------------------------------------------------- /src/motor_control_and_plot.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | // STATES 7 | #define S_MOTOR_INIT 1 8 | #define S_MOTOR_RUN 2 9 | #define S_MOTOR_OFF 3 10 | #define S_OPTOTEST 4 11 | #define S_REPORT 5 12 | #define S_PLOT 6 13 | #define S_FORWARD 7 14 | 15 | char states[8][15] = { 16 | "DUMMY", 17 | "S_MOTOR_INIT", 18 | "S_MOTOR_RUN", 19 | "S_MOTOR_OFF", 20 | "S_OPTOTEST", 21 | "S_REPORT", 22 | "S_PLOT", 23 | "S_FORWARD" 24 | }; 25 | 26 | #define SERIAL_SPEED 115200 27 | 28 | // PINS 29 | #define ENC_A 14 30 | #define ENC_B 15 31 | 32 | #define IN_A 11 33 | #define IN_B 9 34 | #define PWM_PIN 10 35 | 36 | String inputString = ""; // a string to hold incoming data 37 | 38 | #define maxCount 500 39 | int start = 0; 40 | int positionCount = 0; 41 | int positionArray[maxCount]; 42 | int otherArray[maxCount]; 43 | 44 | double kp=0.5; 45 | double ki=0.0; 46 | double kd=0.0; 47 | double position=0, output=0, target=0; 48 | PID myPID(&position, &output, &target, kp, ki, kd, DIRECT); 49 | 50 | boolean counting=false; 51 | 52 | int state = S_MOTOR_OFF; 53 | int prevState = state; 54 | 55 | int reporttoggle = 0; 56 | 57 | bool dir = false; 58 | byte skip=0; 59 | 60 | Encoder encoder(ENC_A, ENC_B); 61 | 62 | // number of samples in rolling buffer 63 | int RAsamples = 100; 64 | RunningAverage RA1(RAsamples); 65 | 66 | void setup() { 67 | inputString.reserve(200); 68 | 69 | pinMode(IN_A, OUTPUT); 70 | pinMode(IN_B, OUTPUT); 71 | pinMode(PWM_PIN, OUTPUT); 72 | 73 | Serial.begin (SERIAL_SPEED); 74 | 75 | recoverPIDfromEEPROM(); 76 | 77 | //Setup the pid 78 | myPID.SetMode(AUTOMATIC); 79 | myPID.SetSampleTime(1); 80 | myPID.SetOutputLimits(-255,255); 81 | } 82 | 83 | void loop(){ 84 | switch (state) { 85 | case S_REPORT: 86 | position = encoder.read(); 87 | printPos(); 88 | state = S_MOTOR_OFF; 89 | break; 90 | case S_PLOT: 91 | plot_data(); 92 | state = S_MOTOR_OFF; 93 | break; 94 | case S_FORWARD: 95 | digitalWrite(IN_A, LOW); 96 | digitalWrite(IN_B, HIGH); 97 | analogWrite(PWM_PIN,200); 98 | break; 99 | case S_MOTOR_INIT: 100 | // clear array 101 | for(int i=0; i 0)) { 159 | if (cmd.equals("probe_device")) { 160 | Serial.println("PID_device"); 161 | state = S_MOTOR_OFF; 162 | } 163 | if (cmd.equals("p")) { 164 | Serial.println("pause"); 165 | state = S_MOTOR_OFF; 166 | } 167 | if (cmd.equals("zero")) { 168 | Serial.println("zero encoder"); 169 | encoder.write(0); 170 | state = S_MOTOR_OFF; 171 | } 172 | if (cmd.equals("report")) { 173 | Serial.println("reporting"); 174 | prevState = state; 175 | state = S_REPORT; 176 | } 177 | if (cmd.equals("F")) { 178 | Serial.println("forward"); 179 | state = S_FORWARD; 180 | } 181 | if (cmd.equals("W")) { 182 | Serial.println("W - write EEPROM:"); 183 | state = S_MOTOR_OFF; 184 | writetoEEPROM(); 185 | } 186 | if (cmd.equals("dump")) { 187 | Serial.println("dumping EEPROM:"); 188 | state = S_MOTOR_OFF; 189 | eedump(); 190 | } 191 | if (cmd.equals("reset")) { 192 | Serial.println("get PIDs from EEPROM"); 193 | state = S_MOTOR_OFF; 194 | recoverPIDfromEEPROM() ; 195 | } 196 | if (cmd.equals("plot")) { 197 | state = S_PLOT; 198 | } 199 | if (cmd.equals("P")) { 200 | Serial.println("set P"); 201 | kp = value.toFloat(); 202 | state = S_MOTOR_OFF; 203 | } 204 | if (cmd.equals("I")) { 205 | Serial.println("set I"); 206 | ki = value.toFloat(); 207 | state = S_MOTOR_OFF; 208 | } 209 | if (cmd.equals("D")) { 210 | Serial.println("set D"); 211 | kd = value.toFloat(); 212 | state = S_MOTOR_OFF; 213 | } 214 | if (cmd.equals("target")) { 215 | Serial.print("target: "); 216 | target = value.toInt(); 217 | Serial.println(target); 218 | state = S_MOTOR_INIT; 219 | } 220 | if (cmd.equals("zt")) { 221 | Serial.print("zero then target: "); 222 | target = value.toInt(); 223 | Serial.println(target); 224 | encoder.write(0); 225 | state = S_MOTOR_INIT; 226 | } 227 | if (cmd.equals("?")) { 228 | Serial.println("help:"); 229 | state = S_MOTOR_OFF; 230 | } 231 | inputString = ""; 232 | } 233 | } 234 | 235 | void serialEvent() { 236 | while (Serial.available()) { 237 | char inChar = (char)Serial.read(); 238 | inputString += inChar; 239 | if (inChar == '\n') { 240 | handle_cmd(); 241 | } 242 | } 243 | } 244 | 245 | void plot_data() { 246 | Serial.println("plot"); 247 | Serial.print("positions "); 248 | for(int i=0; i < maxCount - 1; i++) { 249 | Serial.print(positionArray[i]); 250 | Serial.print(" "); 251 | } 252 | Serial.println(positionArray[maxCount - 1]); 253 | 254 | Serial.print("times "); 255 | for(int i=0; i < maxCount - 1; i++) { 256 | Serial.print(otherArray[i]); 257 | Serial.print(" "); 258 | } 259 | Serial.println(otherArray[maxCount - 1]); 260 | 261 | position = encoder.read(); 262 | Serial.print("position "); 263 | Serial.println(position); 264 | 265 | Serial.print("target "); 266 | Serial.println(target); 267 | 268 | Serial.print("start "); 269 | Serial.println(start); 270 | 271 | Serial.print("kP "); 272 | Serial.println(kp); 273 | 274 | Serial.print("kI "); 275 | Serial.println(ki); 276 | 277 | Serial.print("kD "); 278 | Serial.println(kd); 279 | } 280 | 281 | void printPos() { 282 | Serial.print(F("Position=")); 283 | Serial.print(position); 284 | Serial.print(F(" PID_output=")); 285 | Serial.print(output); 286 | Serial.print(F(" Target=")); 287 | Serial.println(target); 288 | Serial.print(F("P=")); 289 | Serial.print(kp); 290 | Serial.print(F(" I=")); 291 | Serial.print(ki); 292 | Serial.print(F(" D=")); 293 | Serial.println(kd); 294 | Serial.print(F(" state=")); 295 | Serial.println(states[prevState]); 296 | } 297 | 298 | void help() { 299 | } 300 | 301 | // keep PID set values in EEPROM so they are kept when arduino goes off 302 | void writetoEEPROM() { 303 | eeput(kp,0); 304 | eeput(ki,4); 305 | eeput(kd,8); 306 | double cks=0; 307 | for(int i=0; i<12; i++) cks+=EEPROM.read(i); 308 | eeput(cks,12); 309 | Serial.println("\nPID values stored to EEPROM"); 310 | //Serial.println(cks); 311 | } 312 | 313 | void recoverPIDfromEEPROM() { 314 | double cks=0; 315 | double cksEE; 316 | for(int i=0; i<12; i++) cks+=EEPROM.read(i); 317 | cksEE=eeget(12); 318 | //Serial.println(cks); 319 | if(cks==cksEE) { 320 | kp=eeget(0); 321 | ki=eeget(4); 322 | kd=eeget(8); 323 | myPID.SetTunings(kp,ki,kd); 324 | } 325 | else Serial.println(F("*** Bad checksum")); 326 | } 327 | 328 | void eeput(double value, int dir) { 329 | char * addr = (char * ) &value; 330 | for(int i=dir; i 2 | #include 3 | 4 | #define SERIAL_SPEED 115200 5 | #define S_START 1 6 | #define S_RUNNING 2 7 | #define S_REPORT 3 8 | #define S_PLOT 4 9 | #define S_STOP 6 10 | #define S_RANDOM_START 7 11 | #define S_RANDOM 8 12 | 13 | char states[8][15] = { 14 | "DUMMY", 15 | "S_START", 16 | "S_RUNNING", 17 | "S_REPORT", 18 | "S_PLOT", 19 | "S_STOP", 20 | "S_RANDOM_START", 21 | "S_RANDOM" 22 | }; 23 | 24 | long previousMillis = 0; 25 | long currentMillis = 0; 26 | long interval = 1000; 27 | 28 | double kp=0.5; 29 | double ki=0.0; 30 | double kd=0.0; 31 | double position=0, output=0, target=0; 32 | PID myPID(&position, &output, &target, kp, ki, kd, DIRECT); 33 | 34 | int LEDPin = 13; 35 | int en_1 = 9; 36 | int in_1A = 10; 37 | int in_1B = 11; 38 | int sliderPin = A0; 39 | int inputPin = A1; 40 | 41 | int frequency = 1; 42 | int state = S_STOP; 43 | int prevState; 44 | boolean usePotentiometer = true; 45 | 46 | String inputString = ""; // a string to hold incoming data 47 | 48 | void setup() { 49 | Serial.begin(SERIAL_SPEED); 50 | inputString.reserve(200); 51 | 52 | pinMode(en_1, OUTPUT); 53 | pinMode(in_1A, OUTPUT); 54 | pinMode(in_1B, OUTPUT); 55 | 56 | pinMode(LEDPin, OUTPUT); 57 | 58 | recoverPIDfromEEPROM(); 59 | 60 | //Setup the pid 61 | myPID.SetMode(AUTOMATIC); 62 | myPID.SetSampleTime(1); 63 | myPID.SetOutputLimits(-255,255); 64 | 65 | Serial.println("lets rock"); 66 | 67 | setPWMFrequency(en_1, frequency); 68 | } 69 | 70 | void loop() { 71 | switch (state) { 72 | case S_START: 73 | myPID.SetTunings(kp,ki,kd); 74 | state = S_RUNNING; 75 | digitalWrite(LEDPin, HIGH); 76 | break; 77 | case S_RUNNING: 78 | if (usePotentiometer) { 79 | target = analogRead(inputPin); 80 | } 81 | else { 82 | // target comes from somewhere else 83 | } 84 | position = analogRead(sliderPin); 85 | 86 | target = constrain(target, 0, 1024); 87 | 88 | while(!myPID.Compute()); // wait to compute PID 89 | 90 | if(output < 0) { 91 | digitalWrite(in_1A, LOW); 92 | digitalWrite(in_1B, HIGH); 93 | analogWrite(en_1,abs(output)); 94 | } 95 | else { 96 | digitalWrite(in_1A, HIGH); 97 | digitalWrite(in_1B, LOW); 98 | analogWrite(en_1,abs(output)); 99 | } 100 | state = S_RUNNING; 101 | break; 102 | case S_RANDOM_START: 103 | myPID.SetTunings(kp,ki,kd); 104 | target = random(1024); 105 | target = constrain(target, 40, 950); 106 | Serial.println(target); 107 | digitalWrite(LEDPin, HIGH); 108 | previousMillis = millis(); 109 | state = S_RANDOM; 110 | break; 111 | case S_RANDOM: 112 | position = analogRead(sliderPin); 113 | while(!myPID.Compute()); // wait to compute PID 114 | 115 | if(output < 0) { 116 | digitalWrite(in_1A, LOW); 117 | digitalWrite(in_1B, HIGH); 118 | analogWrite(en_1,abs(output)); 119 | } 120 | else { 121 | digitalWrite(in_1A, HIGH); 122 | digitalWrite(in_1B, LOW); 123 | analogWrite(en_1,abs(output)); 124 | } 125 | state = S_RANDOM; 126 | 127 | currentMillis = millis(); 128 | if(currentMillis - previousMillis > interval) { 129 | state = S_RANDOM_START; 130 | } 131 | break; 132 | case S_REPORT: 133 | Serial.println("kP :: kI :: kD"); 134 | Serial.print(kp); 135 | Serial.print(" :: "); 136 | Serial.print(ki); 137 | Serial.print(" :: "); 138 | Serial.println(kd); 139 | 140 | Serial.println("position :: target"); 141 | Serial.print(analogRead(sliderPin)); 142 | Serial.print(" :: "); 143 | Serial.println(target); 144 | 145 | Serial.println(states[prevState]); 146 | 147 | state = S_STOP; 148 | 149 | break; 150 | 151 | case S_STOP: 152 | // Turn off motor 153 | digitalWrite(LEDPin, LOW); 154 | digitalWrite(in_1A, LOW); 155 | digitalWrite(in_1B, LOW); 156 | analogWrite(en_1, 0); 157 | state = S_STOP; 158 | break; 159 | default: 160 | Serial.println ("Unknown state") ; 161 | break; 162 | } 163 | 164 | } 165 | 166 | void handle_cmd() { 167 | inputString.trim(); // removes beginning and ending white spaces 168 | int idx = inputString.indexOf(' '); 169 | String cmd = inputString.substring(0, idx); 170 | String value = inputString.substring(idx + 1); 171 | 172 | if ((cmd.length() > 0)) { 173 | if (cmd.equals("probe_device")) { 174 | Serial.println("PID_device"); 175 | state = S_STOP; 176 | } 177 | if (cmd.equals("p")) { 178 | Serial.println("pause"); 179 | state = S_STOP; 180 | } 181 | if (cmd.equals("report")) { 182 | Serial.println("reporting"); 183 | prevState = state; 184 | state = S_REPORT; 185 | } 186 | if (cmd.equals("random")) { 187 | Serial.println("random movement"); 188 | usePotentiometer = false; 189 | state = S_RANDOM_START; 190 | } 191 | if (cmd.equals("W")) { 192 | Serial.println("W - write EEPROM:"); 193 | state = S_STOP; 194 | writetoEEPROM(); 195 | } 196 | if (cmd.equals("dump")) { 197 | Serial.println("dumping EEPROM:"); 198 | state = S_STOP; 199 | eedump(); 200 | } 201 | if (cmd.equals("reset")) { 202 | Serial.println("get PIDs from EEPROM"); 203 | state = S_STOP; 204 | recoverPIDfromEEPROM() ; 205 | } 206 | if (cmd.equals("plot")) { 207 | state = S_PLOT; 208 | } 209 | if (cmd.equals("R")) { 210 | Serial.println("run potentiometer control"); 211 | usePotentiometer = true; 212 | state = S_START; 213 | } 214 | if (cmd.equals("F")) { 215 | frequency = value.toInt(); 216 | setPWMFrequency(en_1, frequency); 217 | Serial.println("frequencey"); 218 | state = S_STOP; 219 | } 220 | if (cmd.equals("P")) { 221 | Serial.println("set P"); 222 | kp = value.toFloat(); 223 | state = S_STOP; 224 | } 225 | if (cmd.equals("I")) { 226 | Serial.println("set I"); 227 | ki = value.toFloat(); 228 | state = S_STOP; 229 | } 230 | if (cmd.equals("D")) { 231 | Serial.println("set D"); 232 | kd = value.toFloat(); 233 | state = S_STOP; 234 | } 235 | if (cmd.equals("target")) { 236 | Serial.print("target: "); 237 | target = value.toInt(); 238 | Serial.println(target); 239 | usePotentiometer = false; 240 | state = S_START; 241 | } 242 | if (cmd.equals("B") || cmd.equals("b")) { 243 | target = 1024 - target; 244 | Serial.print("bounce: "); 245 | Serial.println(target); 246 | usePotentiometer = false; 247 | state = S_START; 248 | } 249 | if (cmd.equals("?")) { 250 | Serial.println("help:"); 251 | state = S_STOP; 252 | } 253 | inputString = ""; 254 | } 255 | } 256 | 257 | void serialEvent() { 258 | while (Serial.available()) { 259 | char inChar = (char)Serial.read(); 260 | inputString += inChar; 261 | if (inChar == '\n') { 262 | handle_cmd(); 263 | } 264 | } 265 | } 266 | 267 | void setPWMFrequency(int pin, int divisor) { 268 | byte mode; 269 | if(pin == 5 || pin == 6 || pin == 9 || pin == 10) { 270 | switch(divisor) { 271 | case 1: mode = 0x01; break; 272 | case 8: mode = 0x02; break; 273 | case 64: mode = 0x03; break; 274 | case 256: mode = 0x04; break; 275 | case 1024: mode = 0x05; break; 276 | default: return; 277 | } 278 | if(pin == 5 || pin == 6) { 279 | TCCR0B = TCCR0B & 0b11111000 | mode; 280 | } else { 281 | TCCR1B = TCCR1B & 0b11111000 | mode; 282 | } 283 | } else if(pin == 3 || pin == 11) { 284 | switch(divisor) { 285 | case 1: mode = 0x01; break; 286 | case 8: mode = 0x02; break; 287 | case 32: mode = 0x03; break; 288 | case 64: mode = 0x04; break; 289 | case 128: mode = 0x05; break; 290 | case 256: mode = 0x06; break; 291 | case 1024: mode = 0x07; break; 292 | default: return; 293 | } 294 | TCCR2B = TCCR2B & 0b11111000 | mode; 295 | } 296 | } 297 | 298 | // keep PID set values in EEPROM so they are kept when arduino goes off 299 | void writetoEEPROM() { 300 | eeput(kp,0); 301 | eeput(ki,4); 302 | eeput(kd,8); 303 | double cks=0; 304 | for(int i=0; i<12; i++) cks+=EEPROM.read(i); 305 | eeput(cks,12); 306 | Serial.println("\nPID values stored to EEPROM"); 307 | } 308 | 309 | void recoverPIDfromEEPROM() { 310 | double cks=0; 311 | double cksEE; 312 | for(int i=0; i<12; i++) cks+=EEPROM.read(i); 313 | cksEE=eeget(12); 314 | if(cks==cksEE) { 315 | kp=eeget(0); 316 | ki=eeget(4); 317 | kd=eeget(8); 318 | myPID.SetTunings(kp,ki,kd); 319 | } 320 | else Serial.println(F("*** Bad checksum")); 321 | } 322 | 323 | void eeput(double value, int dir) { 324 | char * addr = (char * ) &value; 325 | for(int i=dir; i 0)) { 86 | if (cmd.equals("probe_device")) { 87 | Serial.println("PID_device"); 88 | state = S_MOTOR_OFF; 89 | } 90 | if (cmd.equals("p")) { 91 | Serial.println("pause"); 92 | state = S_MOTOR_OFF; 93 | } 94 | if (cmd.equals("zero")) { 95 | Serial.println("zero encoder"); 96 | encoder.write(0); 97 | state = S_MOTOR_OFF; 98 | } 99 | if (cmd.equals("report")) { 100 | Serial.println("reporting"); 101 | prevState = state; 102 | state = S_REPORT; 103 | } 104 | if (cmd.equals("W")) { 105 | Serial.println("W - write EEPROM:"); 106 | state = S_MOTOR_OFF; 107 | writetoEEPROM(); 108 | } 109 | . 110 | . 111 | . 112 | etc 113 | ``` 114 | 115 | handle_cmd() is invoked when anything comes in from the serial. It's important to note that it's called from an interrupt routine called `serialEvent()`. Since it's being invoked from an interrupt, I like to have it do as little as possible and then return back to the rest of the program. So pretty much all handle_cmd() does is parse a command, then based on that command print out something to the serial and set a variable like: 116 | 117 | `state = S_MOTOR_INIT;` 118 | 119 | 120 | ## Commands to send to the arduino 121 | The following are examples of commands that can be submitted to handle_cmd(): 122 | 123 | * `report` - does a text dump of various variables. 124 | * `W` - write variables kP, kI, and kD to the eeprom 125 | * `dump` - dump all eeprom variables out as text 126 | * `reset` - restore the current kP, kI, and kD variables from what's in eeprom. 127 | * `P 10.0 ` - sets kP to 10.0 128 | * `I 0.002` - sets kI to 0.002 129 | * `D 1.0 ` - sets kD to 1.0 130 | * `target 100` - sets the target to 100 131 | * `plot` - sends data to the python program for graphing 132 | * `P` - pause the motor. 133 | * `zero` - sets the encoder counter to zero. 134 | * `probe_device` - returns a name for the device, in this case "PID_device". I like this when multiple devices are hanging off the same computer, then my python code can poll all the devices and find the one it's interested in. 135 | 136 | Note: you can use all of those commands in the serial terminal of the arduino IDE. To give it a try go to Tools-->serial monitor. 137 | 138 | Moving on. The `setup()` function is not particularly interesting. The function cal `recoverPIDfromEEPROM();` handles retrieving the variables `kP`, `kI`, and `kD` from the eeprom. The `loop()` function looks like this: 139 | 140 | Hopefully you're familiar with state machines. These are really handy if you get tired of having `if {this1} else if {this2} else if {this3}...`. There are some super fast programs for things like balancing robots that use state machines. The basis of this section is the switch statement. Where: 141 | 142 | 143 | ``` 144 | // state = some_value like S_REPORT, S_MOTOR_RUN or some other integer 145 | 146 | switch (state) { 147 | case S_REPORT: 148 | position = encoder.read(); 149 | printPos(); 150 | state = S_MOTOR_OFF; 151 | break; 152 | case S_MOTOR_RUN: 153 | . 154 | . 155 | . 156 | etc 157 | ``` 158 | 159 | The most relevant section of the loop is: 160 | ``` 161 | case S_MOTOR_INIT: 162 | // clear array 163 | for(int i=0; i 0: 242 | buffer += ser.read(1) 243 | if (re.match(r'plot', buffer)): 244 | l.handle_plot(buffer) 245 | else: 246 | print ("%s %s") % (port, buffer) 247 | 248 | except KeyboardInterrupt: 249 | print "Bye" 250 | ser.close() 251 | sys.exit() 252 | ``` 253 | 254 | in the python code handles accepting keyboard input from your computer, sending it to the arduino, and then receiving the response from the arduino. You can use all the [commands](#commands) that we specified before. If you want to exit, type exit, or Control-c. Here's an example session. First start the python program one the comman line like this: 255 | ``` 256 | $ ./serial_n_plot.py 257 | ``` 258 | and then here is an example session: 259 | 260 | ``` 261 | owens-MacBook-Air-2:PID_motor_control owhite$ ./serial_n_plot.py 262 | found: /dev/tty.usbmodem2335471 263 | Enter Message:report 264 | sending report 265 | /dev/tty.usbmodem2335471 reporting 266 | Position=86.00 PID_output=44.80 Target=200.00 267 | P=0.40 I=0.00 D=0.00 268 | state=S_MOTOR_OFF 269 | 270 | Enter Message:P 0.5 271 | sending P 0.5 272 | /dev/tty.usbmodem2335471 set P 273 | 274 | Enter Message:target 250 275 | sending target 250 276 | /dev/tty.usbmodem2335471 target: 250.00 277 | 278 | Enter Message:report 279 | sending report 280 | /dev/tty.usbmodem2335471 reporting 281 | Position=252.00 PID_output=-1.00 Target=250.00 282 | P=0.50 I=0.00 D=0.00 283 | state=S_MOTOR_RUN 284 | 285 | Enter Message:plot 286 | sending plot 287 | 288 | ``` 289 | 290 | you're motor may have different behavior but when you hit plot, you might get a result like this: 291 | 292 | ![PID graph](http://i.imgur.com/hw9jrO1.png) 293 | 294 | and the cool thing is, this is not some theoretical plot that you found on the net, it actually the behavior of the last `target 250` that you just ran. 295 | 296 | Note, when the plot comes up you have to use your mouse to close the window, and then it returns control back to the command line that is running the python code. Be gentle about exiting the python program. Control-c and `quit` is the way to go, otherwise you may gum up the serial port and will probably need to reboot. 297 | 298 | ## Conclusion 299 | 300 | Okay, I have yet to tune my little DC motor, but the next thing I'm going to do is read ["PID Without a PhD"](http://www.wescottdesign.com/articles/pid/pidWithoutAPhd.pdf) and start trying out what it says in the tuning section. (I do have a PhD by way, but it's in plant molecular biology.) 301 | 302 | Below is an example of a system that could greatly benifit from tuned PID: 303 | 304 | [![IMAGE ALT TEXT HERE](http://img.youtube.com/vi/8a_wUuJFRQk/0.jpg)](http://www.youtube.com/watch?v=8a_wUuJFRQk) 305 | 306 | -------------------------------------------------------------------------------- /src/Encoder.h: -------------------------------------------------------------------------------- 1 | /* Encoder Library, for measuring quadrature encoded signals 2 | * http://www.pjrc.com/teensy/td_libs_Encoder.html 3 | * Copyright (c) 2011,2013 PJRC.COM, LLC - Paul Stoffregen 4 | * 5 | * Version 1.2 - fix -2 bug in C-only code 6 | * Version 1.1 - expand to support boards with up to 60 interrupts 7 | * Version 1.0 - initial release 8 | * 9 | * Permission is hereby granted, free of charge, to any person obtaining a copy 10 | * of this software and associated documentation files (the "Software"), to deal 11 | * in the Software without restriction, including without limitation the rights 12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 13 | * copies of the Software, and to permit persons to whom the Software is 14 | * furnished to do so, subject to the following conditions: 15 | * 16 | * The above copyright notice and this permission notice shall be included in 17 | * all copies or substantial portions of the Software. 18 | * 19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 25 | * THE SOFTWARE. 26 | */ 27 | 28 | 29 | #ifndef Encoder_h_ 30 | #define Encoder_h_ 31 | 32 | #if defined(ARDUINO) && ARDUINO >= 100 33 | #include "Arduino.h" 34 | #elif defined(WIRING) 35 | #include "Wiring.h" 36 | #else 37 | #include "WProgram.h" 38 | #include "pins_arduino.h" 39 | #endif 40 | 41 | #include "utility/direct_pin_read.h" 42 | 43 | #if defined(ENCODER_USE_INTERRUPTS) || !defined(ENCODER_DO_NOT_USE_INTERRUPTS) 44 | #define ENCODER_USE_INTERRUPTS 45 | #define ENCODER_ARGLIST_SIZE CORE_NUM_INTERRUPT 46 | #include "utility/interrupt_pins.h" 47 | #ifdef ENCODER_OPTIMIZE_INTERRUPTS 48 | #include "utility/interrupt_config.h" 49 | #endif 50 | #else 51 | #define ENCODER_ARGLIST_SIZE 0 52 | #endif 53 | 54 | 55 | 56 | // All the data needed by interrupts is consolidated into this ugly struct 57 | // to facilitate assembly language optimizing of the speed critical update. 58 | // The assembly code uses auto-incrementing addressing modes, so the struct 59 | // must remain in exactly this order. 60 | typedef struct { 61 | volatile IO_REG_TYPE * pin1_register; 62 | volatile IO_REG_TYPE * pin2_register; 63 | IO_REG_TYPE pin1_bitmask; 64 | IO_REG_TYPE pin2_bitmask; 65 | uint8_t state; 66 | int32_t position; 67 | } Encoder_internal_state_t; 68 | 69 | class Encoder 70 | { 71 | public: 72 | Encoder(uint8_t pin1, uint8_t pin2) { 73 | #ifdef INPUT_PULLUP 74 | pinMode(pin1, INPUT_PULLUP); 75 | pinMode(pin2, INPUT_PULLUP); 76 | #else 77 | pinMode(pin1, INPUT); 78 | digitalWrite(pin1, HIGH); 79 | pinMode(pin2, INPUT); 80 | digitalWrite(pin2, HIGH); 81 | #endif 82 | encoder.pin1_register = PIN_TO_BASEREG(pin1); 83 | encoder.pin1_bitmask = PIN_TO_BITMASK(pin1); 84 | encoder.pin2_register = PIN_TO_BASEREG(pin2); 85 | encoder.pin2_bitmask = PIN_TO_BITMASK(pin2); 86 | encoder.position = 0; 87 | // allow time for a passive R-C filter to charge 88 | // through the pullup resistors, before reading 89 | // the initial state 90 | delayMicroseconds(2000); 91 | uint8_t s = 0; 92 | if (DIRECT_PIN_READ(encoder.pin1_register, encoder.pin1_bitmask)) s |= 1; 93 | if (DIRECT_PIN_READ(encoder.pin2_register, encoder.pin2_bitmask)) s |= 2; 94 | encoder.state = s; 95 | #ifdef ENCODER_USE_INTERRUPTS 96 | interrupts_in_use = attach_interrupt(pin1, &encoder); 97 | interrupts_in_use += attach_interrupt(pin2, &encoder); 98 | #endif 99 | //update_finishup(); // to force linker to include the code (does not work) 100 | } 101 | 102 | 103 | #ifdef ENCODER_USE_INTERRUPTS 104 | inline int32_t read() { 105 | if (interrupts_in_use < 2) { 106 | noInterrupts(); 107 | update(&encoder); 108 | } else { 109 | noInterrupts(); 110 | } 111 | int32_t ret = encoder.position; 112 | interrupts(); 113 | return ret; 114 | } 115 | inline void write(int32_t p) { 116 | noInterrupts(); 117 | encoder.position = p; 118 | interrupts(); 119 | } 120 | #else 121 | inline int32_t read() { 122 | update(&encoder); 123 | return encoder.position; 124 | } 125 | inline void write(int32_t p) { 126 | encoder.position = p; 127 | } 128 | #endif 129 | private: 130 | Encoder_internal_state_t encoder; 131 | #ifdef ENCODER_USE_INTERRUPTS 132 | uint8_t interrupts_in_use; 133 | #endif 134 | public: 135 | static Encoder_internal_state_t * interruptArgs[ENCODER_ARGLIST_SIZE]; 136 | 137 | // _______ _______ 138 | // Pin1 ______| |_______| |______ Pin1 139 | // negative <--- _______ _______ __ --> positive 140 | // Pin2 __| |_______| |_______| Pin2 141 | 142 | // new new old old 143 | // pin2 pin1 pin2 pin1 Result 144 | // ---- ---- ---- ---- ------ 145 | // 0 0 0 0 no movement 146 | // 0 0 0 1 +1 147 | // 0 0 1 0 -1 148 | // 0 0 1 1 +2 (assume pin1 edges only) 149 | // 0 1 0 0 -1 150 | // 0 1 0 1 no movement 151 | // 0 1 1 0 -2 (assume pin1 edges only) 152 | // 0 1 1 1 +1 153 | // 1 0 0 0 +1 154 | // 1 0 0 1 -2 (assume pin1 edges only) 155 | // 1 0 1 0 no movement 156 | // 1 0 1 1 -1 157 | // 1 1 0 0 +2 (assume pin1 edges only) 158 | // 1 1 0 1 -1 159 | // 1 1 1 0 +1 160 | // 1 1 1 1 no movement 161 | /* 162 | // Simple, easy-to-read "documentation" version :-) 163 | // 164 | void update(void) { 165 | uint8_t s = state & 3; 166 | if (digitalRead(pin1)) s |= 4; 167 | if (digitalRead(pin2)) s |= 8; 168 | switch (s) { 169 | case 0: case 5: case 10: case 15: 170 | break; 171 | case 1: case 7: case 8: case 14: 172 | position++; break; 173 | case 2: case 4: case 11: case 13: 174 | position--; break; 175 | case 3: case 12: 176 | position += 2; break; 177 | default: 178 | position -= 2; break; 179 | } 180 | state = (s >> 2); 181 | } 182 | */ 183 | 184 | private: 185 | static void update(Encoder_internal_state_t *arg) { 186 | #if defined(__AVR__) 187 | // The compiler believes this is just 1 line of code, so 188 | // it will inline this function into each interrupt 189 | // handler. That's a tiny bit faster, but grows the code. 190 | // Especially when used with ENCODER_OPTIMIZE_INTERRUPTS, 191 | // the inline nature allows the ISR prologue and epilogue 192 | // to only save/restore necessary registers, for very nice 193 | // speed increase. 194 | asm volatile ( 195 | "ld r30, X+" "\n\t" 196 | "ld r31, X+" "\n\t" 197 | "ld r24, Z" "\n\t" // r24 = pin1 input 198 | "ld r30, X+" "\n\t" 199 | "ld r31, X+" "\n\t" 200 | "ld r25, Z" "\n\t" // r25 = pin2 input 201 | "ld r30, X+" "\n\t" // r30 = pin1 mask 202 | "ld r31, X+" "\n\t" // r31 = pin2 mask 203 | "ld r22, X" "\n\t" // r22 = state 204 | "andi r22, 3" "\n\t" 205 | "and r24, r30" "\n\t" 206 | "breq L%=1" "\n\t" // if (pin1) 207 | "ori r22, 4" "\n\t" // state |= 4 208 | "L%=1:" "and r25, r31" "\n\t" 209 | "breq L%=2" "\n\t" // if (pin2) 210 | "ori r22, 8" "\n\t" // state |= 8 211 | "L%=2:" "ldi r30, lo8(pm(L%=table))" "\n\t" 212 | "ldi r31, hi8(pm(L%=table))" "\n\t" 213 | "add r30, r22" "\n\t" 214 | "adc r31, __zero_reg__" "\n\t" 215 | "asr r22" "\n\t" 216 | "asr r22" "\n\t" 217 | "st X+, r22" "\n\t" // store new state 218 | "ld r22, X+" "\n\t" 219 | "ld r23, X+" "\n\t" 220 | "ld r24, X+" "\n\t" 221 | "ld r25, X+" "\n\t" 222 | "ijmp" "\n\t" // jumps to update_finishup() 223 | // TODO move this table to another static function, 224 | // so it doesn't get needlessly duplicated. Easier 225 | // said than done, due to linker issues and inlining 226 | "L%=table:" "\n\t" 227 | "rjmp L%=end" "\n\t" // 0 228 | "rjmp L%=plus1" "\n\t" // 1 229 | "rjmp L%=minus1" "\n\t" // 2 230 | "rjmp L%=plus2" "\n\t" // 3 231 | "rjmp L%=minus1" "\n\t" // 4 232 | "rjmp L%=end" "\n\t" // 5 233 | "rjmp L%=minus2" "\n\t" // 6 234 | "rjmp L%=plus1" "\n\t" // 7 235 | "rjmp L%=plus1" "\n\t" // 8 236 | "rjmp L%=minus2" "\n\t" // 9 237 | "rjmp L%=end" "\n\t" // 10 238 | "rjmp L%=minus1" "\n\t" // 11 239 | "rjmp L%=plus2" "\n\t" // 12 240 | "rjmp L%=minus1" "\n\t" // 13 241 | "rjmp L%=plus1" "\n\t" // 14 242 | "rjmp L%=end" "\n\t" // 15 243 | "L%=minus2:" "\n\t" 244 | "subi r22, 2" "\n\t" 245 | "sbci r23, 0" "\n\t" 246 | "sbci r24, 0" "\n\t" 247 | "sbci r25, 0" "\n\t" 248 | "rjmp L%=store" "\n\t" 249 | "L%=minus1:" "\n\t" 250 | "subi r22, 1" "\n\t" 251 | "sbci r23, 0" "\n\t" 252 | "sbci r24, 0" "\n\t" 253 | "sbci r25, 0" "\n\t" 254 | "rjmp L%=store" "\n\t" 255 | "L%=plus2:" "\n\t" 256 | "subi r22, 254" "\n\t" 257 | "rjmp L%=z" "\n\t" 258 | "L%=plus1:" "\n\t" 259 | "subi r22, 255" "\n\t" 260 | "L%=z:" "sbci r23, 255" "\n\t" 261 | "sbci r24, 255" "\n\t" 262 | "sbci r25, 255" "\n\t" 263 | "L%=store:" "\n\t" 264 | "st -X, r25" "\n\t" 265 | "st -X, r24" "\n\t" 266 | "st -X, r23" "\n\t" 267 | "st -X, r22" "\n\t" 268 | "L%=end:" "\n" 269 | : : "x" (arg) : "r22", "r23", "r24", "r25", "r30", "r31"); 270 | #else 271 | uint8_t p1val = DIRECT_PIN_READ(arg->pin1_register, arg->pin1_bitmask); 272 | uint8_t p2val = DIRECT_PIN_READ(arg->pin2_register, arg->pin2_bitmask); 273 | uint8_t state = arg->state & 3; 274 | if (p1val) state |= 4; 275 | if (p2val) state |= 8; 276 | arg->state = (state >> 2); 277 | switch (state) { 278 | case 1: case 7: case 8: case 14: 279 | arg->position++; 280 | return; 281 | case 2: case 4: case 11: case 13: 282 | arg->position--; 283 | return; 284 | case 3: case 12: 285 | arg->position += 2; 286 | return; 287 | case 6: case 9: 288 | arg->position -= 2; 289 | return; 290 | } 291 | #endif 292 | } 293 | /* 294 | #if defined(__AVR__) 295 | // TODO: this must be a no inline function 296 | // even noinline does not seem to solve difficult 297 | // problems with this. Oh well, it was only meant 298 | // to shrink code size - there's no performance 299 | // improvement in this, only code size reduction. 300 | __attribute__((noinline)) void update_finishup(void) { 301 | asm volatile ( 302 | "ldi r30, lo8(pm(Ltable))" "\n\t" 303 | "ldi r31, hi8(pm(Ltable))" "\n\t" 304 | "Ltable:" "\n\t" 305 | "rjmp L%=end" "\n\t" // 0 306 | "rjmp L%=plus1" "\n\t" // 1 307 | "rjmp L%=minus1" "\n\t" // 2 308 | "rjmp L%=plus2" "\n\t" // 3 309 | "rjmp L%=minus1" "\n\t" // 4 310 | "rjmp L%=end" "\n\t" // 5 311 | "rjmp L%=minus2" "\n\t" // 6 312 | "rjmp L%=plus1" "\n\t" // 7 313 | "rjmp L%=plus1" "\n\t" // 8 314 | "rjmp L%=minus2" "\n\t" // 9 315 | "rjmp L%=end" "\n\t" // 10 316 | "rjmp L%=minus1" "\n\t" // 11 317 | "rjmp L%=plus2" "\n\t" // 12 318 | "rjmp L%=minus1" "\n\t" // 13 319 | "rjmp L%=plus1" "\n\t" // 14 320 | "rjmp L%=end" "\n\t" // 15 321 | "L%=minus2:" "\n\t" 322 | "subi r22, 2" "\n\t" 323 | "sbci r23, 0" "\n\t" 324 | "sbci r24, 0" "\n\t" 325 | "sbci r25, 0" "\n\t" 326 | "rjmp L%=store" "\n\t" 327 | "L%=minus1:" "\n\t" 328 | "subi r22, 1" "\n\t" 329 | "sbci r23, 0" "\n\t" 330 | "sbci r24, 0" "\n\t" 331 | "sbci r25, 0" "\n\t" 332 | "rjmp L%=store" "\n\t" 333 | "L%=plus2:" "\n\t" 334 | "subi r22, 254" "\n\t" 335 | "rjmp L%=z" "\n\t" 336 | "L%=plus1:" "\n\t" 337 | "subi r22, 255" "\n\t" 338 | "L%=z:" "sbci r23, 255" "\n\t" 339 | "sbci r24, 255" "\n\t" 340 | "sbci r25, 255" "\n\t" 341 | "L%=store:" "\n\t" 342 | "st -X, r25" "\n\t" 343 | "st -X, r24" "\n\t" 344 | "st -X, r23" "\n\t" 345 | "st -X, r22" "\n\t" 346 | "L%=end:" "\n" 347 | : : : "r22", "r23", "r24", "r25", "r30", "r31"); 348 | } 349 | #endif 350 | */ 351 | 352 | 353 | #ifdef ENCODER_USE_INTERRUPTS 354 | // this giant function is an unfortunate consequence of Arduino's 355 | // attachInterrupt function not supporting any way to pass a pointer 356 | // or other context to the attached function. 357 | static uint8_t attach_interrupt(uint8_t pin, Encoder_internal_state_t *state) { 358 | switch (pin) { 359 | #ifdef CORE_INT0_PIN 360 | case CORE_INT0_PIN: 361 | interruptArgs[0] = state; 362 | attachInterrupt(0, isr0, CHANGE); 363 | break; 364 | #endif 365 | #ifdef CORE_INT1_PIN 366 | case CORE_INT1_PIN: 367 | interruptArgs[1] = state; 368 | attachInterrupt(1, isr1, CHANGE); 369 | break; 370 | #endif 371 | #ifdef CORE_INT2_PIN 372 | case CORE_INT2_PIN: 373 | interruptArgs[2] = state; 374 | attachInterrupt(2, isr2, CHANGE); 375 | break; 376 | #endif 377 | #ifdef CORE_INT3_PIN 378 | case CORE_INT3_PIN: 379 | interruptArgs[3] = state; 380 | attachInterrupt(3, isr3, CHANGE); 381 | break; 382 | #endif 383 | #ifdef CORE_INT4_PIN 384 | case CORE_INT4_PIN: 385 | interruptArgs[4] = state; 386 | attachInterrupt(4, isr4, CHANGE); 387 | break; 388 | #endif 389 | #ifdef CORE_INT5_PIN 390 | case CORE_INT5_PIN: 391 | interruptArgs[5] = state; 392 | attachInterrupt(5, isr5, CHANGE); 393 | break; 394 | #endif 395 | #ifdef CORE_INT6_PIN 396 | case CORE_INT6_PIN: 397 | interruptArgs[6] = state; 398 | attachInterrupt(6, isr6, CHANGE); 399 | break; 400 | #endif 401 | #ifdef CORE_INT7_PIN 402 | case CORE_INT7_PIN: 403 | interruptArgs[7] = state; 404 | attachInterrupt(7, isr7, CHANGE); 405 | break; 406 | #endif 407 | #ifdef CORE_INT8_PIN 408 | case CORE_INT8_PIN: 409 | interruptArgs[8] = state; 410 | attachInterrupt(8, isr8, CHANGE); 411 | break; 412 | #endif 413 | #ifdef CORE_INT9_PIN 414 | case CORE_INT9_PIN: 415 | interruptArgs[9] = state; 416 | attachInterrupt(9, isr9, CHANGE); 417 | break; 418 | #endif 419 | #ifdef CORE_INT10_PIN 420 | case CORE_INT10_PIN: 421 | interruptArgs[10] = state; 422 | attachInterrupt(10, isr10, CHANGE); 423 | break; 424 | #endif 425 | #ifdef CORE_INT11_PIN 426 | case CORE_INT11_PIN: 427 | interruptArgs[11] = state; 428 | attachInterrupt(11, isr11, CHANGE); 429 | break; 430 | #endif 431 | #ifdef CORE_INT12_PIN 432 | case CORE_INT12_PIN: 433 | interruptArgs[12] = state; 434 | attachInterrupt(12, isr12, CHANGE); 435 | break; 436 | #endif 437 | #ifdef CORE_INT13_PIN 438 | case CORE_INT13_PIN: 439 | interruptArgs[13] = state; 440 | attachInterrupt(13, isr13, CHANGE); 441 | break; 442 | #endif 443 | #ifdef CORE_INT14_PIN 444 | case CORE_INT14_PIN: 445 | interruptArgs[14] = state; 446 | attachInterrupt(14, isr14, CHANGE); 447 | break; 448 | #endif 449 | #ifdef CORE_INT15_PIN 450 | case CORE_INT15_PIN: 451 | interruptArgs[15] = state; 452 | attachInterrupt(15, isr15, CHANGE); 453 | break; 454 | #endif 455 | #ifdef CORE_INT16_PIN 456 | case CORE_INT16_PIN: 457 | interruptArgs[16] = state; 458 | attachInterrupt(16, isr16, CHANGE); 459 | break; 460 | #endif 461 | #ifdef CORE_INT17_PIN 462 | case CORE_INT17_PIN: 463 | interruptArgs[17] = state; 464 | attachInterrupt(17, isr17, CHANGE); 465 | break; 466 | #endif 467 | #ifdef CORE_INT18_PIN 468 | case CORE_INT18_PIN: 469 | interruptArgs[18] = state; 470 | attachInterrupt(18, isr18, CHANGE); 471 | break; 472 | #endif 473 | #ifdef CORE_INT19_PIN 474 | case CORE_INT19_PIN: 475 | interruptArgs[19] = state; 476 | attachInterrupt(19, isr19, CHANGE); 477 | break; 478 | #endif 479 | #ifdef CORE_INT20_PIN 480 | case CORE_INT20_PIN: 481 | interruptArgs[20] = state; 482 | attachInterrupt(20, isr20, CHANGE); 483 | break; 484 | #endif 485 | #ifdef CORE_INT21_PIN 486 | case CORE_INT21_PIN: 487 | interruptArgs[21] = state; 488 | attachInterrupt(21, isr21, CHANGE); 489 | break; 490 | #endif 491 | #ifdef CORE_INT22_PIN 492 | case CORE_INT22_PIN: 493 | interruptArgs[22] = state; 494 | attachInterrupt(22, isr22, CHANGE); 495 | break; 496 | #endif 497 | #ifdef CORE_INT23_PIN 498 | case CORE_INT23_PIN: 499 | interruptArgs[23] = state; 500 | attachInterrupt(23, isr23, CHANGE); 501 | break; 502 | #endif 503 | #ifdef CORE_INT24_PIN 504 | case CORE_INT24_PIN: 505 | interruptArgs[24] = state; 506 | attachInterrupt(24, isr24, CHANGE); 507 | break; 508 | #endif 509 | #ifdef CORE_INT25_PIN 510 | case CORE_INT25_PIN: 511 | interruptArgs[25] = state; 512 | attachInterrupt(25, isr25, CHANGE); 513 | break; 514 | #endif 515 | #ifdef CORE_INT26_PIN 516 | case CORE_INT26_PIN: 517 | interruptArgs[26] = state; 518 | attachInterrupt(26, isr26, CHANGE); 519 | break; 520 | #endif 521 | #ifdef CORE_INT27_PIN 522 | case CORE_INT27_PIN: 523 | interruptArgs[27] = state; 524 | attachInterrupt(27, isr27, CHANGE); 525 | break; 526 | #endif 527 | #ifdef CORE_INT28_PIN 528 | case CORE_INT28_PIN: 529 | interruptArgs[28] = state; 530 | attachInterrupt(28, isr28, CHANGE); 531 | break; 532 | #endif 533 | #ifdef CORE_INT29_PIN 534 | case CORE_INT29_PIN: 535 | interruptArgs[29] = state; 536 | attachInterrupt(29, isr29, CHANGE); 537 | break; 538 | #endif 539 | 540 | #ifdef CORE_INT30_PIN 541 | case CORE_INT30_PIN: 542 | interruptArgs[30] = state; 543 | attachInterrupt(30, isr30, CHANGE); 544 | break; 545 | #endif 546 | #ifdef CORE_INT31_PIN 547 | case CORE_INT31_PIN: 548 | interruptArgs[31] = state; 549 | attachInterrupt(31, isr31, CHANGE); 550 | break; 551 | #endif 552 | #ifdef CORE_INT32_PIN 553 | case CORE_INT32_PIN: 554 | interruptArgs[32] = state; 555 | attachInterrupt(32, isr32, CHANGE); 556 | break; 557 | #endif 558 | #ifdef CORE_INT33_PIN 559 | case CORE_INT33_PIN: 560 | interruptArgs[33] = state; 561 | attachInterrupt(33, isr33, CHANGE); 562 | break; 563 | #endif 564 | #ifdef CORE_INT34_PIN 565 | case CORE_INT34_PIN: 566 | interruptArgs[34] = state; 567 | attachInterrupt(34, isr34, CHANGE); 568 | break; 569 | #endif 570 | #ifdef CORE_INT35_PIN 571 | case CORE_INT35_PIN: 572 | interruptArgs[35] = state; 573 | attachInterrupt(35, isr35, CHANGE); 574 | break; 575 | #endif 576 | #ifdef CORE_INT36_PIN 577 | case CORE_INT36_PIN: 578 | interruptArgs[36] = state; 579 | attachInterrupt(36, isr36, CHANGE); 580 | break; 581 | #endif 582 | #ifdef CORE_INT37_PIN 583 | case CORE_INT37_PIN: 584 | interruptArgs[37] = state; 585 | attachInterrupt(37, isr37, CHANGE); 586 | break; 587 | #endif 588 | #ifdef CORE_INT38_PIN 589 | case CORE_INT38_PIN: 590 | interruptArgs[38] = state; 591 | attachInterrupt(38, isr38, CHANGE); 592 | break; 593 | #endif 594 | #ifdef CORE_INT39_PIN 595 | case CORE_INT39_PIN: 596 | interruptArgs[39] = state; 597 | attachInterrupt(39, isr39, CHANGE); 598 | break; 599 | #endif 600 | #ifdef CORE_INT40_PIN 601 | case CORE_INT40_PIN: 602 | interruptArgs[40] = state; 603 | attachInterrupt(40, isr40, CHANGE); 604 | break; 605 | #endif 606 | #ifdef CORE_INT41_PIN 607 | case CORE_INT41_PIN: 608 | interruptArgs[41] = state; 609 | attachInterrupt(41, isr41, CHANGE); 610 | break; 611 | #endif 612 | #ifdef CORE_INT42_PIN 613 | case CORE_INT42_PIN: 614 | interruptArgs[42] = state; 615 | attachInterrupt(42, isr42, CHANGE); 616 | break; 617 | #endif 618 | #ifdef CORE_INT43_PIN 619 | case CORE_INT43_PIN: 620 | interruptArgs[43] = state; 621 | attachInterrupt(43, isr43, CHANGE); 622 | break; 623 | #endif 624 | #ifdef CORE_INT44_PIN 625 | case CORE_INT44_PIN: 626 | interruptArgs[44] = state; 627 | attachInterrupt(44, isr44, CHANGE); 628 | break; 629 | #endif 630 | #ifdef CORE_INT45_PIN 631 | case CORE_INT45_PIN: 632 | interruptArgs[45] = state; 633 | attachInterrupt(45, isr45, CHANGE); 634 | break; 635 | #endif 636 | #ifdef CORE_INT46_PIN 637 | case CORE_INT46_PIN: 638 | interruptArgs[46] = state; 639 | attachInterrupt(46, isr46, CHANGE); 640 | break; 641 | #endif 642 | #ifdef CORE_INT47_PIN 643 | case CORE_INT47_PIN: 644 | interruptArgs[47] = state; 645 | attachInterrupt(47, isr47, CHANGE); 646 | break; 647 | #endif 648 | #ifdef CORE_INT48_PIN 649 | case CORE_INT48_PIN: 650 | interruptArgs[48] = state; 651 | attachInterrupt(48, isr48, CHANGE); 652 | break; 653 | #endif 654 | #ifdef CORE_INT49_PIN 655 | case CORE_INT49_PIN: 656 | interruptArgs[49] = state; 657 | attachInterrupt(49, isr49, CHANGE); 658 | break; 659 | #endif 660 | #ifdef CORE_INT50_PIN 661 | case CORE_INT50_PIN: 662 | interruptArgs[50] = state; 663 | attachInterrupt(50, isr50, CHANGE); 664 | break; 665 | #endif 666 | #ifdef CORE_INT51_PIN 667 | case CORE_INT51_PIN: 668 | interruptArgs[51] = state; 669 | attachInterrupt(51, isr51, CHANGE); 670 | break; 671 | #endif 672 | #ifdef CORE_INT52_PIN 673 | case CORE_INT52_PIN: 674 | interruptArgs[52] = state; 675 | attachInterrupt(52, isr52, CHANGE); 676 | break; 677 | #endif 678 | #ifdef CORE_INT53_PIN 679 | case CORE_INT53_PIN: 680 | interruptArgs[53] = state; 681 | attachInterrupt(53, isr53, CHANGE); 682 | break; 683 | #endif 684 | #ifdef CORE_INT54_PIN 685 | case CORE_INT54_PIN: 686 | interruptArgs[54] = state; 687 | attachInterrupt(54, isr54, CHANGE); 688 | break; 689 | #endif 690 | #ifdef CORE_INT55_PIN 691 | case CORE_INT55_PIN: 692 | interruptArgs[55] = state; 693 | attachInterrupt(55, isr55, CHANGE); 694 | break; 695 | #endif 696 | #ifdef CORE_INT56_PIN 697 | case CORE_INT56_PIN: 698 | interruptArgs[56] = state; 699 | attachInterrupt(56, isr56, CHANGE); 700 | break; 701 | #endif 702 | #ifdef CORE_INT57_PIN 703 | case CORE_INT57_PIN: 704 | interruptArgs[57] = state; 705 | attachInterrupt(57, isr57, CHANGE); 706 | break; 707 | #endif 708 | #ifdef CORE_INT58_PIN 709 | case CORE_INT58_PIN: 710 | interruptArgs[58] = state; 711 | attachInterrupt(58, isr58, CHANGE); 712 | break; 713 | #endif 714 | #ifdef CORE_INT59_PIN 715 | case CORE_INT59_PIN: 716 | interruptArgs[59] = state; 717 | attachInterrupt(59, isr59, CHANGE); 718 | break; 719 | #endif 720 | default: 721 | return 0; 722 | } 723 | return 1; 724 | } 725 | #endif // ENCODER_USE_INTERRUPTS 726 | 727 | 728 | #if defined(ENCODER_USE_INTERRUPTS) && !defined(ENCODER_OPTIMIZE_INTERRUPTS) 729 | #ifdef CORE_INT0_PIN 730 | static void isr0(void) { update(interruptArgs[0]); } 731 | #endif 732 | #ifdef CORE_INT1_PIN 733 | static void isr1(void) { update(interruptArgs[1]); } 734 | #endif 735 | #ifdef CORE_INT2_PIN 736 | static void isr2(void) { update(interruptArgs[2]); } 737 | #endif 738 | #ifdef CORE_INT3_PIN 739 | static void isr3(void) { update(interruptArgs[3]); } 740 | #endif 741 | #ifdef CORE_INT4_PIN 742 | static void isr4(void) { update(interruptArgs[4]); } 743 | #endif 744 | #ifdef CORE_INT5_PIN 745 | static void isr5(void) { update(interruptArgs[5]); } 746 | #endif 747 | #ifdef CORE_INT6_PIN 748 | static void isr6(void) { update(interruptArgs[6]); } 749 | #endif 750 | #ifdef CORE_INT7_PIN 751 | static void isr7(void) { update(interruptArgs[7]); } 752 | #endif 753 | #ifdef CORE_INT8_PIN 754 | static void isr8(void) { update(interruptArgs[8]); } 755 | #endif 756 | #ifdef CORE_INT9_PIN 757 | static void isr9(void) { update(interruptArgs[9]); } 758 | #endif 759 | #ifdef CORE_INT10_PIN 760 | static void isr10(void) { update(interruptArgs[10]); } 761 | #endif 762 | #ifdef CORE_INT11_PIN 763 | static void isr11(void) { update(interruptArgs[11]); } 764 | #endif 765 | #ifdef CORE_INT12_PIN 766 | static void isr12(void) { update(interruptArgs[12]); } 767 | #endif 768 | #ifdef CORE_INT13_PIN 769 | static void isr13(void) { update(interruptArgs[13]); } 770 | #endif 771 | #ifdef CORE_INT14_PIN 772 | static void isr14(void) { update(interruptArgs[14]); } 773 | #endif 774 | #ifdef CORE_INT15_PIN 775 | static void isr15(void) { update(interruptArgs[15]); } 776 | #endif 777 | #ifdef CORE_INT16_PIN 778 | static void isr16(void) { update(interruptArgs[16]); } 779 | #endif 780 | #ifdef CORE_INT17_PIN 781 | static void isr17(void) { update(interruptArgs[17]); } 782 | #endif 783 | #ifdef CORE_INT18_PIN 784 | static void isr18(void) { update(interruptArgs[18]); } 785 | #endif 786 | #ifdef CORE_INT19_PIN 787 | static void isr19(void) { update(interruptArgs[19]); } 788 | #endif 789 | #ifdef CORE_INT20_PIN 790 | static void isr20(void) { update(interruptArgs[20]); } 791 | #endif 792 | #ifdef CORE_INT21_PIN 793 | static void isr21(void) { update(interruptArgs[21]); } 794 | #endif 795 | #ifdef CORE_INT22_PIN 796 | static void isr22(void) { update(interruptArgs[22]); } 797 | #endif 798 | #ifdef CORE_INT23_PIN 799 | static void isr23(void) { update(interruptArgs[23]); } 800 | #endif 801 | #ifdef CORE_INT24_PIN 802 | static void isr24(void) { update(interruptArgs[24]); } 803 | #endif 804 | #ifdef CORE_INT25_PIN 805 | static void isr25(void) { update(interruptArgs[25]); } 806 | #endif 807 | #ifdef CORE_INT26_PIN 808 | static void isr26(void) { update(interruptArgs[26]); } 809 | #endif 810 | #ifdef CORE_INT27_PIN 811 | static void isr27(void) { update(interruptArgs[27]); } 812 | #endif 813 | #ifdef CORE_INT28_PIN 814 | static void isr28(void) { update(interruptArgs[28]); } 815 | #endif 816 | #ifdef CORE_INT29_PIN 817 | static void isr29(void) { update(interruptArgs[29]); } 818 | #endif 819 | #ifdef CORE_INT30_PIN 820 | static void isr30(void) { update(interruptArgs[30]); } 821 | #endif 822 | #ifdef CORE_INT31_PIN 823 | static void isr31(void) { update(interruptArgs[31]); } 824 | #endif 825 | #ifdef CORE_INT32_PIN 826 | static void isr32(void) { update(interruptArgs[32]); } 827 | #endif 828 | #ifdef CORE_INT33_PIN 829 | static void isr33(void) { update(interruptArgs[33]); } 830 | #endif 831 | #ifdef CORE_INT34_PIN 832 | static void isr34(void) { update(interruptArgs[34]); } 833 | #endif 834 | #ifdef CORE_INT35_PIN 835 | static void isr35(void) { update(interruptArgs[35]); } 836 | #endif 837 | #ifdef CORE_INT36_PIN 838 | static void isr36(void) { update(interruptArgs[36]); } 839 | #endif 840 | #ifdef CORE_INT37_PIN 841 | static void isr37(void) { update(interruptArgs[37]); } 842 | #endif 843 | #ifdef CORE_INT38_PIN 844 | static void isr38(void) { update(interruptArgs[38]); } 845 | #endif 846 | #ifdef CORE_INT39_PIN 847 | static void isr39(void) { update(interruptArgs[39]); } 848 | #endif 849 | #ifdef CORE_INT40_PIN 850 | static void isr40(void) { update(interruptArgs[40]); } 851 | #endif 852 | #ifdef CORE_INT41_PIN 853 | static void isr41(void) { update(interruptArgs[41]); } 854 | #endif 855 | #ifdef CORE_INT42_PIN 856 | static void isr42(void) { update(interruptArgs[42]); } 857 | #endif 858 | #ifdef CORE_INT43_PIN 859 | static void isr43(void) { update(interruptArgs[43]); } 860 | #endif 861 | #ifdef CORE_INT44_PIN 862 | static void isr44(void) { update(interruptArgs[44]); } 863 | #endif 864 | #ifdef CORE_INT45_PIN 865 | static void isr45(void) { update(interruptArgs[45]); } 866 | #endif 867 | #ifdef CORE_INT46_PIN 868 | static void isr46(void) { update(interruptArgs[46]); } 869 | #endif 870 | #ifdef CORE_INT47_PIN 871 | static void isr47(void) { update(interruptArgs[47]); } 872 | #endif 873 | #ifdef CORE_INT48_PIN 874 | static void isr48(void) { update(interruptArgs[48]); } 875 | #endif 876 | #ifdef CORE_INT49_PIN 877 | static void isr49(void) { update(interruptArgs[49]); } 878 | #endif 879 | #ifdef CORE_INT50_PIN 880 | static void isr50(void) { update(interruptArgs[50]); } 881 | #endif 882 | #ifdef CORE_INT51_PIN 883 | static void isr51(void) { update(interruptArgs[51]); } 884 | #endif 885 | #ifdef CORE_INT52_PIN 886 | static void isr52(void) { update(interruptArgs[52]); } 887 | #endif 888 | #ifdef CORE_INT53_PIN 889 | static void isr53(void) { update(interruptArgs[53]); } 890 | #endif 891 | #ifdef CORE_INT54_PIN 892 | static void isr54(void) { update(interruptArgs[54]); } 893 | #endif 894 | #ifdef CORE_INT55_PIN 895 | static void isr55(void) { update(interruptArgs[55]); } 896 | #endif 897 | #ifdef CORE_INT56_PIN 898 | static void isr56(void) { update(interruptArgs[56]); } 899 | #endif 900 | #ifdef CORE_INT57_PIN 901 | static void isr57(void) { update(interruptArgs[57]); } 902 | #endif 903 | #ifdef CORE_INT58_PIN 904 | static void isr58(void) { update(interruptArgs[58]); } 905 | #endif 906 | #ifdef CORE_INT59_PIN 907 | static void isr59(void) { update(interruptArgs[59]); } 908 | #endif 909 | #endif 910 | }; 911 | 912 | #if defined(ENCODER_USE_INTERRUPTS) && defined(ENCODER_OPTIMIZE_INTERRUPTS) 913 | #if defined(__AVR__) 914 | #if defined(INT0_vect) && CORE_NUM_INTERRUPT > 0 915 | ISR(INT0_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(0)]); } 916 | #endif 917 | #if defined(INT1_vect) && CORE_NUM_INTERRUPT > 1 918 | ISR(INT1_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(1)]); } 919 | #endif 920 | #if defined(INT2_vect) && CORE_NUM_INTERRUPT > 2 921 | ISR(INT2_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(2)]); } 922 | #endif 923 | #if defined(INT3_vect) && CORE_NUM_INTERRUPT > 3 924 | ISR(INT3_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(3)]); } 925 | #endif 926 | #if defined(INT4_vect) && CORE_NUM_INTERRUPT > 4 927 | ISR(INT4_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(4)]); } 928 | #endif 929 | #if defined(INT5_vect) && CORE_NUM_INTERRUPT > 5 930 | ISR(INT5_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(5)]); } 931 | #endif 932 | #if defined(INT6_vect) && CORE_NUM_INTERRUPT > 6 933 | ISR(INT6_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(6)]); } 934 | #endif 935 | #if defined(INT7_vect) && CORE_NUM_INTERRUPT > 7 936 | ISR(INT7_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(7)]); } 937 | #endif 938 | #endif // AVR 939 | #endif // ENCODER_OPTIMIZE_INTERRUPTS 940 | 941 | 942 | #endif 943 | --------------------------------------------------------------------------------