From a798aba3a859c0d1c88ae5c5b32e64b186269b88 Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Sat, 5 Dec 2015 21:56:35 -0500 Subject: [PATCH 1/3] 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 2/3] 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 42d4a25f4724ed277fa92b6645c9a7b95886b051 Mon Sep 17 00:00:00 2001 From: Quentin Torgerson Date: Thu, 17 Dec 2015 20:31:14 -0500 Subject: [PATCH 3/3] 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;