1#include <stdio.h>
2#include <stdlib.h>
3#include <string.h>
4
5#define REGION_KICK 0
6#define REGION_TOP 0
7#define REGION_SQUEEZE 0
8#define REGION_FLUFF 0
9#define REGION_PICK 0
10#define REGION_SLIDE 1
11
12static inline bool sign(unsigned long a, unsigned long b) { return a < b; }
13// Returns whether x is outside of the range [a, b), which may wrap around the maximum value
14static inline bool outside(unsigned long a, unsigned long b, unsigned long x) {
15 return sign(x, a) ^ sign(x, b) ^ sign(a, b);
16}
17template <typename T> inline void swap(T &a, T &b) { T t = a; a = b; b = t; }
18inline int cmp_long(const void *a, const void *b) { return (int)(*(long *)a - *(long *)b); }
19
20struct FungiCapSensor {
21 long base;
22
23 volatile IO_REG_TYPE *sReg;
24 IO_REG_TYPE sBit;
25 volatile IO_REG_TYPE *rReg;
26 IO_REG_TYPE rBit;
27
28 static const long SENSOR_BASE_UNINIT_TAG = 0x3FFFFFF0;
29 static const long SENSOR_TIMEOUT = 0x3FFFFFFF;
30
31 static const int N_SAMPLES_PER_OUTPUT = 11;
32 static const int N_HIST = 3;
33 static const int N_SAMPLES_TOTAL = N_HIST * N_SAMPLES_PER_OUTPUT;
34 long histSamples[N_SAMPLES_TOTAL];
35
36 FungiCapSensor(uint8_t sendPin, uint8_t recvPin) {
37 pinMode(sendPin, OUTPUT);
38 pinMode(recvPin, INPUT);
39 base = SENSOR_BASE_UNINIT_TAG;
40
41 sReg = PIN_TO_BASEREG(sendPin);
42 sBit = PIN_TO_BITMASK(sendPin);
43 rReg = PIN_TO_BASEREG(recvPin);
44 rBit = PIN_TO_BITMASK(recvPin);
45 }
46
47 inline long sampleRaw() {
48 unsigned long t0, tm, t1, t2;
49
50 noInterrupts();
51 DIRECT_WRITE_LOW(sReg, sBit);
52 DIRECT_MODE_INPUT(rReg, rBit);
53 DIRECT_MODE_OUTPUT(rReg, rBit);
54 DIRECT_WRITE_LOW(rReg, rBit);
55 delayMicroseconds(10);
56 DIRECT_MODE_INPUT(rReg, rBit);
57 t0 = micros();
58 tm = t0 + 2000000; // May overflow and wrap around
59 DIRECT_WRITE_HIGH(sReg, sBit);
60 interrupts();
61 while (!DIRECT_READ(rReg, rBit)) {
62 unsigned long T = micros();
63 if (outside(t0, tm, T)) return SENSOR_TIMEOUT;
64 }
65 t1 = micros() - t0;
66
67 noInterrupts();
68 DIRECT_WRITE_HIGH(rReg, rBit);
69 DIRECT_MODE_OUTPUT(rReg, rBit);
70 DIRECT_WRITE_HIGH(rReg, rBit);
71 DIRECT_MODE_INPUT(rReg, rBit);
72 t0 = micros();
73 tm = t0 + 2000000;
74 DIRECT_WRITE_LOW(sReg, sBit);
75 interrupts();
76 while (DIRECT_READ(rReg, rBit)) {
77 unsigned long T = micros();
78 if (outside(t0, tm, T)) return SENSOR_TIMEOUT;
79 }
80 t2 = micros() - t0;
81
82 return (t1 + t2) / 4;
83 }
84
85 inline void populateCalibration() {
86 for (int i = 0; i < N_SAMPLES_TOTAL; i++) histSamples[i] = sampleRaw();
87 }
88
89 inline long sampleCalibrated() {
90 for (int i = 0; i < N_SAMPLES_TOTAL; i++)
91 if (i + N_SAMPLES_PER_OUTPUT < N_SAMPLES_TOTAL)
92 histSamples[i] = histSamples[i + N_SAMPLES_PER_OUTPUT];
93 else
94 histSamples[i] = sampleRaw();
95
96 long a[N_SAMPLES_TOTAL];
97 memcpy(a, histSamples, sizeof a);
98 qsort(a, N_SAMPLES_TOTAL, sizeof(long), cmp_long);
99 long result = 0;
100 const int I_START = N_SAMPLES_TOTAL / 2 - 5;
101 const int I_END = N_SAMPLES_TOTAL / 2 + 5;
102 for (int i = I_START; i < I_END; i++)
103 if ((result += a[i]) >= SENSOR_TIMEOUT) result = SENSOR_TIMEOUT;
104 // Normalize
105 if (result < SENSOR_TIMEOUT) result = result * 10 / (I_END - I_START);
106
107 long delta = base - result;
108 if (delta >= 0) {
109 if (delta >= 1 && base != SENSOR_BASE_UNINIT_TAG) delta = 1;
110 base -= delta;
111 }
112 return result - base;
113 }
114};
115
116FungiCapSensor sensor[] = {
117#if REGION_SQUEEZE || REGION_FLUFF || REGION_PICK || REGION_SLIDE
118 FungiCapSensor(12, 2),
119#endif
120#if REGION_SQUEEZE || REGION_FLUFF || REGION_PICK || REGION_SLIDE || REGION_TOP
121 FungiCapSensor(12, 3),
122 FungiCapSensor(12, 4),
123#endif
124 FungiCapSensor(12, 5),
125 FungiCapSensor(12, 6),
126 FungiCapSensor(12, 7),
127 FungiCapSensor(12, 8),
128 FungiCapSensor(12, 9),
129 FungiCapSensor(12, 10),
130 FungiCapSensor(12, 11),
131};
132const int N_SENSORS = sizeof sensor / sizeof sensor[0];
133unsigned long lastBaseInc;
134
135template <size_t N, size_t A, size_t B, long ThrAbsl, int ThrRel, int Interval> struct SensorHistoryTrigger {
136 long history[N];
137 size_t ptr;
138 int wait;
139 SensorHistoryTrigger() {
140 ptr = 0;
141 wait = N - 1;
142 }
143 inline int shift(long value) {
144 history[ptr] = value;
145 if (++ptr == N) ptr = 0;
146 if (wait == 0) {
147 bool result = false;
148 // Overall trend
149 int upCount = 0;
150 for (int i = 0; i < N - 1; i++) if (history[i] < history[i + 1]) upCount++;
151 if (upCount > N / 2) {
152 long a[N];
153 memcpy(a, history, sizeof a);
154 qsort(a, N, sizeof(long), cmp_long);
155 result = (a[B] - a[A] >= ThrAbsl && a[B] - a[A] >= a[N - 1] * ThrRel / 16);
156 }
157 if (result) wait = Interval;
158 return (int)result;
159 } else {
160 wait--;
161 return 0;
162 }
163 }
164};
165
166template <long ThrAbslFirst, long ThrAbsl, int Capacity, int CountThr> struct SensorContinuousTrigger {
167 int count;
168 bool state;
169 SensorContinuousTrigger() {
170 count = 0;
171 state = false;
172 }
173 inline int shift(long value) {
174 if (value >= (state ? ThrAbsl : ThrAbslFirst)) {
175 if (count < Capacity) count++;
176 } else {
177 if (count > 0) count--;
178 }
179 bool last = state;
180 state = (count >= CountThr);
181 if (last ^ state) {
182 if (state) count = Capacity;
183 return state ? +1 : -1;
184 } else {
185 return 0;
186 }
187 }
188};
189
190#if REGION_KICK
191#define TRIGGER_INSTANT
192SensorHistoryTrigger<5, 1, 3, 20, 8, 30> triggers[N_SENSORS];
193#endif
194
195#if REGION_TOP
196#define TRIGGER_CONT
197SensorContinuousTrigger<100, 80, 1, 1> triggers[N_SENSORS];
198#endif
199
200#if REGION_SQUEEZE
201#define TRIGGER_CONT
202SensorContinuousTrigger<180, 100, 2, 2> triggers[N_SENSORS];
203#endif
204
205#if REGION_PICK
206#define TRIGGER_CONT
207SensorContinuousTrigger<40, 20, 20, 5> triggers[N_SENSORS];
208#endif
209
210#if REGION_SLIDE
211#define TRIGGER_CONT
212SensorContinuousTrigger<30, 20, 5, 2> triggers[N_SENSORS];
213#endif
214
215void setup() {
216 Serial.begin(9600);
217
218 TCCR1A = (1 << WGM10);
219 TCCR1B = (0 << CS12) | (0 << CS11) | (1 << CS10);
220 TIMSK1 |= (1 << TOIE1);
221 pinMode(13, OUTPUT);
222
223 for (int i = 0; i < N_SENSORS; i++) sensor[i].populateCalibration();
224 lastBaseInc = millis();
225}
226
227ISR(TIMER1_OVF_vect) {
228 static int timerCount = 0, pinStatus = 0;
229 if ((timerCount = ((timerCount + 1) & 32767)) == 0)
230 digitalWrite(13, (pinStatus ^= 1));
231}
232
233void loop() {
234 if (outside(lastBaseInc, lastBaseInc + 1000, millis())) {
235 for (int i = 0; i < N_SENSORS; i++) sensor[i].base++;
236 lastBaseInc = millis();
237 }
238
239 long result[N_SENSORS];
240 for (int i = 0; i < N_SENSORS; i++) {
241 result[i] = sensor[i].sampleCalibrated();
242 if (result[i] >= FungiCapSensor::SENSOR_TIMEOUT) continue;
243 #if defined(TRIGGER_INSTANT) || defined(TRIGGER_CONT)
244 int change = triggers[i].shift(result[i]);
245 if (change) {
246 Serial.print(change == +1 ? "Onset " : "Release ");
247 Serial.print(i);
248 Serial.print(" @ ");
249 Serial.print(millis());
250 Serial.println();
251 }
252 #endif
253 }
254
255#define INSPECT_SENSORS 1
256#if INSPECT_SENSORS
257 static int count = 0;
258 // if (++count == 16) count = 0; else return;
259 Serial.print("\t");
260 char line[N_SENSORS * 7 + 1];
261 for (int i = 0; i < N_SENSORS; i++)
262 sprintf(line + i * 7, "%6ld\t", result[i]);
263 Serial.print(line);
264 Serial.print("\t|\t");
265 for (int i = 0; i < N_SENSORS; i++)
266 sprintf(line + i * 7, "%6ld\t", sensor[i].base);
267 Serial.println(line);
268#endif
269}
