dfasd

Encoder.cpp → EncoderMod.cpp

@@ -1,5 +1,5 @@
 
-#include "Encoder.h"
+#include "EncoderMod.h"
 
 // Yes, all the code is in the header file, to provide the user
 // configure options with #define (before they include it), and

Encoder.h → EncoderMod.h

@@ -26,8 +26,8 @@
  */
 
 
-#ifndef Encoder_h_
-#define Encoder_h_
+#ifndef EncoderMod_h_
+#define EncoderMod_h_
 
 #if defined(ARDUINO) && ARDUINO >= 100
 #include "Arduino.h"
@@ -51,6 +51,10 @@
 #define ENCODER_ARGLIST_SIZE 0
 #endif
 
+#define FILTER_TIME_LIMIT 10000 //uS at 100/255 pwm  we get about 1/0.004 = 250uS/tick so this averages about 4 ticks
+#define US_INTERVAL 50 //This must be lower than time difference between ticks will ever be.
+#define FILTER_INTERVALS (FILTER_TIME_LIMIT/US_INTERVAL)
+#define MAX_BUFFER_SIZE ((int)(FILTER_TIME_LIMIT / 166.66)) //For 10:1 micro metal gear motors. 1sec/(3000rpm at shaft * 10:1 * 12enc/rev) = 166uS/tick
 
 
 // All the data needed by interrupts is consolidated into this ugly struct
