From 9ec47e2d17f53d3142236f70cfeeb68d2c1526b3 Mon Sep 17 00:00:00 2001 From: aftersomemath Date: Tue, 29 Mar 2016 20:51:12 -0400 Subject: [PATCH 1/5] 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 2/5] 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 3/5] 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 4/5] 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 5/5] 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;