igor Apu / Mbed 2 deprecated LG Featured

123

Dependencies:   mbed

Fork of LG by igor Apu

DeviceCounters.c@174:0f86eedd511c, 2016-06-26 (annotated)

Committer:
Diletant
Date:
Sun Jun 26 10:16:39 2016 +0000
Revision:
174:0f86eedd511c
Parent:
173:7f938afb0447
Child:
177:672ef279c8e0
Device&... update. Not final!!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Diletant 156:e68ee0bcdcda 1 #include "Device.h"
Diletant 156:e68ee0bcdcda 2 extern Device device;
Diletant 156:e68ee0bcdcda 3
Diletant 167:bedc0a9d559a 4 void InitCountersDefaultSettings(void){
Diletant 173:7f938afb0447 5 }
Diletant 173:7f938afb0447 6
Diletant 173:7f938afb0447 7 void InitCountersState(void){
Diletant 174:0f86eedd511c 8 for (uint8_t i = 0; i < 32; i++) device.counters.dither.state.delta[i] = 0;
Diletant 174:0f86eedd511c 9 device.counters.dither.state.angle = 0;
Diletant 174:0f86eedd511c 10 device.counters.dither.state.displacement = 0;
Diletant 174:0f86eedd511c 11 device.counters.dither.state.amplitude = 0;
Diletant 174:0f86eedd511c 12 device.counters.dither.state.frequency = 0;
Diletant 174:0f86eedd511c 13 device.counters.dither.state.accumulator = 0;
Diletant 156:e68ee0bcdcda 14 }
Diletant 156:e68ee0bcdcda 15
Diletant 173:7f938afb0447 16 void DeviceStartCounters(void){
Diletant 173:7f938afb0447 17 DeviceStartQEI();
Diletant 167:bedc0a9d559a 18 }
Diletant 167:bedc0a9d559a 19
Diletant 173:7f938afb0447 20 void countersProcess(void){
Diletant 174:0f86eedd511c 21 int32_t income = device.controller.QEI.state.delta;
Diletant 174:0f86eedd511c 22 int32_t outcome = device.counters.dither.state.delta[(device.measurement.counter-31) & 0xff];
Diletant 174:0f86eedd511c 23 device.counters.dither.state.delta[device.measurement.counter] = income;
Diletant 156:e68ee0bcdcda 24
Diletant 174:0f86eedd511c 25 //Angle, accumulated for measurement cycle
Diletant 174:0f86eedd511c 26 // Subtract outcoming pulses
Diletant 174:0f86eedd511c 27 device.counters.dither.state.angle -= outcome;
Diletant 174:0f86eedd511c 28 // Add incoming pulses
Diletant 174:0f86eedd511c 29 device.counters.dither.state.angle += income;
Diletant 174:0f86eedd511c 30
Diletant 174:0f86eedd511c 31 //Restored meander counters - low accuracy
Diletant 174:0f86eedd511c 32 //if (income > 0) a += income
Diletant 174:0f86eedd511c 33 //if (income < 0) b += income
Diletant 156:e68ee0bcdcda 34
Diletant 174:0f86eedd511c 35 //Displacement (restored sine angle)
Diletant 174:0f86eedd511c 36 // Subtract outcoming half wave
Diletant 174:0f86eedd511c 37 // Add outcoming half wave end
Diletant 174:0f86eedd511c 38 device.counters.dither.state.displacement += outcome;
Diletant 174:0f86eedd511c 39 // Subtract outcoming half wave start
Diletant 174:0f86eedd511c 40 device.counters.dither.state.displacement -= device.counters.dither.state.delta[(device.measurement.counter-16) & 0xff];
Diletant 174:0f86eedd511c 41 // Add incoming half wave
Diletant 174:0f86eedd511c 42 // Subtract incoming half wave end
Diletant 174:0f86eedd511c 43 device.counters.dither.state.displacement -= device.counters.dither.state.delta[(device.measurement.counter-15) & 0xff];
Diletant 174:0f86eedd511c 44 // Add incoming half wave start
Diletant 174:0f86eedd511c 45 device.counters.dither.state.displacement += income;
Diletant 156:e68ee0bcdcda 46
Diletant 174:0f86eedd511c 47 //Virtual zero sensor - low accuracy
Diletant 174:0f86eedd511c 48 //if (device.counters.state.displacement > 0) a += income
Diletant 174:0f86eedd511c 49 //if (device.counters.state.displacement < 0) b += income
Diletant 174:0f86eedd511c 50
Diletant 174:0f86eedd511c 51 //Amplitude
Diletant 174:0f86eedd511c 52 if (device.counters.dither.state.displacement > 0) device.counters.dither.state.amplitude += device.counters.dither.state.displacement;
Diletant 174:0f86eedd511c 53 if (device.counters.dither.state.displacement < 0) device.counters.dither.state.amplitude -= device.counters.dither.state.displacement;
Diletant 156:e68ee0bcdcda 54
Diletant 174:0f86eedd511c 55 //Output frequency
Diletant 174:0f86eedd511c 56 // Subtract outcoming pulses
Diletant 174:0f86eedd511c 57 if (outcome > 0) device.counters.dither.state.frequency -= outcome;
Diletant 174:0f86eedd511c 58 if (outcome < 0) device.counters.dither.state.frequency += outcome;
Diletant 174:0f86eedd511c 59 // Add incoming pulses
Diletant 174:0f86eedd511c 60 if (income > 0) device.counters.dither.state.frequency += income;
Diletant 174:0f86eedd511c 61 if (income < 0) device.counters.dither.state.frequency -= income;
Diletant 174:0f86eedd511c 62
Diletant 174:0f86eedd511c 63 device.counters.dither.state.accumulator += device.counters.dither.state.angle;
Diletant 156:e68ee0bcdcda 64 }