Merged to branch
Dependencies: USBDevice mbed EquatorStrutController LightWeightSerialTransmit
Fork of EquatorStrutDigitalMonitor by
Diff: main.cpp
- Revision:
- 11:b6958b3dbddf
- Parent:
- 9:ee84a9697de0
- Parent:
- 10:088eeae4287c
- Child:
- 12:814db1249a19
--- a/main.cpp Mon Aug 11 13:48:04 2014 +0000 +++ b/main.cpp Tue Aug 12 06:47:01 2014 +0000 @@ -25,8 +25,33 @@ int interruptPeriod = 0; int lastTime = 0; +int intteruptPeriodArray[15]; +int arrayTotal = 0; +char arrayPos = 0; + char counter = 0; +void SmoothingAdd(int input) +{ + arrayTotal -= intteruptPeriodArray[arrayPos]; + arrayTotal += input; + intteruptPeriodArray[arrayPos] = input; + + if (arrayPos == 14) + { + arrayPos = 0; + } + else + { + arrayTotal++; + } +} + +int SmoothedInterruptPeriod() +{ + return arrayTotal / 15; +} + void RGHSinHandler() { if (PinState == 2) @@ -41,7 +66,7 @@ { direction = 1; position += 0.04 * direction; - interruptPeriod = RunningTime.read_us() - lastTime; + SmoothingAdd(RunningTime.read_us() - lastTime); lastTime = RunningTime.read_us(); } } @@ -65,7 +90,7 @@ { direction = -1; position += 0.04 * direction; - interruptPeriod = RunningTime.read_us() - lastTime; + SmoothingAdd(RunningTime.read_us() - lastTime); lastTime = RunningTime.read_us(); } } @@ -151,7 +176,7 @@ { return 0.0; } - return (direction * 0.04)/((double)interruptPeriod / 1000000.0); + return (direction * 0.04)/((double)SmoothedInterruptPeriod() / 1000000.0); } double Error = 0;