123
Fork of LG by
DeviceCounters.c
- Committer:
- Diletant
- Date:
- 2016-06-26
- Revision:
- 174:0f86eedd511c
- Parent:
- 173:7f938afb0447
- Child:
- 177:672ef279c8e0
File content as of revision 174:0f86eedd511c:
#include "Device.h" extern Device device; void InitCountersDefaultSettings(void){ } void InitCountersState(void){ for (uint8_t i = 0; i < 32; i++) device.counters.dither.state.delta[i] = 0; device.counters.dither.state.angle = 0; device.counters.dither.state.displacement = 0; device.counters.dither.state.amplitude = 0; device.counters.dither.state.frequency = 0; device.counters.dither.state.accumulator = 0; } void DeviceStartCounters(void){ DeviceStartQEI(); } void countersProcess(void){ int32_t income = device.controller.QEI.state.delta; int32_t outcome = device.counters.dither.state.delta[(device.measurement.counter-31) & 0xff]; device.counters.dither.state.delta[device.measurement.counter] = income; //Angle, accumulated for measurement cycle // Subtract outcoming pulses device.counters.dither.state.angle -= outcome; // Add incoming pulses device.counters.dither.state.angle += income; //Restored meander counters - low accuracy //if (income > 0) a += income //if (income < 0) b += income //Displacement (restored sine angle) // Subtract outcoming half wave // Add outcoming half wave end device.counters.dither.state.displacement += outcome; // Subtract outcoming half wave start device.counters.dither.state.displacement -= device.counters.dither.state.delta[(device.measurement.counter-16) & 0xff]; // Add incoming half wave // Subtract incoming half wave end device.counters.dither.state.displacement -= device.counters.dither.state.delta[(device.measurement.counter-15) & 0xff]; // Add incoming half wave start device.counters.dither.state.displacement += income; //Virtual zero sensor - low accuracy //if (device.counters.state.displacement > 0) a += income //if (device.counters.state.displacement < 0) b += income //Amplitude if (device.counters.dither.state.displacement > 0) device.counters.dither.state.amplitude += device.counters.dither.state.displacement; if (device.counters.dither.state.displacement < 0) device.counters.dither.state.amplitude -= device.counters.dither.state.displacement; //Output frequency // Subtract outcoming pulses if (outcome > 0) device.counters.dither.state.frequency -= outcome; if (outcome < 0) device.counters.dither.state.frequency += outcome; // Add incoming pulses if (income > 0) device.counters.dither.state.frequency += income; if (income < 0) device.counters.dither.state.frequency -= income; device.counters.dither.state.accumulator += device.counters.dither.state.angle; }