diff --git a/Encoder.cpp b/EncoderMod.cpp similarity index 90% rename from Encoder.cpp rename to EncoderMod.cpp index 6911b4f..268c79c 100644 --- a/Encoder.cpp +++ b/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 diff --git a/Encoder.h b/EncoderMod.h similarity index 78% rename from Encoder.h rename to EncoderMod.h index 6745eab..f124f4c 100644 --- a/Encoder.h +++ b/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,6 +119,7 @@ class Encoder #ifdef ENCODER_USE_INTERRUPTS inline int32_t read() { + uint8_t old_SREG = SREG; if (interrupts_in_use < 2) { noInterrupts(); update(&encoder); @@ -109,14 +127,93 @@ class Encoder 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 } diff --git a/examples/Basic/Basic.pde b/examples/Basic/Basic.pde index 3394b58..ce853be 100644 --- a/examples/Basic/Basic.pde +++ b/examples/Basic/Basic.pde @@ -4,7 +4,7 @@ * This example code is in the public domain. */ -#include +#include // Change these two numbers to the pins connected to your encoder. // Best Performance: both pins have interrupt capability diff --git a/examples/NoInterrupts/NoInterrupts.pde b/examples/NoInterrupts/NoInterrupts.pde index b890652..254a03a 100644 --- a/examples/NoInterrupts/NoInterrupts.pde +++ b/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 +#include // Beware of Serial.print() speed. Without interrupts, if you // transmit too much data with Serial.print() it can slow your diff --git a/examples/SpeedTest/SpeedTest.pde b/examples/SpeedTest/SpeedTest.pde index 76cf350..a620fa7 100644 --- a/examples/SpeedTest/SpeedTest.pde +++ b/examples/SpeedTest/SpeedTest.pde @@ -36,7 +36,7 @@ // It must be defined before Encoder.h is included. //#define ENCODER_OPTIMIZE_INTERRUPTS -#include +#include #include "pins_arduino.h" // Change these two numbers to the pins connected to your encoder diff --git a/examples/TwoKnobs/TwoKnobs.pde b/examples/TwoKnobs/TwoKnobs.pde index 306b33e..e00ca2d 100644 --- a/examples/TwoKnobs/TwoKnobs.pde +++ b/examples/TwoKnobs/TwoKnobs.pde @@ -4,7 +4,7 @@ * This example code is in the public domain. */ -#include +#include // Change these pin numbers to the pins connected to your encoder. // Best Performance: both pins have interrupt capability diff --git a/keywords.txt b/keywords.txt index a4baa01..7a62895 100644 --- a/keywords.txt +++ b/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 diff --git a/library.json b/library.json index 0a0f653..07fee2a 100644 --- a/library.json +++ b/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": diff --git a/library.properties b/library.properties index 18062fc..4fdb244 100644 --- a/library.properties +++ b/library.properties @@ -1,4 +1,4 @@ -name=Encoder +name=EncoderMod version=1.3 author=Paul Stoffregen maintainer=Paul Stoffregen