@@ -64,11 +68,18 @@ typedef struct {
 	IO_REG_TYPE            pin2_bitmask;
 	uint8_t                state;
 	int32_t                position;
+	unsigned long stepTime1;
+	unsigned long stepTime2;
+	signed int uSBuffer[MAX_BUFFER_SIZE];
+	int32_t bufferIndex;
+	int32_t newTicks;
+	unsigned long timeOfLastTick;
 } Encoder_internal_state_t;
 
 class Encoder
 {
 public:
+
 	Encoder(uint8_t pin1, uint8_t pin2) {
 		#ifdef INPUT_PULLUP
 		pinMode(pin1, INPUT_PULLUP);
@@ -89,6 +100,12 @@ class Encoder
 		// the initial state
 		delayMicroseconds(2000);
 		uint8_t s = 0;
+        encoder.stepTime1 = micros();
+		encoder.stepTime2 = encoder.stepTime1;
+		memset(encoder.uSBuffer, 0, MAX_BUFFER_SIZE*sizeof(int));
+		encoder.bufferIndex = 0;
+		encoder.newTicks = 0;
+		encoder.timeOfLastTick = micros();
 		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;
@@ -102,21 +119,101 @@ class Encoder
 
 #ifdef ENCODER_USE_INTERRUPTS
 	inline int32_t read() {
+        uint8_t old_SREG = SREG;
 		if (interrupts_in_use < 2) {
 			noInterrupts();
 			update(&encoder);
 		} else {
 			noInterrupts();
 		}
 		int32_t ret = encoder.position;
-		interrupts();
+        SREG = old_SREG;
 		return ret;
 	}
 	inline void write(int32_t p) {
+        uint8_t old_SREG = SREG;
 		noInterrupts();
 		encoder.position = p;
-		interrupts();
+        SREG = old_SREG;
 	}
+
+	inline float stepRate() {
+        int8_t old_SREG = SREG;
+        if (interrupts_in_use < 2) {
+            noInterrupts();
+            update(&encoder);
+        }
+        else {
+            noInterrupts();
+        }
+
+		float velocitySum = 0;
+		int index = encoder.bufferIndex - 1;
+		if(index < 0){
+			index += MAX_BUFFER_SIZE;
+		}
+		unsigned long timeSinceLastTick = micros() - encoder.timeOfLastTick;
+
+        SREG = old_SREG;
+
+		if(encoder.uSBuffer[index] == 0){
+			//The buffer is not initialized in the first position so just stop with what we have now. there has never been a tick
+			return 0.0;
+		}
+		//int sumIntervals = timeSinceLastTick/US_INTERVAL;
+		int sumIntervals = 0;
+		if(timeSinceLastTick > FILTER_TIME_LIMIT){
+			return 0.0;
+		}
+
+		while(1){
+			//Get how many discrete intervals
+			if(encoder.uSBuffer[index] == 0){
+				//The buffer is not initialized so just stop with what we have now.
+				break;
+			}
+			int intervals = abs(encoder.uSBuffer[index])/US_INTERVAL;
+			sumIntervals += intervals;
+			if(sumIntervals <= FILTER_INTERVALS){
+				velocitySum += intervals * (2.0/(float)encoder.uSBuffer[index]);
+			} else {
+				velocitySum += (intervals - (sumIntervals - FILTER_INTERVALS)) * (2.0/(float)encoder.uSBuffer[index]);
+				break;
+			}
+			index--;
+			if(index < 0 ){
+				index += MAX_BUFFER_SIZE;
+			}
+		}
+		float velocity = velocitySum / (float)FILTER_INTERVALS;
+        return velocity;
+    }
+
+	inline float extrapolate() {
+        uint8_t old_SREG = SREG;
+        if (interrupts_in_use < 2) {
+            noInterrupts();
+            update(&encoder);
+        }
+        else {
+            noInterrupts();
+        }
+        float lastRate = stepRate();
+        int32_t lastPosition = encoder.position;
+		unsigned long timeSinceLastTick = micros() - encoder.timeOfLastTick;
+        SREG = old_SREG;
+
+        float extrapolatedPosition = lastRate * timeSinceLastTick;
+        if (extrapolatedPosition > 1) {
+            return (lastPosition + 1);
+        }
+        else if (extrapolatedPosition < -1) {
+            return (lastPosition - 1);
+        }
+        else {
+            return (extrapolatedPosition + lastPosition);
+        }
+    }
 #else
 	inline int32_t read() {
 		update(&encoder);
@@ -125,9 +222,35 @@ class Encoder
 	inline void write(int32_t p) {
 		encoder.position = p;
 	}
+    inline float stepRate() {
+        float extrapolatedPosition = encoder.rate * encoder.stepTime + 0.5 * encoder.accel * encoder.stepTime * encoder.stepTime;
+
+        if (extrapolatedPosition > 1) {
+            return (1 / encoder.stepTime);
+        }
+        else if (extrapolatedPosition < -1) {
+            return (-1 / encoder.stepTime);
+        }
+        else {
+            return (encoder.rate + encoder.accel * encoder.stepTime)
+        }
+
+    }
+    inline float extrapolate() {
+        float extrapolatedPosition = encoder.rate * encoder.stepTime + 0.5 * encoder.accel * encoder.stepTime * encoder.stepTime;
+        if (extrapolatedPosition > 1) {
+            return (encoder.position + 1);
+        }
+        else if (extrapolatedPosition < -1) {
+            return (encoder.position - 1);
+        }
+        else {
+            return (extrapolatedPosition + encoder.position);
+        }
+    }
 #endif
 private:
-	Encoder_internal_state_t encoder;
+    Encoder_internal_state_t encoder;
 #ifdef ENCODER_USE_INTERRUPTS
 	uint8_t interrupts_in_use;
 #endif
@@ -274,19 +397,95 @@ class Encoder
 		if (p1val) state |= 4;
 		if (p2val) state |= 8;
 		arg->state = (state >> 2);
+//                           _______         _______       
+//               Pin1 ______|       |_______|       |______ Pin1
+// negative <---         _______         _______         __      --> positive
+//               Pin2 __|       |_______|       |_______|   Pin2
+				//	new	new	old	old
+		//	pin2	pin1	pin2	pin1	Result
+		//	----	----	----	----	------
+		//0	0	0	0	0	no movement
+		//1	0	0	0	1	+1 pin1 edge
+		//2	0	0	1	0	-1 pin2 edge
+		//3	0	0	1	1	+2  (assume pin1 edges only) 
+		//4	0	1	0	0	-1 pin1 edge
+		//5	0	1	0	1	no movement
+		//6	0	1	1	0	-2  (assume pin1 edges only)
+		//7	0	1	1	1	+1 pin2 edge
+		//8	1	0	0	0	+1 pin2 edge
+		//9	1	0	0	1	-2  (assume pin1 edges only)
+		//10	1	0	1	0	no movement
+		//11	1	0	1	1	-1 pin1 edge
+		//12	1	1	0	0	+2  (assume pin1 edges only)
+		//13	1	1	0	1	-1 pin2 edge
+		//14	1	1	1	0	+1 pin1 edge
+		//15	1	1	1	1	no movement
+		unsigned long microsTemp = micros();
+		arg->timeOfLastTick = microsTemp;
 		switch (state) {
-			case 1: case 7: case 8: case 14:
+			case 1: case 14: //+1 pin1 edge
+				arg->uSBuffer[arg->bufferIndex] = microsTemp - arg->stepTime1;
+				arg->bufferIndex++;
+				arg->stepTime1 = microsTemp;
+				if(arg->bufferIndex >= MAX_BUFFER_SIZE){
+					arg->bufferIndex = 0;
+				}
+				//arg->newTicks++;
 				arg->position++;
 				return;
-			case 2: case 4: case 11: case 13:
+
+			case 7: case 8: //+1 pin2 edge
+				arg->uSBuffer[arg->bufferIndex] = microsTemp - arg->stepTime2;
+				arg->bufferIndex++;
+				arg->stepTime2 = microsTemp;
+				if(arg->bufferIndex >= MAX_BUFFER_SIZE){
+					arg->bufferIndex = 0;
+				}
+				//arg->newTicks++;
+				arg->position++;
+				return;
+
+			case 2: case 13: //-1 pin2 edge
+				arg->uSBuffer[arg->bufferIndex] = -(microsTemp - arg->stepTime2);
+				arg->bufferIndex++;
+				arg->stepTime2 = microsTemp;
+				if(arg->bufferIndex >= MAX_BUFFER_SIZE){
+					arg->bufferIndex = 0;
+				}
+				//arg->newTicks++;
 				arg->position--;
 				return;
-			case 3: case 12:
-				arg->position += 2;
+
+			case 4: case 11: //-1 pin1 edge
+				arg->uSBuffer[arg->bufferIndex] = -(microsTemp - arg->stepTime1);
+				arg->bufferIndex++;
+				arg->stepTime1 = microsTemp;
+				if(arg->bufferIndex >= MAX_BUFFER_SIZE){
+					arg->bufferIndex = 0;
+				}
+				//arg->newTicks++;
+				arg->position--;
+				return;
+
+			//+2's -2's to come later
+			//Because you can't know which direction you were going
+			//You will have to infer it from the last time in the buffer
+			//Meaning finding out if its less than or more than 0.
+			//Based on that result you will place the non corrupted timer in
+			//The timebuffer 3 times.
+			//One for the edge you measured correctly. One for the corrupted timer. One for the next corrupted timer which could never be correct.
+			//You don't need to put it in 3 times, just multiply it by three by shifting over one bit and adding itself.
+			//Set a flag for the next update of the corrupted timer so it knows not to add itself and to reset itself.
+			/*
+            case 3: case 12: //+2
+                arg->position += 2;
 				return;
 			case 6: case 9:
-				arg->position -= 2;
+
+                arg->position -= 2;
 				return;
+				*/
+
 		}
 #endif
 	}

examples/Basic/Basic.pde

@@ -4,7 +4,7 @@
  * This example code is in the public domain.
  */
 
-#include <Encoder.h>
+#include <EncoderMod.h>
 
 // Change these two numbers to the pins connected to your encoder.
 //   Best Performance: both pins have interrupt capability

examples/NoInterrupts/NoInterrupts.pde

@@ -11,7 +11,7 @@
 // your program must call the read() function rapidly, or risk
 // missing changes in position.
 #define ENCODER_DO_NOT_USE_INTERRUPTS
-#include <Encoder.h>
+#include <EncoderMod.h>
 
 // Beware of Serial.print() speed.  Without interrupts, if you
 // transmit too much data with Serial.print() it can slow your

examples/SpeedTest/SpeedTest.pde

@@ -36,7 +36,7 @@
 // It must be defined before Encoder.h is included.
 //#define ENCODER_OPTIMIZE_INTERRUPTS
 
-#include <Encoder.h>
+#include <EncoderMod.h>
 #include "pins_arduino.h"
 
 // Change these two numbers to the pins connected to your encoder

examples/TwoKnobs/TwoKnobs.pde

@@ -4,7 +4,7 @@
  * This example code is in the public domain.
  */
 
-#include <Encoder.h>
+#include <EncoderMod.h>
 
 // Change these pin numbers to the pins connected to your encoder.
 //   Best Performance: both pins have interrupt capability

keywords.txt

@@ -1,4 +1,8 @@
 ENCODER_USE_INTERRUPTS	LITERAL1
 ENCODER_OPTIMIZE_INTERRUPTS	LITERAL1
 ENCODER_DO_NOT_USE_INTERRUPTS	LITERAL1
-Encoder	KEYWORD1
+EncoderMod KEYWORD1
+read KEYWORD2
+write KEYWORDD2
+stepRate KEYWORD2
+extrapolate KEYWORD2

library.json

@@ -1,11 +1,11 @@
 {
-  "name": "Encoder",
+  "name": "EncoderMod",
   "keywords": "encoder, signal, pulse",
   "description": "Encoder counts pulses from quadrature encoded signals, which are commonly available from rotary knobs, motor or shaft sensors and other position sensors",
   "repository":
   {
     "type": "git",
-    "url": "https://github.com/PaulStoffregen/Encoder.git"
+    "url": "https://github.com/QuentinTorg/EncoderMod"
   },
   "frameworks": "arduino",
   "platforms":

library.properties

@@ -1,4 +1,4 @@
-name=Encoder
+name=EncoderMod
 version=1.3
 author=Paul Stoffregen
 maintainer=Paul Stoffregen