From cab71efbfa11af1e8e062b223361d89499e68608 Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Sat, 5 Dec 2015 01:19:51 -0500 Subject: [PATCH 1/4] 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 2/4] 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 3/4] 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 4/4] 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);