From 63d1b7955ef70eff1e911833075a5ce80da04e62 Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Fri, 27 Nov 2015 20:31:36 -0500 Subject: [PATCH 01/27] Added speed functionality to the library MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Variables and math have been added to output steps/microsecond, but the syntax is incorrect. An error shows up on compile with the Encoder library saying “Invalid use of member (varialbe_name) in static member function” for the variables I’ve added. --- Encoder.h | 38 +++++++++++++++++++++++++++++++++----- 1 file changed, 33 insertions(+), 5 deletions(-) diff --git a/Encoder.h b/Encoder.h index 6745eab..a9df0fa 100644 --- a/Encoder.h +++ b/Encoder.h @@ -69,6 +69,8 @@ typedef struct { class Encoder { public: + + Encoder(uint8_t pin1, uint8_t pin2) { #ifdef INPUT_PULLUP pinMode(pin1, INPUT_PULLUP); @@ -89,6 +91,8 @@ class Encoder // the initial state delayMicroseconds(2000); uint8_t s = 0; + encoder.stepTime = 0; + encoder.rate = 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; @@ -117,6 +121,17 @@ class Encoder encoder.position = p; interrupts(); } + inline float stepRate() { + if (interrupts_in_use < 2) { + noInterrupts(); + update(&encoder); + } else { + noInterrupts(); + } + float ret = encoder.rate; + interrupts(); + return ret; + } #else inline int32_t read() { update(&encoder); @@ -125,9 +140,14 @@ class Encoder inline void write(int32_t p) { encoder.position = p; } + inline float stepRate() { + return encoder.rate; + } #endif private: - Encoder_internal_state_t encoder; + Encoder_internal_state_t encoder; + elapsedMicros stepTime; + float rate; #ifdef ENCODER_USE_INTERRUPTS uint8_t interrupts_in_use; #endif @@ -276,16 +296,24 @@ class Encoder arg->state = (state >> 2); switch (state) { case 1: case 7: case 8: case 14: - arg->position++; + encoder.rate = 1 / encoder.stepTime; + encoder.stepTime = 0; + arg->position++; return; case 2: case 4: case 11: case 13: - arg->position--; + encoder.rate = -1 / encoder.stepTime; + encoder.stepTime = 0; + arg->position--; return; case 3: case 12: - arg->position += 2; + encoder.rate = 2 / encoder.stepTime; + encoder.stepTime = 0; + arg->position += 2; return; case 6: case 9: - arg->position -= 2; + encoder.rate = -2 / encoder.stepTime; + encoder.stepTime = 0; + arg->position -= 2; return; } #endif From 6cde550ef5b346dbf5dc3cc6d0ee04d5e3e02a98 Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Fri, 27 Nov 2015 20:45:00 -0500 Subject: [PATCH 02/27] Uploaded outdated code on last commit Replacement of last commit --- Encoder.h | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/Encoder.h b/Encoder.h index a9df0fa..ec6d414 100644 --- a/Encoder.h +++ b/Encoder.h @@ -91,8 +91,8 @@ class Encoder // the initial state delayMicroseconds(2000); uint8_t s = 0; - encoder.stepTime = 0; - encoder.rate = 0; + stepTime = 0; + rate = 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; @@ -128,7 +128,7 @@ class Encoder } else { noInterrupts(); } - float ret = encoder.rate; + float ret = rate; interrupts(); return ret; } @@ -141,7 +141,7 @@ class Encoder encoder.position = p; } inline float stepRate() { - return encoder.rate; + return rate; } #endif private: @@ -296,23 +296,23 @@ class Encoder arg->state = (state >> 2); switch (state) { case 1: case 7: case 8: case 14: - encoder.rate = 1 / encoder.stepTime; - encoder.stepTime = 0; + rate = 1 / stepTime; + stepTime = 0; arg->position++; return; case 2: case 4: case 11: case 13: - encoder.rate = -1 / encoder.stepTime; - encoder.stepTime = 0; + rate = -1 / stepTime; + stepTime = 0; arg->position--; return; case 3: case 12: - encoder.rate = 2 / encoder.stepTime; - encoder.stepTime = 0; + rate = 2 / stepTime; + stepTime = 0; arg->position += 2; return; case 6: case 9: - encoder.rate = -2 / encoder.stepTime; - encoder.stepTime = 0; + rate = -2 / stepTime; + stepTime = 0; arg->position -= 2; return; } From a5464e4414ec4b371dc01c5df96bcce9a03e3861 Mon Sep 17 00:00:00 2001 From: Ryan Matthews Date: Fri, 27 Nov 2015 21:36:55 -0500 Subject: [PATCH 03/27] Fix compilation errors --- Encoder.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Encoder.h b/Encoder.h index ec6d414..e8925de 100644 --- a/Encoder.h +++ b/Encoder.h @@ -146,8 +146,8 @@ class Encoder #endif private: Encoder_internal_state_t encoder; - elapsedMicros stepTime; - float rate; + static elapsedMicros stepTime; + static float rate; #ifdef ENCODER_USE_INTERRUPTS uint8_t interrupts_in_use; #endif From 91190119c79a2e70b2e5aa90ee951d4414762856 Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Mon, 30 Nov 2015 00:53:32 -0500 Subject: [PATCH 04/27] Minor Updates fixed link to github so that the proper repository is accessed when arduino checks for updates. Added the static type in front of the elapsed micros and rate variables --- Encoder.h | 0 library.json | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) mode change 100644 => 100755 Encoder.h diff --git a/Encoder.h b/Encoder.h old mode 100644 new mode 100755 diff --git a/library.json b/library.json index 0a0f653..3a80d45 100644 --- a/library.json +++ b/library.json @@ -5,7 +5,7 @@ "repository": { "type": "git", - "url": "https://github.com/PaulStoffregen/Encoder.git" + "url": "https://github.com/QuentinTorg/Encoder" }, "frameworks": "arduino", "platforms": From f9958c484cf03c63c6db23a4869ab8f8d31ff492 Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Tue, 1 Dec 2015 00:08:04 -0500 Subject: [PATCH 05/27] Fixed accuracy of velocity functions Updated stepRate() function for accuracy. Now accounts for out of phase encoder signals by always averaging the latest two readings. Fixed compile errors Added proper keywords so arduino recognizes functions --- Encoder.h | 68 +++++++++++++++++++++++++++++++++++++++------------- keywords.txt | 3 +++ 2 files changed, 55 insertions(+), 16 deletions(-) diff --git a/Encoder.h b/Encoder.h index e8925de..758c771 100755 --- a/Encoder.h +++ b/Encoder.h @@ -64,6 +64,10 @@ typedef struct { IO_REG_TYPE pin2_bitmask; uint8_t state; int32_t position; + elapsedMicros stepTime; + float rate; + float rate1; + float rate2; } Encoder_internal_state_t; class Encoder @@ -91,8 +95,10 @@ class Encoder // the initial state delayMicroseconds(2000); uint8_t s = 0; - stepTime = 0; - rate = 0; + encoder.stepTime = 0; + encoder.rate = 0; + encoder.rate1 = 0; + encoder.rate2 = 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; @@ -128,7 +134,7 @@ class Encoder } else { noInterrupts(); } - float ret = rate; + float ret = encoder.rate; interrupts(); return ret; } @@ -141,13 +147,11 @@ class Encoder encoder.position = p; } inline float stepRate() { - return rate; + return encoder.rate; } #endif private: Encoder_internal_state_t encoder; - static elapsedMicros stepTime; - static float rate; #ifdef ENCODER_USE_INTERRUPTS uint8_t interrupts_in_use; #endif @@ -296,23 +300,55 @@ class Encoder arg->state = (state >> 2); switch (state) { case 1: case 7: case 8: case 14: - rate = 1 / stepTime; - stepTime = 0; + if (arg->position % 2 == 0) { + arg->rate1 = 0.5 / arg->stepTime; + } + else { + arg->rate2 = 0.5 / arg->stepTime; + } + arg->stepTime = 0; + arg->rate = (arg->rate1 + arg->rate2); arg->position++; - return; + return; case 2: case 4: case 11: case 13: - rate = -1 / stepTime; - stepTime = 0; + if (arg->position % 2 == 0) { + arg->rate1 = 0.5 / arg->stepTime; + } + else { + arg->rate2 = 0.5 / arg->stepTime; + } + arg->stepTime = 0; + arg->rate = (arg->rate1 + arg->rate2); arg->position--; return; - case 3: case 12: - rate = 2 / stepTime; - stepTime = 0; + case 3: case 12: + if (arg->position % 2 == 0) { + arg->rate1 = 1 / arg->stepTime; + arg->stepTime = 0; + arg->rate2 = arg->rate1; + arg->rate = arg->rate1; + } + else { + arg->rate2 = 1 / arg->stepTime; + arg->stepTime = 0; + arg->rate1 = arg->rate2; + arg->rate = arg->rate2; + } arg->position += 2; return; case 6: case 9: - rate = -2 / stepTime; - stepTime = 0; + if (arg->position % 2 == 0) { + arg->rate1 = 1 / arg->stepTime; + arg->stepTime = 0; + arg->rate2 = arg->rate1; + arg->rate = arg->rate1; + } + else { + arg->rate2 = 1 / arg->stepTime; + arg->stepTime = 0; + arg->rate1 = arg->rate2; + arg->rate = arg->rate2; + } arg->position -= 2; return; } diff --git a/keywords.txt b/keywords.txt index a4baa01..fc03c99 100644 --- a/keywords.txt +++ b/keywords.txt @@ -2,3 +2,6 @@ ENCODER_USE_INTERRUPTS LITERAL1 ENCODER_OPTIMIZE_INTERRUPTS LITERAL1 ENCODER_DO_NOT_USE_INTERRUPTS LITERAL1 Encoder KEYWORD1 +read KEYWORD2 +write KEYWORDD2 +stepRate KEYWORD2 From 50c85de7233a37e6d493abf0acd33f64aa1e46fc Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Tue, 1 Dec 2015 00:12:36 -0500 Subject: [PATCH 06/27] One last semicolon --- Encoder.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Encoder.h b/Encoder.h index 758c771..1608f20 100755 --- a/Encoder.h +++ b/Encoder.h @@ -98,7 +98,7 @@ class Encoder encoder.stepTime = 0; encoder.rate = 0; encoder.rate1 = 0; - encoder.rate2 = 0 + encoder.rate2 = 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; From e487c079410e94386089e8e7f70bf731b74e47d5 Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Tue, 1 Dec 2015 09:12:46 -0500 Subject: [PATCH 07/27] Corrected mistakes in calculation Negative movements now work on the opposite rate calculation of positive movements. When the encoder goes from 0-1 it has the same phase shift as going from 1-0. This means both should work on the same rate calculation Negative movements now calculate to negative rates --- Encoder.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/Encoder.h b/Encoder.h index 1608f20..e965c12 100755 --- a/Encoder.h +++ b/Encoder.h @@ -311,11 +311,11 @@ class Encoder arg->position++; return; case 2: case 4: case 11: case 13: - if (arg->position % 2 == 0) { - arg->rate1 = 0.5 / arg->stepTime; + if (arg->position % 2 != 0) { + arg->rate1 = -0.5 / arg->stepTime; } else { - arg->rate2 = 0.5 / arg->stepTime; + arg->rate2 = -0.5 / arg->stepTime; } arg->stepTime = 0; arg->rate = (arg->rate1 + arg->rate2); @@ -337,14 +337,14 @@ class Encoder arg->position += 2; return; case 6: case 9: - if (arg->position % 2 == 0) { - arg->rate1 = 1 / arg->stepTime; + if (arg->position % 2 != 0) { + arg->rate1 = -1 / arg->stepTime; arg->stepTime = 0; arg->rate2 = arg->rate1; arg->rate = arg->rate1; } else { - arg->rate2 = 1 / arg->stepTime; + arg->rate2 = -1 / arg->stepTime; arg->stepTime = 0; arg->rate1 = arg->rate2; arg->rate = arg->rate2; From 95fea8165ee1e83146951972fd72a302e96fedec Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Thu, 3 Dec 2015 21:34:28 -0500 Subject: [PATCH 08/27] Update and rename Encoder.h to EncoderMod.h Part of changing the library name to EncoderMod --- Encoder.h => EncoderMod.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) rename Encoder.h => EncoderMod.h (99%) diff --git a/Encoder.h b/EncoderMod.h similarity index 99% rename from Encoder.h rename to EncoderMod.h index e965c12..c473df0 100755 --- 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" From cc47eb4a669ce008c92917f65b93622e1702bb16 Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Thu, 3 Dec 2015 21:35:15 -0500 Subject: [PATCH 09/27] Update and rename Encoder.cpp to EncoderMod.cpp Converting library from Encoder to EncoderMod --- Encoder.cpp => EncoderMod.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename Encoder.cpp => EncoderMod.cpp (90%) 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 From cdc954267b3cd98549c92b676fea5b2d67cc050c Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Thu, 3 Dec 2015 21:38:09 -0500 Subject: [PATCH 10/27] Update library.json --- library.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.json b/library.json index 3a80d45..9b358b4 100644 --- a/library.json +++ b/library.json @@ -1,5 +1,5 @@ { - "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": From 5f69851700c06630fcdbf52cb14ded07dea83751 Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Fri, 4 Dec 2015 22:40:50 -0500 Subject: [PATCH 11/27] Updated examples examples now call EncoderMod.h instead of Encoder.h --- examples/Basic/Basic.pde | 2 +- examples/NoInterrupts/NoInterrupts.pde | 2 +- examples/SpeedTest/SpeedTest.pde | 2 +- examples/TwoKnobs/TwoKnobs.pde | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) 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 From cab71efbfa11af1e8e062b223361d89499e68608 Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Sat, 5 Dec 2015 01:19:51 -0500 Subject: [PATCH 12/27] Added extrapolation the extrapolation is calculated with the velocity until a distance of 1 step offset from the actual is reached, at which point it will be the maximum --- EncoderMod.h | 25 +++++++++++++++++++++++++ keywords.txt | 1 + 2 files changed, 26 insertions(+) diff --git a/EncoderMod.h b/EncoderMod.h index c473df0..463e4a1 100755 --- a/EncoderMod.h +++ b/EncoderMod.h @@ -138,6 +138,23 @@ class Encoder interrupts(); return ret; } + inline float extrapolate() { + if (interrupts_in_use < 2) { + noInterrupts(); + update(&encoder); + } else { + noInterrupts(); + } + float lastRate = encoder.rate; + int32_t lastPosition = encoder.position; + float extrapolation = lastRate * encoder.stepTime; + interrupts(); + if (extrapolation > 1) + extrapolation = 1; + else if (extrapolation < -1) + extrapolation = -1; + return (extrapolation + lastPosition); + } #else inline int32_t read() { update(&encoder); @@ -149,6 +166,14 @@ class Encoder inline float stepRate() { return encoder.rate; } + inline float extrapolate() { + float extrapolation = encoder.rate * encoder.stepTime; + if (extrapolation > 1) + extrapolation = 1; + else if (extrapolation < -1) + extrapolation = -1; + return (extrapolation + encoder.position); + } #endif private: Encoder_internal_state_t encoder; diff --git a/keywords.txt b/keywords.txt index fc03c99..dd08148 100644 --- a/keywords.txt +++ b/keywords.txt @@ -5,3 +5,4 @@ Encoder KEYWORD1 read KEYWORD2 write KEYWORDD2 stepRate KEYWORD2 +extrapolate KEYWORD2 From 9669203153c621f1646f31e4b9388aed04b5bd2a Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Sat, 5 Dec 2015 11:57:33 -0500 Subject: [PATCH 13/27] Improved step rate function MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The step rate function now uses the extrapolate function to calculate velocity. previously the step rate would only be updated when steps were taken, therefore when the encoder stopped moving, it would always return the last recorded step rate. Now, the step rate will deteriorate towards if steps aren’t recorded --- EncoderMod.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/EncoderMod.h b/EncoderMod.h index 463e4a1..bd0ce16 100755 --- a/EncoderMod.h +++ b/EncoderMod.h @@ -134,9 +134,9 @@ class Encoder } else { noInterrupts(); } - float ret = encoder.rate; + int32_t lastPosition = encoder.position; interrupts(); - return ret; + return ((extrapolate() - lastPosition) / encoder.stepTime); } inline float extrapolate() { if (interrupts_in_use < 2) { @@ -164,7 +164,7 @@ class Encoder encoder.position = p; } inline float stepRate() { - return encoder.rate; + return ((extrapolate() - encoder.position) / encoder.stepTime); } inline float extrapolate() { float extrapolation = encoder.rate * encoder.stepTime; From 9c98febd0fc885b31edb0e9512d9d680592dc6e5 Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Sat, 5 Dec 2015 20:17:23 -0500 Subject: [PATCH 14/27] Improved velocity function MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit New step rate function now uses the last recorded step rate until the predicted position is more than one step away, then it uses a position/time calculation to calculate a new velocity assuming the position is 1 step away. A position/time calculation doesn’t work when the elapsed time is below a certain value because it introduces inaccuracy, so the last step rate value is used --- EncoderMod.h | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/EncoderMod.h b/EncoderMod.h index bd0ce16..2d5189b 100755 --- a/EncoderMod.h +++ b/EncoderMod.h @@ -134,9 +134,16 @@ class Encoder } else { noInterrupts(); } - int32_t lastPosition = encoder.position; + loat lastRate = encoder.rate; + float elapsedTime = encoder.stepTime; interrupts(); - return ((extrapolate() - lastPosition) / encoder.stepTime); + float extrapolation = lastRate * elapsedTime; + if (extrapolation > 1) + return (1 / elapsedTime); + else if (extrapolation < -1) + return (-1 / elapsedTime); + else + return (lastRate); } inline float extrapolate() { if (interrupts_in_use < 2) { @@ -147,8 +154,9 @@ class Encoder } float lastRate = encoder.rate; int32_t lastPosition = encoder.position; - float extrapolation = lastRate * encoder.stepTime; + float extrapolation = encoder.stepTime; interrupts(); + extrapolation *= lastRate; if (extrapolation > 1) extrapolation = 1; else if (extrapolation < -1) From 7c837d25f21eb0703100c9b1a007cb97b499ccba Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Sat, 5 Dec 2015 20:58:01 -0500 Subject: [PATCH 15/27] Fixed direction change rate timer issue If a rate timer is double counted (during a direction change) then the new additions will now set the unchanged timer back to 0, giving more accurate positions --- EncoderMod.h | 47 +++++++++++++++++++++++++++++++++++++---------- 1 file changed, 37 insertions(+), 10 deletions(-) diff --git a/EncoderMod.h b/EncoderMod.h index 2d5189b..7ebca4a 100755 --- a/EncoderMod.h +++ b/EncoderMod.h @@ -68,6 +68,7 @@ typedef struct { float rate; float rate1; float rate2; + bool lastRateTimer; } Encoder_internal_state_t; class Encoder @@ -99,6 +100,7 @@ class Encoder encoder.rate = 0; encoder.rate1 = 0; encoder.rate2 = 0; + lastRateTimer = 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; @@ -131,25 +133,30 @@ class Encoder if (interrupts_in_use < 2) { noInterrupts(); update(&encoder); - } else { + } + else { noInterrupts(); } - loat lastRate = encoder.rate; + float lastRate = encoder.rate; float elapsedTime = encoder.stepTime; interrupts(); float extrapolation = lastRate * elapsedTime; - if (extrapolation > 1) + if (extrapolation > 1) { return (1 / elapsedTime); - else if (extrapolation < -1) + } + else if (extrapolation < -1) { return (-1 / elapsedTime); - else + } + else { return (lastRate); + } } inline float extrapolate() { if (interrupts_in_use < 2) { noInterrupts(); update(&encoder); - } else { + } + else { noInterrupts(); } float lastRate = encoder.rate; @@ -157,10 +164,12 @@ class Encoder float extrapolation = encoder.stepTime; interrupts(); extrapolation *= lastRate; - if (extrapolation > 1) + if (extrapolation > 1) { extrapolation = 1; - else if (extrapolation < -1) + } + else if (extrapolation < -1) { extrapolation = -1; + } return (extrapolation + lastPosition); } #else @@ -176,10 +185,12 @@ class Encoder } inline float extrapolate() { float extrapolation = encoder.rate * encoder.stepTime; - if (extrapolation > 1) + if (extrapolation > 1) { extrapolation = 1; - else if (extrapolation < -1) + } + else if (extrapolation < -1) { extrapolation = -1; + } return (extrapolation + encoder.position); } #endif @@ -335,9 +346,17 @@ class Encoder case 1: case 7: case 8: case 14: if (arg->position % 2 == 0) { arg->rate1 = 0.5 / arg->stepTime; + if (arg->lastRateTimer == 0) { + arg->rate2 = 0; + } + arg->lastRateTimer = 0; } else { arg->rate2 = 0.5 / arg->stepTime; + if (arg->lastRateTimer == 1) { + arg->rate1 = 0; + } + arg->lastRateTimer = 1; } arg->stepTime = 0; arg->rate = (arg->rate1 + arg->rate2); @@ -346,9 +365,17 @@ class Encoder case 2: case 4: case 11: case 13: if (arg->position % 2 != 0) { arg->rate1 = -0.5 / arg->stepTime; + if (arg->lastRateTimer == 0) { + arg->rate2 = 0; + } + arg->lastRateTimer = 0; } else { arg->rate2 = -0.5 / arg->stepTime; + if (arg->lastRateTimer == 1) { + arg->rate1 = 0; + } + arg->lastRateTimer = 1; } arg->stepTime = 0; arg->rate = (arg->rate1 + arg->rate2); From a798aba3a859c0d1c88ae5c5b32e64b186269b88 Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Sat, 5 Dec 2015 21:56:35 -0500 Subject: [PATCH 16/27] Fixed Compilation Issue --- EncoderMod.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EncoderMod.h b/EncoderMod.h index 7ebca4a..d53f9a7 100755 --- a/EncoderMod.h +++ b/EncoderMod.h @@ -100,7 +100,7 @@ class Encoder encoder.rate = 0; encoder.rate1 = 0; encoder.rate2 = 0; - lastRateTimer = 0; + encoder.lastRateTimer = 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; From 388a619542e674757106c61074a0deb2fd4ce3eb Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Sun, 6 Dec 2015 01:51:00 -0500 Subject: [PATCH 17/27] Added acceleration based position extrapolation Position extrapolation is now based on an acceleration calculated using the last two step rate measurements. This gives a much smoother extrapolation line. The stepRate() function now extrapolates based on the amount of time since last step rate calculation, and the previous acceleration. --- EncoderMod.h | 79 ++++++++++++++++++++++++++++++++++++---------------- 1 file changed, 55 insertions(+), 24 deletions(-) diff --git a/EncoderMod.h b/EncoderMod.h index d53f9a7..c56c8ce 100755 --- a/EncoderMod.h +++ b/EncoderMod.h @@ -68,6 +68,8 @@ typedef struct { float rate; float rate1; float rate2; + float previousRate; + float accel; bool lastRateTimer; } Encoder_internal_state_t; @@ -100,6 +102,8 @@ class Encoder encoder.rate = 0; encoder.rate1 = 0; encoder.rate2 = 0; + encoder.previousRate = 0; + encoder.accel = 0; encoder.lastRateTimer = 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; @@ -139,16 +143,17 @@ class Encoder } float lastRate = encoder.rate; float elapsedTime = encoder.stepTime; + float lastAccel = encoder.accel; interrupts(); - float extrapolation = lastRate * elapsedTime; - if (extrapolation > 1) { + float extrapolatedPosition = lastRate * elapsedTime + 0.5 * lastAccel * elapsedTime * elapsedTime; + if (extrapolatedPosition > 1) { return (1 / elapsedTime); } - else if (extrapolation < -1) { + else if (extrapolatedPosition < -1) { return (-1 / elapsedTime); } else { - return (lastRate); + return (lastRate + lastAccel * elapsedTime); } } inline float extrapolate() { @@ -161,16 +166,19 @@ class Encoder } float lastRate = encoder.rate; int32_t lastPosition = encoder.position; - float extrapolation = encoder.stepTime; + float extrapolatedPosition = encoder.stepTime; + float lastAccel = encoder.accel; interrupts(); - extrapolation *= lastRate; - if (extrapolation > 1) { - extrapolation = 1; + extrapolatedPosition = lastRate * extrapolatedPosition + 0.5 * lastAccel * extrapolatedPosition * extrapolatedPosition; + if (extrapolatedPosition > 1) { + return (lastPosition + 1); } - else if (extrapolation < -1) { - extrapolation = -1; + else if (extrapolatedPosition < -1) { + return (lastPosition - 1); + } + else { + return (extrapolatedPosition + lastPosition); } - return (extrapolation + lastPosition); } #else inline int32_t read() { @@ -181,17 +189,30 @@ class Encoder encoder.position = p; } inline float stepRate() { - return ((extrapolate() - encoder.position) / encoder.stepTime); + 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 extrapolation = encoder.rate * encoder.stepTime; - if (extrapolation > 1) { - extrapolation = 1; + 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 if (extrapolation < -1) { - extrapolation = -1; + else { + return (extrapolatedPosition + encoder.position); } - return (extrapolation + encoder.position); } #endif private: @@ -344,10 +365,12 @@ class Encoder arg->state = (state >> 2); switch (state) { case 1: case 7: case 8: case 14: + arg->previousRate = arg->rate; if (arg->position % 2 == 0) { arg->rate1 = 0.5 / arg->stepTime; if (arg->lastRateTimer == 0) { arg->rate2 = 0; + arg->previousRate = 0; } arg->lastRateTimer = 0; } @@ -355,18 +378,22 @@ class Encoder arg->rate2 = 0.5 / arg->stepTime; if (arg->lastRateTimer == 1) { arg->rate1 = 0; + arg->previousRate = 0; } arg->lastRateTimer = 1; } - arg->stepTime = 0; arg->rate = (arg->rate1 + arg->rate2); + arg->accel = (arg->rate - arg->previousRate) / arg->stepTime; + arg->stepTime = 0; arg->position++; return; case 2: case 4: case 11: case 13: + arg->previousRate = arg->rate; if (arg->position % 2 != 0) { arg->rate1 = -0.5 / arg->stepTime; if (arg->lastRateTimer == 0) { arg->rate2 = 0; + arg->previousRate = 0; } arg->lastRateTimer = 0; } @@ -374,41 +401,45 @@ class Encoder arg->rate2 = -0.5 / arg->stepTime; if (arg->lastRateTimer == 1) { arg->rate1 = 0; + arg->previousRate = 0; } arg->lastRateTimer = 1; } - arg->stepTime = 0; arg->rate = (arg->rate1 + arg->rate2); + arg->accel = (arg->rate - arg->previousRate) / arg->stepTime; + arg->stepTime = 0; arg->position--; return; case 3: case 12: + arg->previousRate = arg->rate; if (arg->position % 2 == 0) { arg->rate1 = 1 / arg->stepTime; - arg->stepTime = 0; arg->rate2 = arg->rate1; arg->rate = arg->rate1; } else { arg->rate2 = 1 / arg->stepTime; - arg->stepTime = 0; arg->rate1 = arg->rate2; arg->rate = arg->rate2; } + arg->accel = (arg->rate - arg->previousRate) / arg->stepTime; + arg->stepTime = 0; arg->position += 2; return; case 6: case 9: + arg->previousRate = arg->rate; if (arg->position % 2 != 0) { arg->rate1 = -1 / arg->stepTime; - arg->stepTime = 0; arg->rate2 = arg->rate1; arg->rate = arg->rate1; } else { arg->rate2 = -1 / arg->stepTime; - arg->stepTime = 0; arg->rate1 = arg->rate2; arg->rate = arg->rate2; } + arg->accel = (arg->rate - arg->previousRate) / arg->stepTime; + arg->stepTime = 0; arg->position -= 2; return; } From 7109b17307c0bd2bdcfc1feae0922c8f5a015f8d Mon Sep 17 00:00:00 2001 From: Ryan Matthews Date: Thu, 17 Dec 2015 00:50:17 -0500 Subject: [PATCH 18/27] Fix a compilation error, rename the library to EncoderMod I only modified one line in EncoderMod.h to get it to compile. I renamed a couple instances of the word "Encoder" to the word "EncoderMod" so that the library shows up as EncoderMod in the Arduino IDE. --- EncoderMod.h | 2 +- keywords.txt | 2 +- library.json | 2 +- library.properties | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) mode change 100755 => 100644 EncoderMod.h diff --git a/EncoderMod.h b/EncoderMod.h old mode 100755 new mode 100644 index 7ebca4a..d53f9a7 --- a/EncoderMod.h +++ b/EncoderMod.h @@ -100,7 +100,7 @@ class Encoder encoder.rate = 0; encoder.rate1 = 0; encoder.rate2 = 0; - lastRateTimer = 0; + encoder.lastRateTimer = 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; diff --git a/keywords.txt b/keywords.txt index dd08148..7a62895 100644 --- a/keywords.txt +++ b/keywords.txt @@ -1,7 +1,7 @@ ENCODER_USE_INTERRUPTS LITERAL1 ENCODER_OPTIMIZE_INTERRUPTS LITERAL1 ENCODER_DO_NOT_USE_INTERRUPTS LITERAL1 -Encoder KEYWORD1 +EncoderMod KEYWORD1 read KEYWORD2 write KEYWORDD2 stepRate KEYWORD2 diff --git a/library.json b/library.json index 9b358b4..07fee2a 100644 --- a/library.json +++ b/library.json @@ -5,7 +5,7 @@ "repository": { "type": "git", - "url": "https://github.com/QuentinTorg/Encoder" + "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 From 42d4a25f4724ed277fa92b6645c9a7b95886b051 Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Thu, 17 Dec 2015 20:31:14 -0500 Subject: [PATCH 19/27] Added a lot of comments I added a lot of comments about how and why things work. Its not fully commented yet, but the missing comments are mostly repeats of the first ones. --- EncoderMod.h | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/EncoderMod.h b/EncoderMod.h index c56c8ce..63f0f7e 100755 --- a/EncoderMod.h +++ b/EncoderMod.h @@ -365,22 +365,22 @@ class Encoder arg->state = (state >> 2); switch (state) { case 1: case 7: case 8: case 14: - arg->previousRate = arg->rate; - if (arg->position % 2 == 0) { - arg->rate1 = 0.5 / arg->stepTime; - if (arg->lastRateTimer == 0) { - arg->rate2 = 0; - arg->previousRate = 0; + arg->previousRate = arg->rate; // remember previous rate for calculating + if (arg->position % 2 == 0) { // if the previous position was even (0 to 1 step) + arg->rate1 = 0.5 / arg->stepTime; // the 0 to 1 step rate is set to rate1 + if (arg->lastRateTimer == 0) { // if the 0 to 1 step was the previous one calculated + arg->rate2 = 0; // then the rate2 step was skipped due to a direction change, so set it to zero + arg->previousRate = 0; // previous rate is also set to zero. there may be a better way but I have yet to think about it } - arg->lastRateTimer = 0; + arg->lastRateTimer = 0; // remember that rate1 was the last one calculated } - else { - arg->rate2 = 0.5 / arg->stepTime; - if (arg->lastRateTimer == 1) { - arg->rate1 = 0; - arg->previousRate = 0; + else { // if the previous position was odd (step -1 to 0) + arg->rate2 = 0.5 / arg->stepTime; // the -1 to 0 step rate is set to rate2 + if (arg->lastRateTimer == 1) { // if the -1 to 0 step was the previous one calculated + arg->rate1 = 0; // then rate1 step was skipped due to direction change, so it is set to zero + arg->previousRate = 0; // previous rate is also set to zero. there may be a better way but I have yet to think about it } - arg->lastRateTimer = 1; + arg->lastRateTimer = 1; // remember that rate2 was the last one calculated } arg->rate = (arg->rate1 + arg->rate2); arg->accel = (arg->rate - arg->previousRate) / arg->stepTime; @@ -389,15 +389,15 @@ class Encoder return; case 2: case 4: case 11: case 13: arg->previousRate = arg->rate; - if (arg->position % 2 != 0) { - arg->rate1 = -0.5 / arg->stepTime; + if (arg->position % 2 != 0) { // if the previous position was odd (1 to 0 step) + arg->rate1 = -0.5 / arg->stepTime; // the 1 to 0 step rate is set to rate1 if (arg->lastRateTimer == 0) { arg->rate2 = 0; arg->previousRate = 0; } arg->lastRateTimer = 0; } - else { + else { // if the previous position was even arg->rate2 = -0.5 / arg->stepTime; if (arg->lastRateTimer == 1) { arg->rate1 = 0; From 912cf9c64fbe23ef519ca2eda69099c79b484d7d Mon Sep 17 00:00:00 2001 From: Aaron Date: Fri, 8 Jan 2016 17:46:08 -0500 Subject: [PATCH 20/27] Implemented proper SREG handling --- EncoderMod.h | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/EncoderMod.h b/EncoderMod.h index 63f0f7e..f91a7a1 100644 --- a/EncoderMod.h +++ b/EncoderMod.h @@ -118,6 +118,7 @@ class Encoder #ifdef ENCODER_USE_INTERRUPTS inline int32_t read() { + uint8_t old_SREG = SREG; if (interrupts_in_use < 2) { noInterrupts(); update(&encoder); @@ -125,15 +126,17 @@ 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() { + uint8_t old_SREG = SREG; if (interrupts_in_use < 2) { noInterrupts(); update(&encoder); @@ -144,7 +147,7 @@ class Encoder float lastRate = encoder.rate; float elapsedTime = encoder.stepTime; float lastAccel = encoder.accel; - interrupts(); + SREG = old_SREG; float extrapolatedPosition = lastRate * elapsedTime + 0.5 * lastAccel * elapsedTime * elapsedTime; if (extrapolatedPosition > 1) { return (1 / elapsedTime); @@ -157,6 +160,7 @@ class Encoder } } inline float extrapolate() { + uint8_t old_SREG = SREG; if (interrupts_in_use < 2) { noInterrupts(); update(&encoder); @@ -168,7 +172,7 @@ class Encoder int32_t lastPosition = encoder.position; float extrapolatedPosition = encoder.stepTime; float lastAccel = encoder.accel; - interrupts(); + SREG = old_SREG; extrapolatedPosition = lastRate * extrapolatedPosition + 0.5 * lastAccel * extrapolatedPosition * extrapolatedPosition; if (extrapolatedPosition > 1) { return (lastPosition + 1); From 9ec47e2d17f53d3142236f70cfeeb68d2c1526b3 Mon Sep 17 00:00:00 2001 From: aftersomemath Date: Tue, 29 Mar 2016 20:51:12 -0400 Subject: [PATCH 21/27] Initial setup, much to do --- EncoderMod.h | 160 ++++++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 134 insertions(+), 26 deletions(-) diff --git a/EncoderMod.h b/EncoderMod.h index f91a7a1..7e6fa1c 100644 --- a/EncoderMod.h +++ b/EncoderMod.h @@ -51,6 +51,8 @@ #define ENCODER_ARGLIST_SIZE 0 #endif +#define FILTER_TIME_LIMIT 1000 //uS at 100/255 pwm we get about 1/0.004 = 250uS/tick so this averages about 4 ticks +#define MAX_BUFFER_SIZE 100 //This needs to be larger than the max number of ticks that can happen in our filter time limit it needs to be larger so that memory access violations don't occur // All the data needed by interrupts is consolidated into this ugly struct @@ -64,13 +66,19 @@ typedef struct { IO_REG_TYPE pin2_bitmask; uint8_t state; int32_t position; - elapsedMicros stepTime; - float rate; - float rate1; - float rate2; - float previousRate; - float accel; - bool lastRateTimer; + //elapsedMicros stepTime1; + //elapsedMicros stepTime2; + unsigned long stepTime1; + unsigned long stepTime2; + //float rate; + //float rate1; + //float rate2; + //float previousRate; + //float accel; + //bool lastRateTimer; + signed long uSBuffer[MAX_BUFFER_SIZE]; + int32_t bufferIndex; + int32_t newTicks; } Encoder_internal_state_t; class Encoder @@ -98,13 +106,17 @@ class Encoder // the initial state delayMicroseconds(2000); uint8_t s = 0; - encoder.stepTime = 0; - encoder.rate = 0; - encoder.rate1 = 0; - encoder.rate2 = 0; - encoder.previousRate = 0; - encoder.accel = 0; - encoder.lastRateTimer = 0; + encoder.stepTime1 = micros(); + encoder.stepTime2 = encoder.stepTime1; + //encoder.rate = 0; + //encoder.rate1 = 0; + //encoder.rate2 = 0; + //encoder.previousRate = 0; + //encoder.accel = 0; + //encoder.lastRateTimer = 0; + memset(encoder.uSBuffer, 0, MAX_BUFFER_SIZE); + encoder.bufferIndex = 0; + encoder.newTicks = 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; @@ -136,7 +148,7 @@ class Encoder SREG = old_SREG; } inline float stepRate() { - uint8_t old_SREG = SREG; + int8_t old_SREG = SREG; if (interrupts_in_use < 2) { noInterrupts(); update(&encoder); @@ -144,22 +156,46 @@ class Encoder else { noInterrupts(); } - float lastRate = encoder.rate; - float elapsedTime = encoder.stepTime; - float lastAccel = encoder.accel; + //float lastRate = encoder.rate; + //float elapsedTime = encoder.stepTime; + //float lastAccel = encoder.accel; + + float velocitySum = 0; + int index = encoder.bufferIndex; + int timeSum = 0; + int ticks = 0; + SREG = old_SREG; - float extrapolatedPosition = lastRate * elapsedTime + 0.5 * lastAccel * elapsedTime * elapsedTime; + interrupts(); + + while(1){ + timeSum += encoder.uSBuffer[index]; + + if(timeSum > FILTER_TIME_LIMIT){ + break; + } + velocitySum += 1.0/(float)encoder.uSBuffer[index]; //Should cache the array index instead of accessing twice + ticks++; + index = (index - 1) % MAX_BUFFER_SIZE; + if(ticks >= MAX_BUFFER_SIZE){ + break;//We have summed every item in the buffer + } + } + + float velocity = velocitySum / (float)ticks; + + /*float extrapolatedPosition = velocity * timeSum; //+ 0.5 * lastAccel * elapsedTime * elapsedTime; if (extrapolatedPosition > 1) { - return (1 / elapsedTime); + return (1 / timeSum); } else if (extrapolatedPosition < -1) { - return (-1 / elapsedTime); - } - else { - return (lastRate + lastAccel * elapsedTime); + return (-1 / timeSum); } + else {*/ + return velocity; + //} } - inline float extrapolate() { + /*inline float extrapolate() { uint8_t old_SREG = SREG; if (interrupts_in_use < 2) { noInterrupts(); @@ -183,7 +219,7 @@ class Encoder else { return (extrapolatedPosition + lastPosition); } - } + }*/ #else inline int32_t read() { update(&encoder); @@ -367,7 +403,78 @@ 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(); switch (state) { + case 1: case 14: //+1 pin1 edge + arg->uSBuffer[arg->bufferIndex] = microsTemp - arg->stepTime1; + arg->stepTime1 = microsTemp; + arg->bufferIndex++; + if(arg->bufferIndex > MAX_BUFFER_SIZE){ + arg->bufferIndex = 0; + } + //arg->newTicks++; + arg->position++; + return; + + case 7: case 8: //+1 pin2 edge + arg->uSBuffer[arg->bufferIndex] = microsTemp - arg->stepTime2; + arg->stepTime2 = microsTemp; + arg->bufferIndex++; + 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->stepTime2 = microsTemp; + arg->bufferIndex++; + if(arg->bufferIndex > MAX_BUFFER_SIZE){ + arg->bufferIndex = 0; + } + //arg->newTicks++;*/ + arg->position--; + return; + + case 4: case 11: //-1 pin1 edge + arg->uSBuffer[arg->bufferIndex] = -(microsTemp - arg->stepTime1); + arg->stepTime1 = microsTemp; + arg->bufferIndex++; + if(arg->bufferIndex > MAX_BUFFER_SIZE){ + arg->bufferIndex = 0; + } + //arg->newTicks++;*/ + arg->position--; + return; + + //+2's -2's to come later + + /* case 1: case 7: case 8: case 14: arg->previousRate = arg->rate; // remember previous rate for calculating if (arg->position % 2 == 0) { // if the previous position was even (0 to 1 step) @@ -446,6 +553,7 @@ class Encoder arg->stepTime = 0; arg->position -= 2; return; + */ } #endif } From f6e682e397d812c512c6f7f5bad1cc22b5be44e1 Mon Sep 17 00:00:00 2001 From: aftersomemath Date: Thu, 31 Mar 2016 20:27:14 -0400 Subject: [PATCH 22/27] Got it working! Filter time is easily set, it is not as optimized as it could be. However the interrupt is extremely tight --- EncoderMod.h | 259 ++++++++++++++++++++++++++++++++++++++++----------- 1 file changed, 203 insertions(+), 56 deletions(-) diff --git a/EncoderMod.h b/EncoderMod.h index 7e6fa1c..ee9d728 100644 --- a/EncoderMod.h +++ b/EncoderMod.h @@ -51,8 +51,10 @@ #define ENCODER_ARGLIST_SIZE 0 #endif -#define FILTER_TIME_LIMIT 1000 //uS at 100/255 pwm we get about 1/0.004 = 250uS/tick so this averages about 4 ticks -#define MAX_BUFFER_SIZE 100 //This needs to be larger than the max number of ticks that can happen in our filter time limit it needs to be larger so that memory access violations don't occur +#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 5000 //This needs to be larger than the max number of ticks that can happen in our filter time limit it needs to be larger so that memory access violations don't occur // All the data needed by interrupts is consolidated into this ugly struct @@ -66,26 +68,18 @@ typedef struct { IO_REG_TYPE pin2_bitmask; uint8_t state; int32_t position; - //elapsedMicros stepTime1; - //elapsedMicros stepTime2; unsigned long stepTime1; unsigned long stepTime2; - //float rate; - //float rate1; - //float rate2; - //float previousRate; - //float accel; - //bool lastRateTimer; - signed long uSBuffer[MAX_BUFFER_SIZE]; + 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); @@ -108,15 +102,10 @@ class Encoder uint8_t s = 0; encoder.stepTime1 = micros(); encoder.stepTime2 = encoder.stepTime1; - //encoder.rate = 0; - //encoder.rate1 = 0; - //encoder.rate2 = 0; - //encoder.previousRate = 0; - //encoder.accel = 0; - //encoder.lastRateTimer = 0; - memset(encoder.uSBuffer, 0, MAX_BUFFER_SIZE); + 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; @@ -147,7 +136,8 @@ class Encoder encoder.position = p; SREG = old_SREG; } - inline float stepRate() { + + inline float stepRate() { int8_t old_SREG = SREG; if (interrupts_in_use < 2) { noInterrupts(); @@ -156,45 +146,201 @@ class Encoder else { noInterrupts(); } - //float lastRate = encoder.rate; - //float elapsedTime = encoder.stepTime; - //float lastAccel = encoder.accel; float velocitySum = 0; - int index = encoder.bufferIndex; + int index = encoder.bufferIndex - 1; + if(index < 0){ + index += MAX_BUFFER_SIZE; + } int timeSum = 0; int ticks = 0; + unsigned long timeSinceLastTick = micros() - encoder.timeOfLastTick; SREG = old_SREG; interrupts(); + + 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){ - timeSum += encoder.uSBuffer[index]; - - if(timeSum > FILTER_TIME_LIMIT){ + //Get how many discrete intervals + if(encoder.uSBuffer[index] == 0){ + //The buffer is not initialized so just stop with what we have now. break; } - velocitySum += 1.0/(float)encoder.uSBuffer[index]; //Should cache the array index instead of accessing twice - ticks++; - index = (index - 1) % MAX_BUFFER_SIZE; - if(ticks >= MAX_BUFFER_SIZE){ - break;//We have summed every item in the buffer + 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)ticks; - - /*float extrapolatedPosition = velocity * timeSum; //+ 0.5 * lastAccel * elapsedTime * elapsedTime; - if (extrapolatedPosition > 1) { - return (1 / timeSum); + float velocity = velocitySum / (float)FILTER_INTERVALS; + return velocity; + } + + + inline float stepRateOptimized(){ + int8_t old_SREG = SREG; + if (interrupts_in_use < 2) { + noInterrupts(); + update(&encoder); } - else if (extrapolatedPosition < -1) { - return (-1 / timeSum); + else { + noInterrupts(); } - else {*/ - return velocity; - //} - } + + float static velocitySum = 0; + int static timeSum = 0; + //int static oldTicks = 0; + int static ticksInAverage = 0; + //int ticks = 0; + int bufferIndex = encoder.bufferIndex - 1; + if(bufferIndex < 0){ + bufferIndex+=MAX_BUFFER_SIZE; + } + int newTicks = encoder.newTicks; + encoder.newTicks = 0; + + SREG = old_SREG; + interrupts(); + + //Only do this if we have newTicks + if(newTicks > 0){ + //Add the all the newticks + int stopIndex = bufferIndex - newTicks; //We are going backwards, stop at the index where the newticks stop + if(stopIndex < 0){ + stopIndex += MAX_BUFFER_SIZE; + } + int i = bufferIndex; + //timeSum = 0; + //Serial.print("Newticks: "); + //Serial.print(newTicks); + //Serial.print(" "); + //Serial.println(); + //Serial.print("velocitysum before:"); + //Serial.println(velocitySum); + //Serial.print(" "); + for(int j = 0; j < newTicks; j++){ + timeSum += abs(encoder.uSBuffer[i]); //Add the time to the buffer + velocitySum += 1.0/(float)encoder.uSBuffer[i]; //Should catch this access to uSBuffer, also this is just adding the newvelocities to the sum this could lose precision on the float + //Serial.print("velocitysum:"); + //Serial.print(velocitySum); + //Serial.print(" "); + //Serial.print("added:"); + //Serial.println(1.0/(float)encoder.uSBuffer[i]); + /*if(abs(velocitySum) > 100){ + int temp = encoder.uSBuffer[i]; + //Serial.print(" "); + //delay(5000); + Serial.print("index:"); + Serial.print(i); + Serial.print(" "); + Serial.print("buffer value:"); + Serial.print(temp); + Serial.print(" "); + Serial.print("velocitysum:"); + Serial.print(velocitySum); + Serial.print(" "); + Serial.print("added:"); + Serial.print(1.0/temp); + Serial.print(" "); + delay(5000); + /*for(int i = 0; i < 100000; i++){ + Serial.println(temp); + delay(10); + }*/ + //} + //if(abs(velocitySum) > 10000000000){ + // velocitySum = 0; //Should catch this access to uSBuffer, also this is just adding the newvelocities to the sum this could lose precision on the float + //} + ticksInAverage++; //Increase the number of ticks + i--; + if(i <= stopIndex){ + break; + } + if(i < 0){ + i += MAX_BUFFER_SIZE; + } + } + //velocitySum = 0; + //Serial.print("velocitysum:"); + //Serial.print(velocitySum); + //Serial.print(" "); + /*if(abs(velocitySum) > 10000000000){ + Serial.print("BAD "); + }*/ + + + if(ticksInAverage > MAX_BUFFER_SIZE){ + ticksInAverage = MAX_BUFFER_SIZE; + } + + int startIndex = bufferIndex - ticksInAverage; + //Serial.print("start:"); + //Serial.print(startIndex); + //Serial.print(" "); + if(startIndex < 0){ + startIndex += MAX_BUFFER_SIZE; + } + /*Serial.print("start:"); + Serial.print(startIndex); + Serial.print(" "); + Serial.print(ticksInAverage); + Serial.print(" "); + //Serial.print(timeSum); + //Serial.print(" ");*/ + int loopcounts = 0; + int totalTicks = ticksInAverage; + for(int i = 0; i < totalTicks; i++){ + //for(int i = startIndex; i < bufferIndex; i = (i+1)%MAX_BUFFER_SIZE) { //Stop just before our current buffer index if we do get that far + loopcounts++; + if(timeSum < FILTER_TIME_LIMIT){ //This is our normal case. Stop subtracting when we are back within bounds + /*Serial.print("TIMELIMIT:"); + Serial.print(ticksInAverage); + Serial.print(" "); + Serial.print(timeSum); + Serial.print(" ");*/ + break; + } + + int index = bufferIndex-totalTicks+i; + if(index < 0) + index+=MAX_BUFFER_SIZE; + timeSum -= abs(encoder.uSBuffer[index]); //remove the time; + ticksInAverage--; //We have less ticks now + velocitySum -= 1.0/(float)encoder.uSBuffer[index]; //Again this access could be cached + } + /*Serial.print("loops:"); + Serial.print(loopcounts); + Serial.print(" "); + Serial.print(ticksInAverage); + Serial.print(" "); + Serial.print(timeSum); + Serial.print(" ");*/ + } + + float velocity = velocitySum / (float)(ticksInAverage/2.0); + Serial.print("ticksinAverage: "); + Serial.print(ticksInAverage); + Serial.print(" "); + //Serial.print("VELOCITY: "); + //Serial.println(velocity * 10000); + return velocity; + } /*inline float extrapolate() { uint8_t old_SREG = SREG; if (interrupts_in_use < 2) { @@ -427,48 +573,49 @@ class Encoder //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 14: //+1 pin1 edge arg->uSBuffer[arg->bufferIndex] = microsTemp - arg->stepTime1; - arg->stepTime1 = microsTemp; arg->bufferIndex++; - if(arg->bufferIndex > MAX_BUFFER_SIZE){ + arg->stepTime1 = microsTemp; + if(arg->bufferIndex >= MAX_BUFFER_SIZE){ arg->bufferIndex = 0; } - //arg->newTicks++; + arg->newTicks++; arg->position++; return; case 7: case 8: //+1 pin2 edge arg->uSBuffer[arg->bufferIndex] = microsTemp - arg->stepTime2; - arg->stepTime2 = microsTemp; arg->bufferIndex++; - if(arg->bufferIndex > MAX_BUFFER_SIZE){ + arg->stepTime2 = microsTemp; + if(arg->bufferIndex >= MAX_BUFFER_SIZE){ arg->bufferIndex = 0; } - //arg->newTicks++;*/ + arg->newTicks++; arg->position++; return; case 2: case 13: //-1 pin2 edge arg->uSBuffer[arg->bufferIndex] = -(microsTemp - arg->stepTime2); - arg->stepTime2 = microsTemp; arg->bufferIndex++; - if(arg->bufferIndex > MAX_BUFFER_SIZE){ + arg->stepTime2 = microsTemp; + if(arg->bufferIndex >= MAX_BUFFER_SIZE){ arg->bufferIndex = 0; } - //arg->newTicks++;*/ + arg->newTicks++; arg->position--; return; case 4: case 11: //-1 pin1 edge arg->uSBuffer[arg->bufferIndex] = -(microsTemp - arg->stepTime1); - arg->stepTime1 = microsTemp; arg->bufferIndex++; - if(arg->bufferIndex > MAX_BUFFER_SIZE){ + arg->stepTime1 = microsTemp; + if(arg->bufferIndex >= MAX_BUFFER_SIZE){ arg->bufferIndex = 0; } - //arg->newTicks++;*/ + arg->newTicks++; arg->position--; return; From 9b5ef204a19f10dbd6551200c962147c393f819d Mon Sep 17 00:00:00 2001 From: aftersomemath Date: Thu, 31 Mar 2016 21:28:02 -0400 Subject: [PATCH 23/27] Cleaned up the code, added extrapolate --- EncoderMod.h | 258 +++++---------------------------------------------- 1 file changed, 23 insertions(+), 235 deletions(-) diff --git a/EncoderMod.h b/EncoderMod.h index ee9d728..31efb5a 100644 --- a/EncoderMod.h +++ b/EncoderMod.h @@ -191,157 +191,8 @@ class Encoder float velocity = velocitySum / (float)FILTER_INTERVALS; return velocity; } - - - inline float stepRateOptimized(){ - int8_t old_SREG = SREG; - if (interrupts_in_use < 2) { - noInterrupts(); - update(&encoder); - } - else { - noInterrupts(); - } - - float static velocitySum = 0; - int static timeSum = 0; - //int static oldTicks = 0; - int static ticksInAverage = 0; - //int ticks = 0; - int bufferIndex = encoder.bufferIndex - 1; - if(bufferIndex < 0){ - bufferIndex+=MAX_BUFFER_SIZE; - } - int newTicks = encoder.newTicks; - encoder.newTicks = 0; - - SREG = old_SREG; - interrupts(); - - //Only do this if we have newTicks - if(newTicks > 0){ - //Add the all the newticks - int stopIndex = bufferIndex - newTicks; //We are going backwards, stop at the index where the newticks stop - if(stopIndex < 0){ - stopIndex += MAX_BUFFER_SIZE; - } - int i = bufferIndex; - //timeSum = 0; - //Serial.print("Newticks: "); - //Serial.print(newTicks); - //Serial.print(" "); - //Serial.println(); - //Serial.print("velocitysum before:"); - //Serial.println(velocitySum); - //Serial.print(" "); - for(int j = 0; j < newTicks; j++){ - timeSum += abs(encoder.uSBuffer[i]); //Add the time to the buffer - velocitySum += 1.0/(float)encoder.uSBuffer[i]; //Should catch this access to uSBuffer, also this is just adding the newvelocities to the sum this could lose precision on the float - //Serial.print("velocitysum:"); - //Serial.print(velocitySum); - //Serial.print(" "); - //Serial.print("added:"); - //Serial.println(1.0/(float)encoder.uSBuffer[i]); - /*if(abs(velocitySum) > 100){ - int temp = encoder.uSBuffer[i]; - //Serial.print(" "); - //delay(5000); - Serial.print("index:"); - Serial.print(i); - Serial.print(" "); - Serial.print("buffer value:"); - Serial.print(temp); - Serial.print(" "); - Serial.print("velocitysum:"); - Serial.print(velocitySum); - Serial.print(" "); - Serial.print("added:"); - Serial.print(1.0/temp); - Serial.print(" "); - delay(5000); - /*for(int i = 0; i < 100000; i++){ - Serial.println(temp); - delay(10); - }*/ - //} - //if(abs(velocitySum) > 10000000000){ - // velocitySum = 0; //Should catch this access to uSBuffer, also this is just adding the newvelocities to the sum this could lose precision on the float - //} - ticksInAverage++; //Increase the number of ticks - i--; - if(i <= stopIndex){ - break; - } - if(i < 0){ - i += MAX_BUFFER_SIZE; - } - } - //velocitySum = 0; - //Serial.print("velocitysum:"); - //Serial.print(velocitySum); - //Serial.print(" "); - /*if(abs(velocitySum) > 10000000000){ - Serial.print("BAD "); - }*/ - - - if(ticksInAverage > MAX_BUFFER_SIZE){ - ticksInAverage = MAX_BUFFER_SIZE; - } - - int startIndex = bufferIndex - ticksInAverage; - //Serial.print("start:"); - //Serial.print(startIndex); - //Serial.print(" "); - if(startIndex < 0){ - startIndex += MAX_BUFFER_SIZE; - } - /*Serial.print("start:"); - Serial.print(startIndex); - Serial.print(" "); - Serial.print(ticksInAverage); - Serial.print(" "); - //Serial.print(timeSum); - //Serial.print(" ");*/ - int loopcounts = 0; - int totalTicks = ticksInAverage; - for(int i = 0; i < totalTicks; i++){ - //for(int i = startIndex; i < bufferIndex; i = (i+1)%MAX_BUFFER_SIZE) { //Stop just before our current buffer index if we do get that far - loopcounts++; - if(timeSum < FILTER_TIME_LIMIT){ //This is our normal case. Stop subtracting when we are back within bounds - /*Serial.print("TIMELIMIT:"); - Serial.print(ticksInAverage); - Serial.print(" "); - Serial.print(timeSum); - Serial.print(" ");*/ - break; - } - - int index = bufferIndex-totalTicks+i; - if(index < 0) - index+=MAX_BUFFER_SIZE; - timeSum -= abs(encoder.uSBuffer[index]); //remove the time; - ticksInAverage--; //We have less ticks now - velocitySum -= 1.0/(float)encoder.uSBuffer[index]; //Again this access could be cached - } - /*Serial.print("loops:"); - Serial.print(loopcounts); - Serial.print(" "); - Serial.print(ticksInAverage); - Serial.print(" "); - Serial.print(timeSum); - Serial.print(" ");*/ - } - - float velocity = velocitySum / (float)(ticksInAverage/2.0); - Serial.print("ticksinAverage: "); - Serial.print(ticksInAverage); - Serial.print(" "); - //Serial.print("VELOCITY: "); - //Serial.println(velocity * 10000); - return velocity; - } - /*inline float extrapolate() { + + inline float extrapolate() { uint8_t old_SREG = SREG; if (interrupts_in_use < 2) { noInterrupts(); @@ -350,12 +201,12 @@ class Encoder else { noInterrupts(); } - float lastRate = encoder.rate; + float lastRate = stepRate(); int32_t lastPosition = encoder.position; - float extrapolatedPosition = encoder.stepTime; - float lastAccel = encoder.accel; + unsigned long timeSinceLastTick = micros() - encoder.timeOfLastTick; SREG = old_SREG; - extrapolatedPosition = lastRate * extrapolatedPosition + 0.5 * lastAccel * extrapolatedPosition * extrapolatedPosition; + + float extrapolatedPosition = lastRate * timeSinceLastTick; if (extrapolatedPosition > 1) { return (lastPosition + 1); } @@ -365,7 +216,7 @@ class Encoder else { return (extrapolatedPosition + lastPosition); } - }*/ + } #else inline int32_t read() { update(&encoder); @@ -559,7 +410,7 @@ class Encoder //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) + //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) @@ -582,7 +433,7 @@ class Encoder if(arg->bufferIndex >= MAX_BUFFER_SIZE){ arg->bufferIndex = 0; } - arg->newTicks++; + //arg->newTicks++; arg->position++; return; @@ -593,7 +444,7 @@ class Encoder if(arg->bufferIndex >= MAX_BUFFER_SIZE){ arg->bufferIndex = 0; } - arg->newTicks++; + //arg->newTicks++; arg->position++; return; @@ -604,7 +455,7 @@ class Encoder if(arg->bufferIndex >= MAX_BUFFER_SIZE){ arg->bufferIndex = 0; } - arg->newTicks++; + //arg->newTicks++; arg->position--; return; @@ -615,92 +466,29 @@ class Encoder if(arg->bufferIndex >= MAX_BUFFER_SIZE){ arg->bufferIndex = 0; } - arg->newTicks++; + //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 1: case 7: case 8: case 14: - arg->previousRate = arg->rate; // remember previous rate for calculating - if (arg->position % 2 == 0) { // if the previous position was even (0 to 1 step) - arg->rate1 = 0.5 / arg->stepTime; // the 0 to 1 step rate is set to rate1 - if (arg->lastRateTimer == 0) { // if the 0 to 1 step was the previous one calculated - arg->rate2 = 0; // then the rate2 step was skipped due to a direction change, so set it to zero - arg->previousRate = 0; // previous rate is also set to zero. there may be a better way but I have yet to think about it - } - arg->lastRateTimer = 0; // remember that rate1 was the last one calculated - } - else { // if the previous position was odd (step -1 to 0) - arg->rate2 = 0.5 / arg->stepTime; // the -1 to 0 step rate is set to rate2 - if (arg->lastRateTimer == 1) { // if the -1 to 0 step was the previous one calculated - arg->rate1 = 0; // then rate1 step was skipped due to direction change, so it is set to zero - arg->previousRate = 0; // previous rate is also set to zero. there may be a better way but I have yet to think about it - } - arg->lastRateTimer = 1; // remember that rate2 was the last one calculated - } - arg->rate = (arg->rate1 + arg->rate2); - arg->accel = (arg->rate - arg->previousRate) / arg->stepTime; - arg->stepTime = 0; - arg->position++; - return; - case 2: case 4: case 11: case 13: - arg->previousRate = arg->rate; - if (arg->position % 2 != 0) { // if the previous position was odd (1 to 0 step) - arg->rate1 = -0.5 / arg->stepTime; // the 1 to 0 step rate is set to rate1 - if (arg->lastRateTimer == 0) { - arg->rate2 = 0; - arg->previousRate = 0; - } - arg->lastRateTimer = 0; - } - else { // if the previous position was even - arg->rate2 = -0.5 / arg->stepTime; - if (arg->lastRateTimer == 1) { - arg->rate1 = 0; - arg->previousRate = 0; - } - arg->lastRateTimer = 1; - } - arg->rate = (arg->rate1 + arg->rate2); - arg->accel = (arg->rate - arg->previousRate) / arg->stepTime; - arg->stepTime = 0; - arg->position--; - return; - case 3: case 12: - arg->previousRate = arg->rate; - if (arg->position % 2 == 0) { - arg->rate1 = 1 / arg->stepTime; - arg->rate2 = arg->rate1; - arg->rate = arg->rate1; - } - else { - arg->rate2 = 1 / arg->stepTime; - arg->rate1 = arg->rate2; - arg->rate = arg->rate2; - } - arg->accel = (arg->rate - arg->previousRate) / arg->stepTime; - arg->stepTime = 0; + case 3: case 12: //+2 arg->position += 2; return; case 6: case 9: - arg->previousRate = arg->rate; - if (arg->position % 2 != 0) { - arg->rate1 = -1 / arg->stepTime; - arg->rate2 = arg->rate1; - arg->rate = arg->rate1; - } - else { - arg->rate2 = -1 / arg->stepTime; - arg->rate1 = arg->rate2; - arg->rate = arg->rate2; - } - arg->accel = (arg->rate - arg->previousRate) / arg->stepTime; - arg->stepTime = 0; + arg->position -= 2; return; */ + } #endif } From f2644e1fab5ce5856ced312600cae67f5cf272c3 Mon Sep 17 00:00:00 2001 From: aftersomemath Date: Thu, 31 Mar 2016 21:29:00 -0400 Subject: [PATCH 24/27] Fix SREG handling --- EncoderMod.h | 1 - 1 file changed, 1 deletion(-) diff --git a/EncoderMod.h b/EncoderMod.h index 31efb5a..99b067b 100644 --- a/EncoderMod.h +++ b/EncoderMod.h @@ -157,7 +157,6 @@ class Encoder unsigned long timeSinceLastTick = micros() - encoder.timeOfLastTick; SREG = old_SREG; - interrupts(); 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 From 4a9730dde186054c1f380b5fa314b21faf4ebfff Mon Sep 17 00:00:00 2001 From: aftersomemath Date: Thu, 31 Mar 2016 21:37:40 -0400 Subject: [PATCH 25/27] Remove unused variables --- EncoderMod.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/EncoderMod.h b/EncoderMod.h index 99b067b..d94b299 100644 --- a/EncoderMod.h +++ b/EncoderMod.h @@ -152,8 +152,6 @@ class Encoder if(index < 0){ index += MAX_BUFFER_SIZE; } - int timeSum = 0; - int ticks = 0; unsigned long timeSinceLastTick = micros() - encoder.timeOfLastTick; SREG = old_SREG; From 43af12a408e45bc4d4bceb66319e76a745d6d97a Mon Sep 17 00:00:00 2001 From: aftersomemath Date: Sat, 2 Apr 2016 13:46:07 -0400 Subject: [PATCH 26/27] Add automatically sized buffer for filter --- EncoderMod.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/EncoderMod.h b/EncoderMod.h index d94b299..ee3afcf 100644 --- a/EncoderMod.h +++ b/EncoderMod.h @@ -54,7 +54,7 @@ #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 5000 //This needs to be larger than the max number of ticks that can happen in our filter time limit it needs to be larger so that memory access violations don't occur +#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 @@ -166,6 +166,7 @@ class Encoder return 0.0; } + int ticksInFilter = 0; while(1){ //Get how many discrete intervals if(encoder.uSBuffer[index] == 0){ @@ -174,6 +175,7 @@ class Encoder } int intervals = abs(encoder.uSBuffer[index])/US_INTERVAL; sumIntervals += intervals; + ticksInFilter++; if(sumIntervals <= FILTER_INTERVALS){ velocitySum += intervals * (2.0/(float)encoder.uSBuffer[index]); } else { From a0df79cd2b7d7b739830b068ea5d0d8a11c50d69 Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Sat, 2 Apr 2016 15:15:09 -0400 Subject: [PATCH 27/27] Update EncoderMod.h Delete unneeded variable. --- EncoderMod.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/EncoderMod.h b/EncoderMod.h index ee3afcf..f124f4c 100644 --- a/EncoderMod.h +++ b/EncoderMod.h @@ -166,7 +166,6 @@ class Encoder return 0.0; } - int ticksInFilter = 0; while(1){ //Get how many discrete intervals if(encoder.uSBuffer[index] == 0){ @@ -175,7 +174,6 @@ class Encoder } int intervals = abs(encoder.uSBuffer[index])/US_INTERVAL; sumIntervals += intervals; - ticksInFilter++; if(sumIntervals <= FILTER_INTERVALS){ velocitySum += intervals * (2.0/(float)encoder.uSBuffer[index]); } else {