123
Fork of LG by
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