Diff: DeviceCounters.c

Revision:

156:e68ee0bcdcda

Child:

167:bedc0a9d559a

diff -r 94e7ac5fea58 -r e68ee0bcdcda DeviceCounters.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DeviceCounters.c	Mon May 09 20:03:26 2016 +0000
@@ -0,0 +1,34 @@
+#include "Device.h"
+extern Device device;
+
+void InitCountersWithDefaults(void){
+  for (uint8_t i = 0; i < 32; i++) device.counters.state.delta[i] = 0;
+  device.counters.state.angle = 0;
+  device.counters.state.amplitude = 0;
+  device.counters.state.frequency = 0;
+}
+
+void InitCounters(void){
+}
+
+void DeviceCount(void){
+  int32_t delta = device.controller.QEI.state.delta;
+  
+  device.counters.state.angle -= device.counters.state.delta[(device.measurement.counter-31) & 0xff];
+  device.counters.state.angle += delta;
+  
+  device.counters.state.displacement += device.counters.state.delta[(device.measurement.counter-31) & 0xff];
+  device.counters.state.displacement -= device.counters.state.delta[(device.measurement.counter-16) & 0xff];
+  device.counters.state.displacement -= device.counters.state.delta[(device.measurement.counter-15) & 0xff];
+  device.counters.state.displacement += delta;
+  
+  device.counters.state.delta[device.measurement.counter] = delta;
+  
+  if (device.counters.state.displacement > 0) device.counters.state.amplitude += device.counters.state.displacement;
+  if (device.counters.state.displacement < 0) device.counters.state.amplitude -= device.counters.state.displacement;
+  
+  if (delta > 0) device.counters.state.frequency += delta;
+  if (delta < 0) device.counters.state.frequency -= delta;
+  
+  device.counters.state.accumulator += device.counters.state.angle;
+}
\ No newline at end of file