diff --git a/include/Program.h b/include/Program.h
index 39638fc..d59e814 100644
--- a/include/Program.h
+++ b/include/Program.h
@@ -21,6 +21,11 @@ private:
      * Encoder object
      */
     Encoder* encoder;
+
+    /**
+     * Old encoder position
+     */
+    long oldPosition;
 };
 
 #endif
diff --git a/lib/Encoder/.piopm b/lib/Encoder/.piopm
new file mode 100644
index 0000000..605d1ff
--- /dev/null
+++ b/lib/Encoder/.piopm
@@ -0,0 +1 @@
+{"type": "library", "name": "Encoder", "version": "1.4.2", "spec": {"owner": "paulstoffregen", "id": 129, "name": "Encoder", "requirements": null, "uri": null}}
\ No newline at end of file
diff --git a/lib/Encoder/Encoder.cpp b/lib/Encoder/Encoder.cpp
new file mode 100644
index 0000000..6911b4f
--- /dev/null
+++ b/lib/Encoder/Encoder.cpp
@@ -0,0 +1,10 @@
+
+#include "Encoder.h"
+
+// Yes, all the code is in the header file, to provide the user
+// configure options with #define (before they include it), and
+// to facilitate some crafty optimizations!
+
+Encoder_internal_state_t * Encoder::interruptArgs[];
+
+
diff --git a/lib/Encoder/Encoder.h b/lib/Encoder/Encoder.h
new file mode 100644
index 0000000..8d50de8
--- /dev/null
+++ b/lib/Encoder/Encoder.h
@@ -0,0 +1,969 @@
+/* Encoder Library, for measuring quadrature encoded signals
+ * http://www.pjrc.com/teensy/td_libs_Encoder.html
+ * Copyright (c) 2011,2013 PJRC.COM, LLC - Paul Stoffregen <paul@pjrc.com>
+ *
+ * Version 1.2 - fix -2 bug in C-only code
+ * Version 1.1 - expand to support boards with up to 60 interrupts
+ * Version 1.0 - initial release
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+
+#ifndef Encoder_h_
+#define Encoder_h_
+
+#if defined(ARDUINO) && ARDUINO >= 100
+#include "Arduino.h"
+#elif defined(WIRING)
+#include "Wiring.h"
+#else
+#include "WProgram.h"
+#include "pins_arduino.h"
+#endif
+
+#include "utility/direct_pin_read.h"
+
+#if defined(ENCODER_USE_INTERRUPTS) || !defined(ENCODER_DO_NOT_USE_INTERRUPTS)
+#define ENCODER_USE_INTERRUPTS
+#define ENCODER_ARGLIST_SIZE CORE_NUM_INTERRUPT
+#include "utility/interrupt_pins.h"
+#ifdef ENCODER_OPTIMIZE_INTERRUPTS
+#include "utility/interrupt_config.h"
+#endif
+#else
+#define ENCODER_ARGLIST_SIZE 0
+#endif
+
+
+
+// All the data needed by interrupts is consolidated into this ugly struct
+// to facilitate assembly language optimizing of the speed critical update.
+// The assembly code uses auto-incrementing addressing modes, so the struct
+// must remain in exactly this order.
+typedef struct {
+	volatile IO_REG_TYPE * pin1_register;
+	volatile IO_REG_TYPE * pin2_register;
+	IO_REG_TYPE            pin1_bitmask;
+	IO_REG_TYPE            pin2_bitmask;
+	uint8_t                state;
+	int32_t                position;
+} Encoder_internal_state_t;
+
+class Encoder
+{
+public:
+	Encoder(uint8_t pin1, uint8_t pin2) {
+		#ifdef INPUT_PULLUP
+		pinMode(pin1, INPUT_PULLUP);
+		pinMode(pin2, INPUT_PULLUP);
+		#else
+		pinMode(pin1, INPUT);
+		digitalWrite(pin1, HIGH);
+		pinMode(pin2, INPUT);
+		digitalWrite(pin2, HIGH);
+		#endif
+		encoder.pin1_register = PIN_TO_BASEREG(pin1);
+		encoder.pin1_bitmask = PIN_TO_BITMASK(pin1);
+		encoder.pin2_register = PIN_TO_BASEREG(pin2);
+		encoder.pin2_bitmask = PIN_TO_BITMASK(pin2);
+		encoder.position = 0;
+		// allow time for a passive R-C filter to charge
+		// through the pullup resistors, before reading
+		// the initial state
+		delayMicroseconds(2000);
+		uint8_t s = 0;
+		if (DIRECT_PIN_READ(encoder.pin1_register, encoder.pin1_bitmask)) s |= 1;
+		if (DIRECT_PIN_READ(encoder.pin2_register, encoder.pin2_bitmask)) s |= 2;
+		encoder.state = s;
+#ifdef ENCODER_USE_INTERRUPTS
+		interrupts_in_use = attach_interrupt(pin1, &encoder);
+		interrupts_in_use += attach_interrupt(pin2, &encoder);
+#endif
+		//update_finishup();  // to force linker to include the code (does not work)
+	}
+
+
+#ifdef ENCODER_USE_INTERRUPTS
+	inline int32_t read() {
+		if (interrupts_in_use < 2) {
+			noInterrupts();
+			update(&encoder);
+		} else {
+			noInterrupts();
+		}
+		int32_t ret = encoder.position;
+		interrupts();
+		return ret;
+	}
+	inline int32_t readAndReset() {
+		if (interrupts_in_use < 2) {
+			noInterrupts();
+			update(&encoder);
+		} else {
+			noInterrupts();
+		}
+		int32_t ret = encoder.position;
+		encoder.position = 0;
+		interrupts();
+		return ret;
+	}
+	inline void write(int32_t p) {
+		noInterrupts();
+		encoder.position = p;
+		interrupts();
+	}
+#else
+	inline int32_t read() {
+		update(&encoder);
+		return encoder.position;
+	}
+	inline int32_t readAndReset() {
+		update(&encoder);
+		int32_t ret = encoder.position;
+		encoder.position = 0;
+		return ret;
+	}
+	inline void write(int32_t p) {
+		encoder.position = p;
+	}
+#endif
+private:
+	Encoder_internal_state_t encoder;
+#ifdef ENCODER_USE_INTERRUPTS
+	uint8_t interrupts_in_use;
+#endif
+public:
+	static Encoder_internal_state_t * interruptArgs[ENCODER_ARGLIST_SIZE];
+
+//                           _______         _______
+//               Pin1 ______|       |_______|       |______ Pin1
+// negative <---         _______         _______         __      --> positive
+//               Pin2 __|       |_______|       |_______|   Pin2
+
+		//	new	new	old	old
+		//	pin2	pin1	pin2	pin1	Result
+		//	----	----	----	----	------
+		//	0	0	0	0	no movement
+		//	0	0	0	1	+1
+		//	0	0	1	0	-1
+		//	0	0	1	1	+2  (assume pin1 edges only)
+		//	0	1	0	0	-1
+		//	0	1	0	1	no movement
+		//	0	1	1	0	-2  (assume pin1 edges only)
+		//	0	1	1	1	+1
+		//	1	0	0	0	+1
+		//	1	0	0	1	-2  (assume pin1 edges only)
+		//	1	0	1	0	no movement
+		//	1	0	1	1	-1
+		//	1	1	0	0	+2  (assume pin1 edges only)
+		//	1	1	0	1	-1
+		//	1	1	1	0	+1
+		//	1	1	1	1	no movement
+/*
+	// Simple, easy-to-read "documentation" version :-)
+	//
+	void update(void) {
+		uint8_t s = state & 3;
+		if (digitalRead(pin1)) s |= 4;
+		if (digitalRead(pin2)) s |= 8;
+		switch (s) {
+			case 0: case 5: case 10: case 15:
+				break;
+			case 1: case 7: case 8: case 14:
+				position++; break;
+			case 2: case 4: case 11: case 13:
+				position--; break;
+			case 3: case 12:
+				position += 2; break;
+			default:
+				position -= 2; break;
+		}
+		state = (s >> 2);
+	}
+*/
+
+public:
+	// update() is not meant to be called from outside Encoder,
+	// but it is public to allow static interrupt routines.
+	// DO NOT call update() directly from sketches.
+	static void update(Encoder_internal_state_t *arg) {
+#if defined(__AVR__)
+		// The compiler believes this is just 1 line of code, so
+		// it will inline this function into each interrupt
+		// handler.  That's a tiny bit faster, but grows the code.
+		// Especially when used with ENCODER_OPTIMIZE_INTERRUPTS,
+		// the inline nature allows the ISR prologue and epilogue
+		// to only save/restore necessary registers, for very nice
+		// speed increase.
+		asm volatile (
+			"ld	r30, X+"		"\n\t"
+			"ld	r31, X+"		"\n\t"
+			"ld	r24, Z"			"\n\t"	// r24 = pin1 input
+			"ld	r30, X+"		"\n\t"
+			"ld	r31, X+"		"\n\t"
+			"ld	r25, Z"			"\n\t"  // r25 = pin2 input
+			"ld	r30, X+"		"\n\t"  // r30 = pin1 mask
+			"ld	r31, X+"		"\n\t"	// r31 = pin2 mask
+			"ld	r22, X"			"\n\t"	// r22 = state
+			"andi	r22, 3"			"\n\t"
+			"and	r24, r30"		"\n\t"
+			"breq	L%=1"			"\n\t"	// if (pin1)
+			"ori	r22, 4"			"\n\t"	//	state |= 4
+		"L%=1:"	"and	r25, r31"		"\n\t"
+			"breq	L%=2"			"\n\t"	// if (pin2)
+			"ori	r22, 8"			"\n\t"	//	state |= 8
+		"L%=2:" "ldi	r30, lo8(pm(L%=table))"	"\n\t"
+			"ldi	r31, hi8(pm(L%=table))"	"\n\t"
+			"add	r30, r22"		"\n\t"
+			"adc	r31, __zero_reg__"	"\n\t"
+			"asr	r22"			"\n\t"
+			"asr	r22"			"\n\t"
+			"st	X+, r22"		"\n\t"  // store new state
+			"ld	r22, X+"		"\n\t"
+			"ld	r23, X+"		"\n\t"
+			"ld	r24, X+"		"\n\t"
+			"ld	r25, X+"		"\n\t"
+			"ijmp"				"\n\t"	// jumps to update_finishup()
+			// TODO move this table to another static function,
+			// so it doesn't get needlessly duplicated.  Easier
+			// said than done, due to linker issues and inlining
+		"L%=table:"				"\n\t"
+			"rjmp	L%=end"			"\n\t"	// 0
+			"rjmp	L%=plus1"		"\n\t"	// 1
+			"rjmp	L%=minus1"		"\n\t"	// 2
+			"rjmp	L%=plus2"		"\n\t"	// 3
+			"rjmp	L%=minus1"		"\n\t"	// 4
+			"rjmp	L%=end"			"\n\t"	// 5
+			"rjmp	L%=minus2"		"\n\t"	// 6
+			"rjmp	L%=plus1"		"\n\t"	// 7
+			"rjmp	L%=plus1"		"\n\t"	// 8
+			"rjmp	L%=minus2"		"\n\t"	// 9
+			"rjmp	L%=end"			"\n\t"	// 10
+			"rjmp	L%=minus1"		"\n\t"	// 11
+			"rjmp	L%=plus2"		"\n\t"	// 12
+			"rjmp	L%=minus1"		"\n\t"	// 13
+			"rjmp	L%=plus1"		"\n\t"	// 14
+			"rjmp	L%=end"			"\n\t"	// 15
+		"L%=minus2:"				"\n\t"
+			"subi	r22, 2"			"\n\t"
+			"sbci	r23, 0"			"\n\t"
+			"sbci	r24, 0"			"\n\t"
+			"sbci	r25, 0"			"\n\t"
+			"rjmp	L%=store"		"\n\t"
+		"L%=minus1:"				"\n\t"
+			"subi	r22, 1"			"\n\t"
+			"sbci	r23, 0"			"\n\t"
+			"sbci	r24, 0"			"\n\t"
+			"sbci	r25, 0"			"\n\t"
+			"rjmp	L%=store"		"\n\t"
+		"L%=plus2:"				"\n\t"
+			"subi	r22, 254"		"\n\t"
+			"rjmp	L%=z"			"\n\t"
+		"L%=plus1:"				"\n\t"
+			"subi	r22, 255"		"\n\t"
+		"L%=z:"	"sbci	r23, 255"		"\n\t"
+			"sbci	r24, 255"		"\n\t"
+			"sbci	r25, 255"		"\n\t"
+		"L%=store:"				"\n\t"
+			"st	-X, r25"		"\n\t"
+			"st	-X, r24"		"\n\t"
+			"st	-X, r23"		"\n\t"
+			"st	-X, r22"		"\n\t"
+		"L%=end:"				"\n"
+		: : "x" (arg) : "r22", "r23", "r24", "r25", "r30", "r31");
+#else
+		uint8_t p1val = DIRECT_PIN_READ(arg->pin1_register, arg->pin1_bitmask);
+		uint8_t p2val = DIRECT_PIN_READ(arg->pin2_register, arg->pin2_bitmask);
+		uint8_t state = arg->state & 3;
+		if (p1val) state |= 4;
+		if (p2val) state |= 8;
+		arg->state = (state >> 2);
+		switch (state) {
+			case 1: case 7: case 8: case 14:
+				arg->position++;
+				return;
+			case 2: case 4: case 11: case 13:
+				arg->position--;
+				return;
+			case 3: case 12:
+				arg->position += 2;
+				return;
+			case 6: case 9:
+				arg->position -= 2;
+				return;
+		}
+#endif
+	}
+private:
+/*
+#if defined(__AVR__)
+	// TODO: this must be a no inline function
+	// even noinline does not seem to solve difficult
+	// problems with this.  Oh well, it was only meant
+	// to shrink code size - there's no performance
+	// improvement in this, only code size reduction.
+	__attribute__((noinline)) void update_finishup(void) {
+		asm volatile (
+			"ldi	r30, lo8(pm(Ltable))"	"\n\t"
+			"ldi	r31, hi8(pm(Ltable))"	"\n\t"
+		"Ltable:"				"\n\t"
+			"rjmp	L%=end"			"\n\t"	// 0
+			"rjmp	L%=plus1"		"\n\t"	// 1
+			"rjmp	L%=minus1"		"\n\t"	// 2
+			"rjmp	L%=plus2"		"\n\t"	// 3
+			"rjmp	L%=minus1"		"\n\t"	// 4
+			"rjmp	L%=end"			"\n\t"	// 5
+			"rjmp	L%=minus2"		"\n\t"	// 6
+			"rjmp	L%=plus1"		"\n\t"	// 7
+			"rjmp	L%=plus1"		"\n\t"	// 8
+			"rjmp	L%=minus2"		"\n\t"	// 9
+			"rjmp	L%=end"			"\n\t"	// 10
+			"rjmp	L%=minus1"		"\n\t"	// 11
+			"rjmp	L%=plus2"		"\n\t"	// 12
+			"rjmp	L%=minus1"		"\n\t"	// 13
+			"rjmp	L%=plus1"		"\n\t"	// 14
+			"rjmp	L%=end"			"\n\t"	// 15
+		"L%=minus2:"				"\n\t"
+			"subi	r22, 2"			"\n\t"
+			"sbci	r23, 0"			"\n\t"
+			"sbci	r24, 0"			"\n\t"
+			"sbci	r25, 0"			"\n\t"
+			"rjmp	L%=store"		"\n\t"
+		"L%=minus1:"				"\n\t"
+			"subi	r22, 1"			"\n\t"
+			"sbci	r23, 0"			"\n\t"
+			"sbci	r24, 0"			"\n\t"
+			"sbci	r25, 0"			"\n\t"
+			"rjmp	L%=store"		"\n\t"
+		"L%=plus2:"				"\n\t"
+			"subi	r22, 254"		"\n\t"
+			"rjmp	L%=z"			"\n\t"
+		"L%=plus1:"				"\n\t"
+			"subi	r22, 255"		"\n\t"
+		"L%=z:"	"sbci	r23, 255"		"\n\t"
+			"sbci	r24, 255"		"\n\t"
+			"sbci	r25, 255"		"\n\t"
+		"L%=store:"				"\n\t"
+			"st	-X, r25"		"\n\t"
+			"st	-X, r24"		"\n\t"
+			"st	-X, r23"		"\n\t"
+			"st	-X, r22"		"\n\t"
+		"L%=end:"				"\n"
+		: : : "r22", "r23", "r24", "r25", "r30", "r31");
+	}
+#endif
+*/
+
+
+#ifdef ENCODER_USE_INTERRUPTS
+	// this giant function is an unfortunate consequence of Arduino's
+	// attachInterrupt function not supporting any way to pass a pointer
+	// or other context to the attached function.
+	static uint8_t attach_interrupt(uint8_t pin, Encoder_internal_state_t *state) {
+		switch (pin) {
+		#ifdef CORE_INT0_PIN
+			case CORE_INT0_PIN:
+				interruptArgs[0] = state;
+				attachInterrupt(0, isr0, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT1_PIN
+			case CORE_INT1_PIN:
+				interruptArgs[1] = state;
+				attachInterrupt(1, isr1, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT2_PIN
+			case CORE_INT2_PIN:
+				interruptArgs[2] = state;
+				attachInterrupt(2, isr2, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT3_PIN
+			case CORE_INT3_PIN:
+				interruptArgs[3] = state;
+				attachInterrupt(3, isr3, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT4_PIN
+			case CORE_INT4_PIN:
+				interruptArgs[4] = state;
+				attachInterrupt(4, isr4, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT5_PIN
+			case CORE_INT5_PIN:
+				interruptArgs[5] = state;
+				attachInterrupt(5, isr5, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT6_PIN
+			case CORE_INT6_PIN:
+				interruptArgs[6] = state;
+				attachInterrupt(6, isr6, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT7_PIN
+			case CORE_INT7_PIN:
+				interruptArgs[7] = state;
+				attachInterrupt(7, isr7, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT8_PIN
+			case CORE_INT8_PIN:
+				interruptArgs[8] = state;
+				attachInterrupt(8, isr8, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT9_PIN
+			case CORE_INT9_PIN:
+				interruptArgs[9] = state;
+				attachInterrupt(9, isr9, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT10_PIN
+			case CORE_INT10_PIN:
+				interruptArgs[10] = state;
+				attachInterrupt(10, isr10, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT11_PIN
+			case CORE_INT11_PIN:
+				interruptArgs[11] = state;
+				attachInterrupt(11, isr11, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT12_PIN
+			case CORE_INT12_PIN:
+				interruptArgs[12] = state;
+				attachInterrupt(12, isr12, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT13_PIN
+			case CORE_INT13_PIN:
+				interruptArgs[13] = state;
+				attachInterrupt(13, isr13, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT14_PIN
+			case CORE_INT14_PIN:
+				interruptArgs[14] = state;
+				attachInterrupt(14, isr14, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT15_PIN
+			case CORE_INT15_PIN:
+				interruptArgs[15] = state;
+				attachInterrupt(15, isr15, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT16_PIN
+			case CORE_INT16_PIN:
+				interruptArgs[16] = state;
+				attachInterrupt(16, isr16, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT17_PIN
+			case CORE_INT17_PIN:
+				interruptArgs[17] = state;
+				attachInterrupt(17, isr17, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT18_PIN
+			case CORE_INT18_PIN:
+				interruptArgs[18] = state;
+				attachInterrupt(18, isr18, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT19_PIN
+			case CORE_INT19_PIN:
+				interruptArgs[19] = state;
+				attachInterrupt(19, isr19, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT20_PIN
+			case CORE_INT20_PIN:
+				interruptArgs[20] = state;
+				attachInterrupt(20, isr20, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT21_PIN
+			case CORE_INT21_PIN:
+				interruptArgs[21] = state;
+				attachInterrupt(21, isr21, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT22_PIN
+			case CORE_INT22_PIN:
+				interruptArgs[22] = state;
+				attachInterrupt(22, isr22, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT23_PIN
+			case CORE_INT23_PIN:
+				interruptArgs[23] = state;
+				attachInterrupt(23, isr23, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT24_PIN
+			case CORE_INT24_PIN:
+				interruptArgs[24] = state;
+				attachInterrupt(24, isr24, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT25_PIN
+			case CORE_INT25_PIN:
+				interruptArgs[25] = state;
+				attachInterrupt(25, isr25, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT26_PIN
+			case CORE_INT26_PIN:
+				interruptArgs[26] = state;
+				attachInterrupt(26, isr26, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT27_PIN
+			case CORE_INT27_PIN:
+				interruptArgs[27] = state;
+				attachInterrupt(27, isr27, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT28_PIN
+			case CORE_INT28_PIN:
+				interruptArgs[28] = state;
+				attachInterrupt(28, isr28, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT29_PIN
+			case CORE_INT29_PIN:
+				interruptArgs[29] = state;
+				attachInterrupt(29, isr29, CHANGE);
+				break;
+		#endif
+
+		#ifdef CORE_INT30_PIN
+			case CORE_INT30_PIN:
+				interruptArgs[30] = state;
+				attachInterrupt(30, isr30, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT31_PIN
+			case CORE_INT31_PIN:
+				interruptArgs[31] = state;
+				attachInterrupt(31, isr31, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT32_PIN
+			case CORE_INT32_PIN:
+				interruptArgs[32] = state;
+				attachInterrupt(32, isr32, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT33_PIN
+			case CORE_INT33_PIN:
+				interruptArgs[33] = state;
+				attachInterrupt(33, isr33, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT34_PIN
+			case CORE_INT34_PIN:
+				interruptArgs[34] = state;
+				attachInterrupt(34, isr34, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT35_PIN
+			case CORE_INT35_PIN:
+				interruptArgs[35] = state;
+				attachInterrupt(35, isr35, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT36_PIN
+			case CORE_INT36_PIN:
+				interruptArgs[36] = state;
+				attachInterrupt(36, isr36, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT37_PIN
+			case CORE_INT37_PIN:
+				interruptArgs[37] = state;
+				attachInterrupt(37, isr37, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT38_PIN
+			case CORE_INT38_PIN:
+				interruptArgs[38] = state;
+				attachInterrupt(38, isr38, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT39_PIN
+			case CORE_INT39_PIN:
+				interruptArgs[39] = state;
+				attachInterrupt(39, isr39, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT40_PIN
+			case CORE_INT40_PIN:
+				interruptArgs[40] = state;
+				attachInterrupt(40, isr40, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT41_PIN
+			case CORE_INT41_PIN:
+				interruptArgs[41] = state;
+				attachInterrupt(41, isr41, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT42_PIN
+			case CORE_INT42_PIN:
+				interruptArgs[42] = state;
+				attachInterrupt(42, isr42, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT43_PIN
+			case CORE_INT43_PIN:
+				interruptArgs[43] = state;
+				attachInterrupt(43, isr43, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT44_PIN
+			case CORE_INT44_PIN:
+				interruptArgs[44] = state;
+				attachInterrupt(44, isr44, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT45_PIN
+			case CORE_INT45_PIN:
+				interruptArgs[45] = state;
+				attachInterrupt(45, isr45, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT46_PIN
+			case CORE_INT46_PIN:
+				interruptArgs[46] = state;
+				attachInterrupt(46, isr46, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT47_PIN
+			case CORE_INT47_PIN:
+				interruptArgs[47] = state;
+				attachInterrupt(47, isr47, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT48_PIN
+			case CORE_INT48_PIN:
+				interruptArgs[48] = state;
+				attachInterrupt(48, isr48, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT49_PIN
+			case CORE_INT49_PIN:
+				interruptArgs[49] = state;
+				attachInterrupt(49, isr49, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT50_PIN
+			case CORE_INT50_PIN:
+				interruptArgs[50] = state;
+				attachInterrupt(50, isr50, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT51_PIN
+			case CORE_INT51_PIN:
+				interruptArgs[51] = state;
+				attachInterrupt(51, isr51, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT52_PIN
+			case CORE_INT52_PIN:
+				interruptArgs[52] = state;
+				attachInterrupt(52, isr52, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT53_PIN
+			case CORE_INT53_PIN:
+				interruptArgs[53] = state;
+				attachInterrupt(53, isr53, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT54_PIN
+			case CORE_INT54_PIN:
+				interruptArgs[54] = state;
+				attachInterrupt(54, isr54, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT55_PIN
+			case CORE_INT55_PIN:
+				interruptArgs[55] = state;
+				attachInterrupt(55, isr55, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT56_PIN
+			case CORE_INT56_PIN:
+				interruptArgs[56] = state;
+				attachInterrupt(56, isr56, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT57_PIN
+			case CORE_INT57_PIN:
+				interruptArgs[57] = state;
+				attachInterrupt(57, isr57, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT58_PIN
+			case CORE_INT58_PIN:
+				interruptArgs[58] = state;
+				attachInterrupt(58, isr58, CHANGE);
+				break;
+		#endif
+		#ifdef CORE_INT59_PIN
+			case CORE_INT59_PIN:
+				interruptArgs[59] = state;
+				attachInterrupt(59, isr59, CHANGE);
+				break;
+		#endif
+			default:
+				return 0;
+		}
+		return 1;
+	}
+#endif // ENCODER_USE_INTERRUPTS
+
+
+#if defined(ENCODER_USE_INTERRUPTS) && !defined(ENCODER_OPTIMIZE_INTERRUPTS)
+	#ifdef CORE_INT0_PIN
+	static void IRAM_ATTR isr0(void) { update(interruptArgs[0]); }
+	#endif
+	#ifdef CORE_INT1_PIN
+	static void IRAM_ATTR isr1(void) { update(interruptArgs[1]); }
+	#endif
+	#ifdef CORE_INT2_PIN
+	static void IRAM_ATTR isr2(void) { update(interruptArgs[2]); }
+	#endif
+	#ifdef CORE_INT3_PIN
+	static void IRAM_ATTR isr3(void) { update(interruptArgs[3]); }
+	#endif
+	#ifdef CORE_INT4_PIN
+	static void IRAM_ATTR isr4(void) { update(interruptArgs[4]); }
+	#endif
+	#ifdef CORE_INT5_PIN
+	static void IRAM_ATTR isr5(void) { update(interruptArgs[5]); }
+	#endif
+	#ifdef CORE_INT6_PIN
+	static void IRAM_ATTR isr6(void) { update(interruptArgs[6]); }
+	#endif
+	#ifdef CORE_INT7_PIN
+	static void IRAM_ATTR isr7(void) { update(interruptArgs[7]); }
+	#endif
+	#ifdef CORE_INT8_PIN
+	static void IRAM_ATTR isr8(void) { update(interruptArgs[8]); }
+	#endif
+	#ifdef CORE_INT9_PIN
+	static void IRAM_ATTR isr9(void) { update(interruptArgs[9]); }
+	#endif
+	#ifdef CORE_INT10_PIN
+	static void IRAM_ATTR isr10(void) { update(interruptArgs[10]); }
+	#endif
+	#ifdef CORE_INT11_PIN
+	static void IRAM_ATTR isr11(void) { update(interruptArgs[11]); }
+	#endif
+	#ifdef CORE_INT12_PIN
+	static void IRAM_ATTR isr12(void) { update(interruptArgs[12]); }
+	#endif
+	#ifdef CORE_INT13_PIN
+	static void IRAM_ATTR isr13(void) { update(interruptArgs[13]); }
+	#endif
+	#ifdef CORE_INT14_PIN
+	static void IRAM_ATTR isr14(void) { update(interruptArgs[14]); }
+	#endif
+	#ifdef CORE_INT15_PIN
+	static void IRAM_ATTR isr15(void) { update(interruptArgs[15]); }
+	#endif
+	#ifdef CORE_INT16_PIN
+	static void IRAM_ATTR isr16(void) { update(interruptArgs[16]); }
+	#endif
+	#ifdef CORE_INT17_PIN
+	static void IRAM_ATTR isr17(void) { update(interruptArgs[17]); }
+	#endif
+	#ifdef CORE_INT18_PIN
+	static void IRAM_ATTR isr18(void) { update(interruptArgs[18]); }
+	#endif
+	#ifdef CORE_INT19_PIN
+	static void IRAM_ATTR isr19(void) { update(interruptArgs[19]); }
+	#endif
+	#ifdef CORE_INT20_PIN
+	static void IRAM_ATTR isr20(void) { update(interruptArgs[20]); }
+	#endif
+	#ifdef CORE_INT21_PIN
+	static void IRAM_ATTR isr21(void) { update(interruptArgs[21]); }
+	#endif
+	#ifdef CORE_INT22_PIN
+	static void IRAM_ATTR isr22(void) { update(interruptArgs[22]); }
+	#endif
+	#ifdef CORE_INT23_PIN
+	static void IRAM_ATTR isr23(void) { update(interruptArgs[23]); }
+	#endif
+	#ifdef CORE_INT24_PIN
+	static void IRAM_ATTR isr24(void) { update(interruptArgs[24]); }
+	#endif
+	#ifdef CORE_INT25_PIN
+	static void IRAM_ATTR isr25(void) { update(interruptArgs[25]); }
+	#endif
+	#ifdef CORE_INT26_PIN
+	static void IRAM_ATTR isr26(void) { update(interruptArgs[26]); }
+	#endif
+	#ifdef CORE_INT27_PIN
+	static void IRAM_ATTR isr27(void) { update(interruptArgs[27]); }
+	#endif
+	#ifdef CORE_INT28_PIN
+	static void IRAM_ATTR isr28(void) { update(interruptArgs[28]); }
+	#endif
+	#ifdef CORE_INT29_PIN
+	static void IRAM_ATTR isr29(void) { update(interruptArgs[29]); }
+	#endif
+	#ifdef CORE_INT30_PIN
+	static void IRAM_ATTR isr30(void) { update(interruptArgs[30]); }
+	#endif
+	#ifdef CORE_INT31_PIN
+	static void IRAM_ATTR isr31(void) { update(interruptArgs[31]); }
+	#endif
+	#ifdef CORE_INT32_PIN
+	static void IRAM_ATTR isr32(void) { update(interruptArgs[32]); }
+	#endif
+	#ifdef CORE_INT33_PIN
+	static void IRAM_ATTR isr33(void) { update(interruptArgs[33]); }
+	#endif
+	#ifdef CORE_INT34_PIN
+	static void IRAM_ATTR isr34(void) { update(interruptArgs[34]); }
+	#endif
+	#ifdef CORE_INT35_PIN
+	static void IRAM_ATTR isr35(void) { update(interruptArgs[35]); }
+	#endif
+	#ifdef CORE_INT36_PIN
+	static void IRAM_ATTR isr36(void) { update(interruptArgs[36]); }
+	#endif
+	#ifdef CORE_INT37_PIN
+	static void IRAM_ATTR isr37(void) { update(interruptArgs[37]); }
+	#endif
+	#ifdef CORE_INT38_PIN
+	static void IRAM_ATTR isr38(void) { update(interruptArgs[38]); }
+	#endif
+	#ifdef CORE_INT39_PIN
+	static void IRAM_ATTR isr39(void) { update(interruptArgs[39]); }
+	#endif
+	#ifdef CORE_INT40_PIN
+	static void IRAM_ATTR isr40(void) { update(interruptArgs[40]); }
+	#endif
+	#ifdef CORE_INT41_PIN
+	static void IRAM_ATTR isr41(void) { update(interruptArgs[41]); }
+	#endif
+	#ifdef CORE_INT42_PIN
+	static void IRAM_ATTR isr42(void) { update(interruptArgs[42]); }
+	#endif
+	#ifdef CORE_INT43_PIN
+	static void IRAM_ATTR isr43(void) { update(interruptArgs[43]); }
+	#endif
+	#ifdef CORE_INT44_PIN
+	static void IRAM_ATTR isr44(void) { update(interruptArgs[44]); }
+	#endif
+	#ifdef CORE_INT45_PIN
+	static void IRAM_ATTR isr45(void) { update(interruptArgs[45]); }
+	#endif
+	#ifdef CORE_INT46_PIN
+	static void IRAM_ATTR isr46(void) { update(interruptArgs[46]); }
+	#endif
+	#ifdef CORE_INT47_PIN
+	static void IRAM_ATTR isr47(void) { update(interruptArgs[47]); }
+	#endif
+	#ifdef CORE_INT48_PIN
+	static void IRAM_ATTR isr48(void) { update(interruptArgs[48]); }
+	#endif
+	#ifdef CORE_INT49_PIN
+	static void IRAM_ATTR isr49(void) { update(interruptArgs[49]); }
+	#endif
+	#ifdef CORE_INT50_PIN
+	static void IRAM_ATTR isr50(void) { update(interruptArgs[50]); }
+	#endif
+	#ifdef CORE_INT51_PIN
+	static void IRAM_ATTR isr51(void) { update(interruptArgs[51]); }
+	#endif
+	#ifdef CORE_INT52_PIN
+	static void IRAM_ATTR isr52(void) { update(interruptArgs[52]); }
+	#endif
+	#ifdef CORE_INT53_PIN
+	static void IRAM_ATTR isr53(void) { update(interruptArgs[53]); }
+	#endif
+	#ifdef CORE_INT54_PIN
+	static void IRAM_ATTR isr54(void) { update(interruptArgs[54]); }
+	#endif
+	#ifdef CORE_INT55_PIN
+	static void IRAM_ATTR isr55(void) { update(interruptArgs[55]); }
+	#endif
+	#ifdef CORE_INT56_PIN
+	static void IRAM_ATTR isr56(void) { update(interruptArgs[56]); }
+	#endif
+	#ifdef CORE_INT57_PIN
+	static void IRAM_ATTR isr57(void) { update(interruptArgs[57]); }
+	#endif
+	#ifdef CORE_INT58_PIN
+	static void IRAM_ATTR isr58(void) { update(interruptArgs[58]); }
+	#endif
+	#ifdef CORE_INT59_PIN
+	static void IRAM_ATTR isr59(void) { update(interruptArgs[59]); }
+	#endif
+#endif
+};
+
+#if defined(ENCODER_USE_INTERRUPTS) && defined(ENCODER_OPTIMIZE_INTERRUPTS)
+#if defined(__AVR__)
+#if defined(INT0_vect) && CORE_NUM_INTERRUPT > 0
+ISR(INT0_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(0)]); }
+#endif
+#if defined(INT1_vect) && CORE_NUM_INTERRUPT > 1
+ISR(INT1_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(1)]); }
+#endif
+#if defined(INT2_vect) && CORE_NUM_INTERRUPT > 2
+ISR(INT2_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(2)]); }
+#endif
+#if defined(INT3_vect) && CORE_NUM_INTERRUPT > 3
+ISR(INT3_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(3)]); }
+#endif
+#if defined(INT4_vect) && CORE_NUM_INTERRUPT > 4
+ISR(INT4_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(4)]); }
+#endif
+#if defined(INT5_vect) && CORE_NUM_INTERRUPT > 5
+ISR(INT5_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(5)]); }
+#endif
+#if defined(INT6_vect) && CORE_NUM_INTERRUPT > 6
+ISR(INT6_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(6)]); }
+#endif
+#if defined(INT7_vect) && CORE_NUM_INTERRUPT > 7
+ISR(INT7_vect) { Encoder::update(Encoder::interruptArgs[SCRAMBLE_INT_ORDER(7)]); }
+#endif
+#endif // AVR
+#if defined(attachInterrupt)
+// Don't intefere with other libraries or sketch use of attachInterrupt()
+// https://github.com/PaulStoffregen/Encoder/issues/8
+#undef attachInterrupt
+#endif
+#endif // ENCODER_OPTIMIZE_INTERRUPTS
+
+
+#endif
diff --git a/lib/Encoder/README.md b/lib/Encoder/README.md
new file mode 100644
index 0000000..6a4f638
--- /dev/null
+++ b/lib/Encoder/README.md
@@ -0,0 +1,9 @@
+# Encoder Library
+
+Encoder counts pulses from quadrature encoded signals, which are commonly available from rotary knobs, motor or shaft sensors and other position sensors. 
+
+http://www.pjrc.com/teensy/td_libs_Encoder.html
+
+http://www.youtube.com/watch?v=2puhIong-cs
+
+![Encoder Knobs Demo](http://www.pjrc.com/teensy/td_libs_Encoder_1.jpg)
diff --git a/lib/Encoder/examples/Basic/Basic.pde b/lib/Encoder/examples/Basic/Basic.pde
new file mode 100644
index 0000000..3394b58
--- /dev/null
+++ b/lib/Encoder/examples/Basic/Basic.pde
@@ -0,0 +1,29 @@
+/* Encoder Library - Basic Example
+ * http://www.pjrc.com/teensy/td_libs_Encoder.html
+ *
+ * This example code is in the public domain.
+ */
+
+#include <Encoder.h>
+
+// Change these two numbers to the pins connected to your encoder.
+//   Best Performance: both pins have interrupt capability
+//   Good Performance: only the first pin has interrupt capability
+//   Low Performance:  neither pin has interrupt capability
+Encoder myEnc(5, 6);
+//   avoid using pins with LEDs attached
+
+void setup() {
+  Serial.begin(9600);
+  Serial.println("Basic Encoder Test:");
+}
+
+long oldPosition  = -999;
+
+void loop() {
+  long newPosition = myEnc.read();
+  if (newPosition != oldPosition) {
+    oldPosition = newPosition;
+    Serial.println(newPosition);
+  }
+}
diff --git a/lib/Encoder/examples/NoInterrupts/NoInterrupts.pde b/lib/Encoder/examples/NoInterrupts/NoInterrupts.pde
new file mode 100644
index 0000000..b890652
--- /dev/null
+++ b/lib/Encoder/examples/NoInterrupts/NoInterrupts.pde
@@ -0,0 +1,46 @@
+/* Encoder Library - NoInterrupts Example
+ * http://www.pjrc.com/teensy/td_libs_Encoder.html
+ *
+ * This example code is in the public domain.
+ */
+
+// If you define ENCODER_DO_NOT_USE_INTERRUPTS *before* including
+// Encoder, the library will never use interrupts.  This is mainly
+// useful to reduce the size of the library when you are using it
+// with pins that do not support interrupts.  Without interrupts,
+// your program must call the read() function rapidly, or risk
+// missing changes in position.
+#define ENCODER_DO_NOT_USE_INTERRUPTS
+#include <Encoder.h>
+
+// Beware of Serial.print() speed.  Without interrupts, if you
+// transmit too much data with Serial.print() it can slow your
+// reading from Encoder.  Arduino 1.0 has improved transmit code.
+// Using the fastest baud rate also helps.  Teensy has USB packet
+// buffering.  But all boards can experience problems if you print
+// too much and fill up buffers.
+
+// Change these two numbers to the pins connected to your encoder.
+//   With ENCODER_DO_NOT_USE_INTERRUPTS, no interrupts are ever
+//   used, even if the pin has interrupt capability
+Encoder myEnc(5, 6);
+//   avoid using pins with LEDs attached
+
+void setup() {
+  Serial.begin(9600);
+  Serial.println("Basic NoInterrupts Test:");
+}
+
+long position  = -999;
+
+void loop() {
+  long newPos = myEnc.read();
+  if (newPos != position) {
+    position = newPos;
+    Serial.println(position);
+  }
+  // With any substantial delay added, Encoder can only track
+  // very slow motion.  You may uncomment this line to see
+  // how badly a delay affects your encoder.
+  //delay(50);
+}
diff --git a/lib/Encoder/examples/SpeedTest/SpeedTest.pde b/lib/Encoder/examples/SpeedTest/SpeedTest.pde
new file mode 100644
index 0000000..f136fbb
--- /dev/null
+++ b/lib/Encoder/examples/SpeedTest/SpeedTest.pde
@@ -0,0 +1,113 @@
+/* Encoder Library - SpeedTest - for measuring maximum Encoder speed
+ * http://www.pjrc.com/teensy/td_libs_Encoder.html
+ *
+ * This example code is in the public domain.
+ */
+
+
+// This SpeedTest example provides a simple way to verify how much
+// CPU time Encoder is consuming.  Connect a DC voltmeter to the
+// output pin and measure the voltage while the encoder is stopped
+// or running at a very slow speed.  Even though the pin is rapidly
+// pulsing, a DC voltmeter will show the average voltage.  Due to
+// software timing, it will read a number much less than a steady
+// logic high, but this number will give you a baseline reading
+// for output with minimal interrupt overhead.  Then increase the
+// encoder speed.  The voltage will decrease as the processor spends
+// more time in Encoder's interrupt routines counting the pulses
+// and less time pulsing the output pin.  When the voltage is
+// close to zero and will not decrease any farther, you have reached
+// the absolute speed limit.  Or, if using a mechanical system where
+// you reach a speed limit imposed by your motors or other hardware,
+// the amount this voltage has decreased, compared to the baseline,
+// should give you a good approximation of the portion of available
+// CPU time Encoder is consuming at your maximum speed.
+
+// Encoder requires low latency interrupt response.  Available CPU
+// time does NOT necessarily prove or guarantee correct performance.
+// If another library, like NewSoftSerial, is disabling interrupts
+// for lengthy periods of time, Encoder can be prevented from
+// properly counting the intput signals while interrupt are disabled.
+
+
+// This optional setting causes Encoder to use more optimized code,
+// but the downside is a conflict if any other part of your sketch
+// or any other library you're using requires attachInterrupt().
+// It must be defined before Encoder.h is included.
+//#define ENCODER_OPTIMIZE_INTERRUPTS
+
+#include <Encoder.h>
+#include "pins_arduino.h"
+
+// Change these two numbers to the pins connected to your encoder
+// or shift register circuit which emulates a quadrature encoder
+//  case 1: both pins are interrupts
+//  case 2: only first pin used as interrupt
+Encoder myEnc(5, 6);
+
+// Connect a DC voltmeter to this pin.
+const int outputPin = 12;
+
+/* This simple circuit, using a Dual Flip-Flop chip, can emulate
+   quadrature encoder signals.  The clock can come from a fancy
+   function generator or a cheap 555 timer chip.  The clock
+   frequency can be measured with another board running FreqCount
+   http://www.pjrc.com/teensy/td_libs_FreqCount.html
+
+                        +5V
+                         |        Quadrature Encoder Signal Emulator
+ Clock                   |
+ Input o----*--------------------------      ---------------------------o Output1
+            |            |14           |    |
+            |     _______|_______      |    |     _______________ 
+            |    |    CD4013     |     |    |    |    CD4013     |
+            |  5 |               | 1   |    |  9 |               | 13
+        ---------|  D         Q  |-----|----*----|  D         Q  |------o Output2
+       |    |    |               |     |         |               |
+       |    |  3 |               |     |      11 |               |
+       |     ----|> Clk          |      ---------|> Clk          |
+       |         |               |               |               |
+       |       6 |               |             8 |               |
+       |     ----|  S            |           ----|  S            |
+       |    |    |               |          |    |               |
+       |    |  4 |            _  | 2        | 10 |            _  | 12
+       |    *----|  R         Q  |---       *----|  R         Q  |----
+       |    |    |               |          |    |               |    |
+       |    |    |_______________|          |    |_______________|    |
+       |    |            |                  |                         |
+       |    |            | 7                |                         |
+       |    |            |                  |                         |
+        --------------------------------------------------------------
+            |            |                  |
+            |            |                  |
+          -----        -----              -----
+           ---          ---                ---
+            -            -                  -
+*/
+
+
+void setup() {
+  pinMode(outputPin, OUTPUT);
+}
+
+#if defined(__AVR__) || defined(TEENSYDUINO)
+#define REGTYPE unsigned char
+#else
+#define REGTYPE unsigned long
+#endif
+
+void loop() {
+  volatile int count = 0;
+  volatile REGTYPE *reg = portOutputRegister(digitalPinToPort(outputPin));
+  REGTYPE mask = digitalPinToBitMask(outputPin);
+
+  while (1) {
+    myEnc.read();	// Read the encoder while interrupts are enabled.
+    noInterrupts();
+    *reg |= mask;	// Pulse the pin high, while interrupts are disabled.
+    count = count + 1;
+    *reg &= ~mask;
+    interrupts();
+  }
+}
+
diff --git a/lib/Encoder/examples/TwoKnobs/TwoKnobs.pde b/lib/Encoder/examples/TwoKnobs/TwoKnobs.pde
new file mode 100644
index 0000000..306b33e
--- /dev/null
+++ b/lib/Encoder/examples/TwoKnobs/TwoKnobs.pde
@@ -0,0 +1,46 @@
+/* Encoder Library - TwoKnobs Example
+ * http://www.pjrc.com/teensy/td_libs_Encoder.html
+ *
+ * This example code is in the public domain.
+ */
+
+#include <Encoder.h>
+
+// Change these pin numbers to the pins connected to your encoder.
+//   Best Performance: both pins have interrupt capability
+//   Good Performance: only the first pin has interrupt capability
+//   Low Performance:  neither pin has interrupt capability
+Encoder knobLeft(5, 6);
+Encoder knobRight(7, 8);
+//   avoid using pins with LEDs attached
+
+void setup() {
+  Serial.begin(9600);
+  Serial.println("TwoKnobs Encoder Test:");
+}
+
+long positionLeft  = -999;
+long positionRight = -999;
+
+void loop() {
+  long newLeft, newRight;
+  newLeft = knobLeft.read();
+  newRight = knobRight.read();
+  if (newLeft != positionLeft || newRight != positionRight) {
+    Serial.print("Left = ");
+    Serial.print(newLeft);
+    Serial.print(", Right = ");
+    Serial.print(newRight);
+    Serial.println();
+    positionLeft = newLeft;
+    positionRight = newRight;
+  }
+  // if a character is sent from the serial monitor,
+  // reset both back to zero.
+  if (Serial.available()) {
+    Serial.read();
+    Serial.println("Reset both knobs to zero");
+    knobLeft.write(0);
+    knobRight.write(0);
+  }
+}
diff --git a/lib/Encoder/include/Encoder.h b/lib/Encoder/include/Encoder.h
deleted file mode 100644
index b0c30b6..0000000
--- a/lib/Encoder/include/Encoder.h
+++ /dev/null
@@ -1,64 +0,0 @@
-#ifndef ENCODER_H
-#define ENCODER_H
-
-#include <Arduino.h>
-
-class Encoder{
-public:
-    /**
-     * @brief Construct a new Encoder object with the given pins
-     *
-     * @param pinSW encoder switch pin
-     * @param pinDT encoder DT pin
-     * @param pinCLK encoder CLK pin
-     */
-    Encoder(int pinSW, int pinDT, int pinCLK);
-
-    /**
-     * @brief returns the instance of the encoder
-     */
-    static Encoder* getInstance();
-
-
-private:
-
-    /**
-     * @brief pinSW is the pin number of the switch
-     */
-    int pinSW;
-
-    /**
-     * @brief pinDT is the pin number of the DT pin
-     */
-    int pinDT;
-
-    /**
-     * @brief pinCLK is the pin number of the CLK pin
-     */
-    int pinCLK;
-
-    /**
-     * @brief swEvent is the event called when the switch is pressed
-     */
-    IRAM_ATTR static void swEvent();
-
-    /**
-     * @brief dtEvent is the event called when the encoder is turned
-     */
-    IRAM_ATTR static void dtEvent();
-
-    /**
-     * @brief number is the current number of the encoder
-     */
-    int number;
-
-    /**
-     * @brief instance is the instance of the encoder
-     */
-    static Encoder* instance;
-
-};
-
-
-
-#endif
diff --git a/lib/Encoder/keywords.txt b/lib/Encoder/keywords.txt
new file mode 100644
index 0000000..a4baa01
--- /dev/null
+++ b/lib/Encoder/keywords.txt
@@ -0,0 +1,4 @@
+ENCODER_USE_INTERRUPTS	LITERAL1
+ENCODER_OPTIMIZE_INTERRUPTS	LITERAL1
+ENCODER_DO_NOT_USE_INTERRUPTS	LITERAL1
+Encoder	KEYWORD1
diff --git a/lib/Encoder/library.properties b/lib/Encoder/library.properties
new file mode 100644
index 0000000..712abd4
--- /dev/null
+++ b/lib/Encoder/library.properties
@@ -0,0 +1,10 @@
+name=Encoder
+version=1.4.2
+author=Paul Stoffregen
+maintainer=Paul Stoffregen
+sentence=Counts quadrature pulses from rotary & linear position encoders.
+paragraph=Encoder counts pulses from quadrature encoded signals, which are commonly available from rotary knobs, motor or shaft sensors and other position sensors.
+category=Signal Input/Output
+url=http://www.pjrc.com/teensy/td_libs_Encoder.html
+architectures=*
+
diff --git a/lib/Encoder/src/Encoder.cpp b/lib/Encoder/src/Encoder.cpp
deleted file mode 100644
index 8ea77a4..0000000
--- a/lib/Encoder/src/Encoder.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-#include "../include/Encoder.h"
-
-Encoder* Encoder::instance = nullptr;
-
-Encoder* Encoder::getInstance() {
-    return Encoder::instance;
-}
-
-Encoder::Encoder(int pinSW, int pinDT, int pinCLK) {
-    this->pinSW = pinSW;
-    this->pinDT = pinDT;
-    this->pinCLK = pinCLK;
-
-    pinMode(pinSW, INPUT);
-    pinMode(pinDT, INPUT);
-    pinMode(pinCLK, INPUT);
-
-    attachInterrupt(digitalPinToInterrupt(this->pinCLK), Encoder::dtEvent, RISING);
-    attachInterrupt(digitalPinToInterrupt(this->pinSW), Encoder::swEvent, RISING);
-
-    this->number = 0;
-
-    Encoder::instance = this;
-}
-
-IRAM_ATTR void Encoder::dtEvent() {
-    Encoder* encoder = Encoder::getInstance();
-    if(digitalRead(encoder->pinDT)){
-        encoder->number++;
-    } else {
-        encoder->number--;
-    }
-    Serial.println(encoder->number);
-    delay(10);
-}
-
-IRAM_ATTR void Encoder::swEvent() {
-    Serial.print("test :");
-    Serial.print(digitalRead(ENCODER_SWITCH));
-    Serial.print(" ");
-    Serial.print(digitalRead(ENCODER_DT));
-    Serial.print(" ");
-    Serial.println(digitalRead(ENCODER_CLK));
-    delay(10);
-}
diff --git a/lib/Encoder/utility/direct_pin_read.h b/lib/Encoder/utility/direct_pin_read.h
new file mode 100644
index 0000000..493880b
--- /dev/null
+++ b/lib/Encoder/utility/direct_pin_read.h
@@ -0,0 +1,104 @@
+#ifndef direct_pin_read_h_
+#define direct_pin_read_h_
+
+#if defined(__AVR__)
+
+#define IO_REG_TYPE			uint8_t
+#define PIN_TO_BASEREG(pin)             (portInputRegister(digitalPinToPort(pin)))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)     (((*(base)) & (mask)) ? 1 : 0)
+
+#elif defined(TEENSYDUINO) && (defined(KINETISK) || defined(KINETISL))
+
+#define IO_REG_TYPE			uint8_t
+#define PIN_TO_BASEREG(pin)             (portInputRegister(digitalPinToPort(pin)))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)     (((*(base)) & (mask)) ? 1 : 0)
+
+#elif defined(__IMXRT1052__) || defined(__IMXRT1062__)
+
+#define IO_REG_TYPE			uint32_t
+#define PIN_TO_BASEREG(pin)             (portOutputRegister(pin))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)     (((*(base)) & (mask)) ? 1 : 0)
+
+#elif defined(__SAM3X8E__)  // || defined(ESP8266)
+
+#define IO_REG_TYPE			uint32_t
+#define PIN_TO_BASEREG(pin)             (portInputRegister(digitalPinToPort(pin)))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)     (((*(base)) & (mask)) ? 1 : 0)
+
+#elif defined(__PIC32MX__)
+
+#define IO_REG_TYPE			uint32_t
+#define PIN_TO_BASEREG(pin)             (portModeRegister(digitalPinToPort(pin)))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)	(((*(base+4)) & (mask)) ? 1 : 0)
+
+/* ESP8266 v2.0.0 Arduino workaround for bug https://github.com/esp8266/Arduino/issues/1110 */
+#elif defined(ESP8266)
+
+#define IO_REG_TYPE			uint32_t
+#define PIN_TO_BASEREG(pin)             ((volatile uint32_t *)(0x60000000+(0x318)))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)     (((*(base)) & (mask)) ? 1 : 0)
+
+/* ESP32  Arduino (https://github.com/espressif/arduino-esp32) */
+#elif defined(ESP32)
+
+#define IO_REG_TYPE			uint32_t
+#define PIN_TO_BASEREG(pin)             (portInputRegister(digitalPinToPort(pin)))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)     (((*(base)) & (mask)) ? 1 : 0)
+
+#elif defined(__SAMD21G18A__)
+
+#define IO_REG_TYPE                     uint32_t
+#define PIN_TO_BASEREG(pin)             portModeRegister(digitalPinToPort(pin))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)     (((*((base)+8)) & (mask)) ? 1 : 0)
+
+#elif defined(__SAMD51__)
+
+#define IO_REG_TYPE                     uint32_t
+#define PIN_TO_BASEREG(pin)             portInputRegister(digitalPinToPort(pin))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define DIRECT_PIN_READ(base, mask)     (((*(base)) & (mask)) ? 1 : 0)
+
+#elif defined(RBL_NRF51822)
+
+#define IO_REG_TYPE                     uint32_t
+#define PIN_TO_BASEREG(pin)             (0)
+#define PIN_TO_BITMASK(pin)             (pin)
+#define DIRECT_PIN_READ(base, pin)      nrf_gpio_pin_read(pin)
+
+#elif defined(__arc__) /* Arduino101/Genuino101 specifics */
+
+#include "scss_registers.h"
+#include "portable.h"
+#include "avr/pgmspace.h"
+#define GPIO_ID(pin)                    (g_APinDescription[pin].ulGPIOId)
+#define GPIO_TYPE(pin)                  (g_APinDescription[pin].ulGPIOType)
+#define GPIO_BASE(pin)                  (g_APinDescription[pin].ulGPIOBase)
+#define EXT_PORT_OFFSET_SS              0x0A
+#define EXT_PORT_OFFSET_SOC             0x50
+#define PIN_TO_BASEREG(pin)             ((volatile uint32_t *)g_APinDescription[pin].ulGPIOBase)
+#define PIN_TO_BITMASK(pin)             pin
+#define IO_REG_TYPE                     uint32_t
+static inline __attribute__((always_inline))
+IO_REG_TYPE directRead(volatile IO_REG_TYPE *base, IO_REG_TYPE pin)
+{
+    IO_REG_TYPE ret;
+    if (SS_GPIO == GPIO_TYPE(pin)) {
+        ret = READ_ARC_REG(((IO_REG_TYPE)base + EXT_PORT_OFFSET_SS));
+    } else {
+        ret = MMIO_REG_VAL_FROM_BASE((IO_REG_TYPE)base, EXT_PORT_OFFSET_SOC);
+    }
+    return ((ret >> GPIO_ID(pin)) & 0x01);
+}
+#define DIRECT_PIN_READ(base, pin)      directRead(base, pin)
+
+#endif
+
+#endif
diff --git a/lib/Encoder/utility/interrupt_config.h b/lib/Encoder/utility/interrupt_config.h
new file mode 100644
index 0000000..cde6adf
--- /dev/null
+++ b/lib/Encoder/utility/interrupt_config.h
@@ -0,0 +1,87 @@
+#if defined(__AVR__)
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
+#define attachInterrupt(num, func, mode) enableInterrupt(num)
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+#define SCRAMBLE_INT_ORDER(num) ((num < 4) ? num + 2 : ((num < 6) ? num - 4 : num))
+#define DESCRAMBLE_INT_ORDER(num) ((num < 2) ? num + 4 : ((num < 6) ? num - 2 : num))
+#else
+#define SCRAMBLE_INT_ORDER(num) (num)
+#define DESCRAMBLE_INT_ORDER(num) (num)
+#endif
+
+static void enableInterrupt(uint8_t num)
+{
+	switch (DESCRAMBLE_INT_ORDER(num)) {
+		#if defined(EICRA) && defined(EIMSK)
+		case 0:
+			EICRA = (EICRA & 0xFC) | 0x01;
+			EIMSK |= 0x01;
+			return;
+		case 1:
+			EICRA = (EICRA & 0xF3) | 0x04;
+			EIMSK |= 0x02;
+			return;
+		case 2:
+			EICRA = (EICRA & 0xCF) | 0x10;
+			EIMSK |= 0x04;
+			return;
+		case 3:
+			EICRA = (EICRA & 0x3F) | 0x40;
+			EIMSK |= 0x08;
+			return;
+		#elif defined(MCUCR) && defined(GICR)
+		case 0:
+			MCUCR = (MCUCR & ~((1 << ISC00) | (1 << ISC01))) | (mode << ISC00);
+			GICR |= (1 << INT0);
+			return;
+		case 1:
+			MCUCR = (MCUCR & ~((1 << ISC10) | (1 << ISC11))) | (mode << ISC10);
+			GICR |= (1 << INT1);
+			return;
+		#elif defined(MCUCR) && defined(GIMSK)
+		case 0:
+			MCUCR = (MCUCR & ~((1 << ISC00) | (1 << ISC01))) | (mode << ISC00);
+			GIMSK |= (1 << INT0);
+			return;
+		case 1:
+			MCUCR = (MCUCR & ~((1 << ISC10) | (1 << ISC11))) | (mode << ISC10);
+			GIMSK |= (1 << INT1);
+			return;
+		#endif
+		#if defined(EICRB) && defined(EIMSK)
+		case 4:
+			EICRB = (EICRB & 0xFC) | 0x01;
+			EIMSK |= 0x10;
+			return;
+		case 5:
+			EICRB = (EICRB & 0xF3) | 0x04;
+			EIMSK |= 0x20;
+			return;
+		case 6:
+			EICRB = (EICRB & 0xCF) | 0x10;
+			EIMSK |= 0x40;
+			return;
+		case 7:
+			EICRB = (EICRB & 0x3F) | 0x40;
+			EIMSK |= 0x80;
+			return;
+		#endif
+	}
+}
+
+#elif defined(__PIC32MX__)
+
+#ifdef ENCODER_OPTIMIZE_INTERRUPTS
+#undef ENCODER_OPTIMIZE_INTERRUPTS
+#endif
+
+#else
+
+#ifdef ENCODER_OPTIMIZE_INTERRUPTS
+#undef ENCODER_OPTIMIZE_INTERRUPTS
+#endif
+
+#endif
diff --git a/lib/Encoder/utility/interrupt_pins.h b/lib/Encoder/utility/interrupt_pins.h
new file mode 100644
index 0000000..304706f
--- /dev/null
+++ b/lib/Encoder/utility/interrupt_pins.h
@@ -0,0 +1,340 @@
+// interrupt pins for known boards
+
+// Teensy (and maybe others) define these automatically
+#if !defined(CORE_NUM_INTERRUPT)
+
+// Wiring boards
+#if defined(WIRING)
+  #define CORE_NUM_INTERRUPT	NUM_EXTERNAL_INTERRUPTS
+  #if NUM_EXTERNAL_INTERRUPTS > 0
+  #define CORE_INT0_PIN		EI0
+  #endif
+  #if NUM_EXTERNAL_INTERRUPTS > 1
+  #define CORE_INT1_PIN		EI1
+  #endif
+  #if NUM_EXTERNAL_INTERRUPTS > 2
+  #define CORE_INT2_PIN		EI2
+  #endif
+  #if NUM_EXTERNAL_INTERRUPTS > 3
+  #define CORE_INT3_PIN		EI3
+  #endif
+  #if NUM_EXTERNAL_INTERRUPTS > 4
+  #define CORE_INT4_PIN		EI4
+  #endif
+  #if NUM_EXTERNAL_INTERRUPTS > 5
+  #define CORE_INT5_PIN		EI5
+  #endif
+  #if NUM_EXTERNAL_INTERRUPTS > 6
+  #define CORE_INT6_PIN		EI6
+  #endif
+  #if NUM_EXTERNAL_INTERRUPTS > 7
+  #define CORE_INT7_PIN		EI7
+  #endif
+
+// Arduino Uno, Duemilanove, Diecimila, LilyPad, Mini, Fio, etc...
+#elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328PB__) ||defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__)
+  #define CORE_NUM_INTERRUPT	2
+  #define CORE_INT0_PIN		2
+  #define CORE_INT1_PIN		3
+
+// Arduino Mega
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+  #define CORE_NUM_INTERRUPT	6
+  #define CORE_INT0_PIN		2
+  #define CORE_INT1_PIN		3
+  #define CORE_INT2_PIN		21
+  #define CORE_INT3_PIN		20
+  #define CORE_INT4_PIN		19
+  #define CORE_INT5_PIN		18
+
+// Arduino Nano Every, Uno R2 Wifi
+#elif defined(__AVR_ATmega4809__)
+  #define CORE_NUM_INTERRUPT  22
+  #define CORE_INT0_PIN   0
+  #define CORE_INT1_PIN   1
+  #define CORE_INT2_PIN   2
+  #define CORE_INT3_PIN   3
+  #define CORE_INT4_PIN   4
+  #define CORE_INT5_PIN   5
+  #define CORE_INT6_PIN   6
+  #define CORE_INT7_PIN   7
+  #define CORE_INT8_PIN   8
+  #define CORE_INT9_PIN   9
+  #define CORE_INT10_PIN  10
+  #define CORE_INT11_PIN  11
+  #define CORE_INT12_PIN  12
+  #define CORE_INT13_PIN  13
+  #define CORE_INT14_PIN  14
+  #define CORE_INT15_PIN  15
+  #define CORE_INT16_PIN  16
+  #define CORE_INT17_PIN  17
+  #define CORE_INT18_PIN  18
+  #define CORE_INT19_PIN  19
+  #define CORE_INT20_PIN  20
+  #define CORE_INT21_PIN  21
+
+// Arduino Leonardo (untested)
+#elif defined(__AVR_ATmega32U4__) && !defined(CORE_TEENSY)
+  #define CORE_NUM_INTERRUPT	5
+  #define CORE_INT0_PIN		3
+  #define CORE_INT1_PIN		2
+  #define CORE_INT2_PIN		0
+  #define CORE_INT3_PIN		1
+  #define CORE_INT4_PIN		7
+
+// Sanguino (untested) and ATmega1284P
+#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) || defined(__AVR_ATmega1284P__)
+  #define CORE_NUM_INTERRUPT	3
+  #define CORE_INT0_PIN		10
+  #define CORE_INT1_PIN		11
+  #define CORE_INT2_PIN		2
+
+// ATmega32u2 and ATmega32u16 based boards with HoodLoader2
+#elif defined(__AVR_ATmega32U2__) || defined(__AVR_ATmega16U2__)
+  #define CORE_NUM_INTERRUPT 8
+  #define CORE_INT0_PIN 8
+  #define CORE_INT1_PIN 17
+  #define CORE_INT2_PIN 13
+  #define CORE_INT3_PIN 14
+  #define CORE_INT4_PIN 15
+  #define CORE_INT5_PIN 16
+  #define CORE_INT6_PIN 19
+  #define CORE_INT7_PIN 20
+
+// Chipkit Uno32 - attachInterrupt may not support CHANGE option
+#elif defined(__PIC32MX__) && defined(_BOARD_UNO_)
+  #define CORE_NUM_INTERRUPT	5
+  #define CORE_INT0_PIN		38
+  #define CORE_INT1_PIN		2
+  #define CORE_INT2_PIN		7
+  #define CORE_INT3_PIN		8
+  #define CORE_INT4_PIN		35
+
+// Chipkit Uno32 - attachInterrupt may not support CHANGE option
+#elif defined(__PIC32MX__) && defined(_BOARD_MEGA_)
+  #define CORE_NUM_INTERRUPT	5
+  #define CORE_INT0_PIN		3
+  #define CORE_INT1_PIN		2
+  #define CORE_INT2_PIN		7
+  #define CORE_INT3_PIN		21
+  #define CORE_INT4_PIN		20
+
+// http://hlt.media.mit.edu/?p=1229
+#elif defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__)
+  #define CORE_NUM_INTERRUPT    1
+  #define CORE_INT0_PIN		2
+
+// ATtiny441 ATtiny841
+#elif defined(__AVR_ATtiny441__) || defined(__AVR_ATtiny841__)
+  #define CORE_NUM_INTERRUPT	1
+  #define CORE_INT0_PIN		9
+
+//https://github.com/SpenceKonde/ATTinyCore/blob/master/avr/extras/ATtiny_x313.md
+#elif defined(__AVR_ATtinyX313__)
+  #define CORE_NUM_INTERRUPT    2
+  #define CORE_INT0_PIN		4
+  #define CORE_INT1_PIN		5
+ 
+// Attiny167 same core as abobe
+#elif defined(__AVR_ATtiny167__)
+  #define CORE_NUM_INTERRUPT	2
+  #define CORE_INT0_PIN		14
+  #define CORE_INT1_PIN		3
+
+  
+// Arduino Due
+#elif defined(__SAM3X8E__) 
+  #define CORE_NUM_INTERRUPT	54
+  #define CORE_INT0_PIN		0
+  #define CORE_INT1_PIN		1
+  #define CORE_INT2_PIN		2
+  #define CORE_INT3_PIN		3
+  #define CORE_INT4_PIN		4
+  #define CORE_INT5_PIN		5
+  #define CORE_INT6_PIN		6
+  #define CORE_INT7_PIN		7
+  #define CORE_INT8_PIN		8
+  #define CORE_INT9_PIN		9
+  #define CORE_INT10_PIN	10
+  #define CORE_INT11_PIN	11
+  #define CORE_INT12_PIN	12
+  #define CORE_INT13_PIN	13
+  #define CORE_INT14_PIN	14
+  #define CORE_INT15_PIN	15
+  #define CORE_INT16_PIN	16
+  #define CORE_INT17_PIN	17
+  #define CORE_INT18_PIN	18
+  #define CORE_INT19_PIN	19
+  #define CORE_INT20_PIN	20
+  #define CORE_INT21_PIN	21
+  #define CORE_INT22_PIN	22
+  #define CORE_INT23_PIN	23
+  #define CORE_INT24_PIN	24
+  #define CORE_INT25_PIN	25
+  #define CORE_INT26_PIN	26
+  #define CORE_INT27_PIN	27
+  #define CORE_INT28_PIN	28
+  #define CORE_INT29_PIN	29
+  #define CORE_INT30_PIN	30
+  #define CORE_INT31_PIN	31
+  #define CORE_INT32_PIN	32
+  #define CORE_INT33_PIN	33
+  #define CORE_INT34_PIN	34
+  #define CORE_INT35_PIN	35
+  #define CORE_INT36_PIN	36
+  #define CORE_INT37_PIN	37
+  #define CORE_INT38_PIN	38
+  #define CORE_INT39_PIN	39
+  #define CORE_INT40_PIN	40
+  #define CORE_INT41_PIN	41
+  #define CORE_INT42_PIN	42
+  #define CORE_INT43_PIN	43
+  #define CORE_INT44_PIN	44
+  #define CORE_INT45_PIN	45
+  #define CORE_INT46_PIN	46
+  #define CORE_INT47_PIN	47
+  #define CORE_INT48_PIN	48
+  #define CORE_INT49_PIN	49
+  #define CORE_INT50_PIN	50
+  #define CORE_INT51_PIN	51
+  #define CORE_INT52_PIN	52
+  #define CORE_INT53_PIN	53
+
+// ESP8266 (https://github.com/esp8266/Arduino/)
+#elif defined(ESP8266)
+  #define CORE_NUM_INTERRUPT EXTERNAL_NUM_INTERRUPTS
+  #define CORE_INT0_PIN		0
+  #define CORE_INT1_PIN		1
+  #define CORE_INT2_PIN		2
+  #define CORE_INT3_PIN		3
+  #define CORE_INT4_PIN		4
+  #define CORE_INT5_PIN		5
+  // GPIO6-GPIO11 are typically used to interface with the flash memory IC on 
+  // most esp8266 modules, so we should avoid adding interrupts to these pins.
+  #define CORE_INT12_PIN	12
+  #define CORE_INT13_PIN	13
+  #define CORE_INT14_PIN	14
+  #define CORE_INT15_PIN	15
+
+// ESP32 (https://github.com/espressif/arduino-esp32)
+#elif defined(ESP32)
+
+  #define CORE_NUM_INTERRUPT  40 
+  #define CORE_INT0_PIN		0
+  #define CORE_INT1_PIN		1
+  #define CORE_INT2_PIN		2
+  #define CORE_INT3_PIN		3
+  #define CORE_INT4_PIN		4
+  #define CORE_INT5_PIN		5
+  // GPIO6-GPIO11 are typically used to interface with the flash memory IC on 
+  // esp32, so we should avoid adding interrupts to these pins.
+  #define CORE_INT12_PIN	12
+  #define CORE_INT13_PIN	13
+  #define CORE_INT14_PIN	14
+  #define CORE_INT15_PIN	15
+  #define CORE_INT16_PIN        16
+  #define CORE_INT17_PIN        17
+  #define CORE_INT18_PIN        18
+  #define CORE_INT19_PIN        19
+  #define CORE_INT21_PIN        21
+  #define CORE_INT22_PIN        22
+  #define CORE_INT23_PIN        23
+  #define CORE_INT25_PIN        25
+  #define CORE_INT26_PIN        26
+  #define CORE_INT27_PIN        27
+  #define CORE_INT32_PIN        32
+  #define CORE_INT33_PIN        33
+  #define CORE_INT34_PIN        34
+  #define CORE_INT35_PIN        35
+  #define CORE_INT36_PIN        36
+  #define CORE_INT39_PIN        39
+
+
+// Arduino Zero - TODO: interrupts do not seem to work
+//                      please help, contribute a fix!
+#elif defined(__SAMD21G18A__)
+  #define CORE_NUM_INTERRUPT	31
+  #define CORE_INT0_PIN		0
+  #define CORE_INT1_PIN		1
+  #define CORE_INT2_PIN		2
+  #define CORE_INT3_PIN		3
+  #define CORE_INT4_PIN		4
+  #define CORE_INT5_PIN		5
+  #define CORE_INT6_PIN		6
+  #define CORE_INT7_PIN		7
+  #define CORE_INT8_PIN		8
+  #define CORE_INT9_PIN		9
+  #define CORE_INT10_PIN	10
+  #define CORE_INT11_PIN	11
+  #define CORE_INT12_PIN	12
+  #define CORE_INT13_PIN	13
+  #define CORE_INT14_PIN	14
+  #define CORE_INT15_PIN	15
+  #define CORE_INT16_PIN	16
+  #define CORE_INT17_PIN	17
+  #define CORE_INT18_PIN	18
+  #define CORE_INT19_PIN	19
+  #define CORE_INT20_PIN	20
+  #define CORE_INT21_PIN	21
+  #define CORE_INT22_PIN	22
+  #define CORE_INT23_PIN	23
+  #define CORE_INT24_PIN	24
+  #define CORE_INT25_PIN	25
+  #define CORE_INT26_PIN	26
+  #define CORE_INT27_PIN	27
+  #define CORE_INT28_PIN	28
+  #define CORE_INT29_PIN	29
+  #define CORE_INT30_PIN	30
+
+#elif defined(__SAMD51__)
+  #define CORE_NUM_INTERRUPT	26
+  #define CORE_INT0_PIN		0
+  #define CORE_INT1_PIN		1
+  #define CORE_INT2_PIN		2
+  #define CORE_INT3_PIN		3
+  #define CORE_INT4_PIN		4
+  #define CORE_INT5_PIN		5
+  #define CORE_INT6_PIN		6
+  #define CORE_INT7_PIN		7
+  #define CORE_INT8_PIN		8
+  #define CORE_INT9_PIN		9
+  #define CORE_INT10_PIN	10
+  #define CORE_INT11_PIN	11
+  #define CORE_INT12_PIN	12
+  #define CORE_INT13_PIN	13
+  #define CORE_INT14_PIN	14
+  #define CORE_INT15_PIN	15
+  #define CORE_INT16_PIN	16
+  #define CORE_INT17_PIN	17
+  #define CORE_INT18_PIN	18
+  #define CORE_INT19_PIN	19
+  #define CORE_INT20_PIN	20
+  #define CORE_INT21_PIN	21
+  #define CORE_INT22_PIN	22
+  #define CORE_INT23_PIN	23
+  #define CORE_INT24_PIN	24
+  #define CORE_INT25_PIN	25
+
+// Arduino 101
+#elif defined(__arc__)
+  #define CORE_NUM_INTERRUPT	14
+  #define CORE_INT2_PIN		2
+  #define CORE_INT5_PIN		5
+  #define CORE_INT7_PIN		7
+  #define CORE_INT8_PIN		8
+  #define CORE_INT10_PIN	10
+  #define CORE_INT11_PIN	11
+  #define CORE_INT12_PIN	12
+  #define CORE_INT13_PIN	13
+
+#endif
+#endif
+
+#if !defined(CORE_NUM_INTERRUPT)
+#error "Interrupts are unknown for this board, please add to this code"
+#endif
+#if CORE_NUM_INTERRUPT <= 0
+#error "Encoder requires interrupt pins, but this board does not have any :("
+#error "You could try defining ENCODER_DO_NOT_USE_INTERRUPTS as a kludge."
+#endif
+
diff --git a/src/Program.cpp b/src/Program.cpp
index 80a5656..2a01622 100644
--- a/src/Program.cpp
+++ b/src/Program.cpp
@@ -4,10 +4,15 @@ Program::Program() {
     // Startup
     Serial.begin(MONITOR_SPEED);
 
-    this->encoder = new Encoder(ENCODER_SWITCH, ENCODER_DT, ENCODER_CLK);
+    this->encoder = new Encoder(ENCODER_DT, ENCODER_CLK);
 
+    this->oldPosition = -999;
 }
 
 void Program::loop() {
-    // Loop
+    long newPosition = this->encoder->read()/4;
+    if (newPosition != this->oldPosition) {
+        this->oldPosition = newPosition;
+        Serial.println(newPosition);
+    }
 }
diff --git a/src/main.cpp b/src/main.cpp
index 74bf5a7..9daaeeb 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -9,3 +9,24 @@ void setup() {
 void loop() {
     program->loop();
 }
+
+
+// #include <Arduino.h>
+// #include <Encoder.h>
+
+// Encoder myEnc(ENCODER_DT, ENCODER_CLK);
+
+// void setup() {
+//     Serial.begin(MONITOR_SPEED);
+// }
+
+
+// long oldPosition  = -999;
+
+// void loop() {
+//     long newPosition = myEnc.read();
+//     if (newPosition != oldPosition) {
+//         oldPosition = newPosition;
+//         Serial.println(newPosition/4);
+//     }
+// }