From 3a93b2a8c75850ccbc15c06b3e046bedf788877f Mon Sep 17 00:00:00 2001 From: dakkar Date: Fri, 29 Apr 2016 17:50:43 +0100 Subject: maybe nicer factoring and loop-niceness --- eeg.ino | 190 +++++++++++++++++++++++++++++++++++++--------------------------- 1 file changed, 109 insertions(+), 81 deletions(-) diff --git a/eeg.ino b/eeg.ino index ccdae1e..d129026 100644 --- a/eeg.ino +++ b/eeg.ino @@ -3,85 +3,101 @@ #include +template class Queue { + private: + E queue[Len]; + byte qHead,qTail; + public: + Queue() : qHead(0), qTail(0) { } + bool empty() { + return qHead == qTail; + } + bool full() { + return ((qHead+Len-1)%Len) == qTail; + } + E* current() { + return &queue[qHead]; + } + void remove() { + if (empty()) return; + qHead++; + qHead %= Len; + } + void add(const E &value) { + if (full()) return; + + queue[qTail] = value; + + qTail++; + qTail %= Len; + } +}; + #define MK_FREQ 49600L // Set clock to 50kHz (actualy 49.6kHz seems to work better) #define SOUND (byte)0x52 // Sound controller (device ID 82). Write to 0xA4, read from 0xA5. #define BUTTON (byte)0x50 // Button controller (device ID 80). Write to 0xA0, read from 0xA1. #define MOTOR (byte)0x55 // Motor controller (device ID 85). Write to 0xAA, read from 0xAB. class Keepon { + public: + enum State { WAITING=1, READY, QFULL }; + typedef void (*callback)(State); private: + static const int maxAttempts = 50; struct Command { byte device; byte command; byte arg; }; - static const byte qLen = 10; - static const byte maxAttempts = 50; - Command queue[qLen]; - byte qHead,qTail; + Queue queue; byte attempts; - bool queue_empty() { - return qHead == qTail; - } - bool queue_full() { - return ((qHead+qLen-1)%qLen) == qTail; - } - Command* queue_current() { - return &queue[qHead]; - } - void queue_remove() { - if (queue_empty()) return; - qHead++; - qHead %= qLen; - } - void queue_add(byte d,byte c, byte a) { - if (queue_full()) return; - - queue[qTail].device=d; - queue[qTail].command=c; - queue[qTail].arg=a; + callback display_callback; - qTail++; - qTail %= qLen; + void take_control() { + digitalWrite(SDA, LOW); + digitalWrite(SCL, LOW); + display_callback(WAITING); + while (analogRead(0) < 512); // Wait until we see voltage on A0 pin + display_callback(READY); + delay(1000); + Wire.begin(); + TWBR = ((F_CPU / MK_FREQ) - 16) / 2; } - public: - Keepon() { - qHead = 0; - qTail = 0; - attempts = 0; + + void check_connection() { + if (analogRead(0) > 512) return; + take_control(); } + public: + + Keepon(callback x) : attempts(0), display_callback(x) { } void begin() { pinMode(SDA, OUTPUT); // Data wire on My Keepon pinMode(SCL, OUTPUT); // Clock wire on My Keepon digitalWrite(SDA, LOW); digitalWrite(SCL, LOW); } - void bootup(void(*cb)(bool)) { - digitalWrite(SDA, LOW); - digitalWrite(SCL, LOW); - cb(false); - while (analogRead(0) < 512); // Wait until we see voltage on A0 pin - cb(true); - delay(1000); - Wire.begin(); - TWBR = ((F_CPU / MK_FREQ) - 16) / 2; + bool update() { + if (queue.empty()) return false; + return true; } void send() { + check_connection(); if (attempts > maxAttempts) { attempts = 0; - queue_remove(); + queue.remove(); } - if (queue_empty()) return; + if (queue.empty()) return; - Command* current = queue_current(); + Command* current = queue.current(); Wire.beginTransmission(current->device); Wire.write(current->command); Wire.write(current->arg); int result = (int)Wire.endTransmission(); if (result == 0) { - queue_remove(); + queue.remove(); attempts = 0; } else { @@ -89,30 +105,38 @@ class Keepon { } } + void add(const Command&c) { + if (queue.full()) { + display_callback(QFULL); + return; + } + queue.add(c); + } + void pan(byte pos) { - queue_add(MOTOR,4,pos); + add(Command{MOTOR,4,pos}); } void tilt(byte pos) { - queue_add(MOTOR,2,pos); + add(Command{MOTOR,2,pos}); } }; Brain brain(Serial); -Keepon keepon; -void kscan_callback(bool found) { - if (not found) { - uView.clear(PAGE); - uView.setCursor(0,0);uView.println("K?"); - uView.display(); +void kscan_callback(Keepon::State s) { + if (s == Keepon::WAITING) { + uView.setCursor(25,0);uView.println("K?"); } - else { - uView.clear(PAGE); - uView.setCursor(0,0);uView.println("K!"); - uView.display(); + else if (s == Keepon::READY){ + uView.setCursor(25,0);uView.println("K!"); + } + else if (s == Keepon::QFULL){ + uView.setCursor(25,0);uView.println("K+"); } } +Keepon keepon(&kscan_callback); + void setup() { Serial.begin(9600); @@ -121,7 +145,6 @@ void setup() { uView.setFontType(0); keepon.begin(); - keepon.bootup(&kscan_callback); } const int hist_width=3; @@ -130,39 +153,44 @@ int samples_delay; void loop() { byte attention, meditation, quality; - if (brain.update()) { + bool bu = brain.update(); bool ku = keepon.update(); + if (bu || ku) { uView.clear(PAGE); - uView.setCursor(0,0); uView.print(quality=brain.readSignalQuality()); + if (bu) { + uView.setCursor(0,0); uView.print(quality=brain.readSignalQuality()); - uView.setCursor(0,9); uView.print(attention=brain.readAttention()); - uView.setCursor(18,9); uView.print(meditation=brain.readMeditation()); + uView.setCursor(0,9); uView.print(attention=brain.readAttention()); + uView.setCursor(18,9); uView.print(meditation=brain.readMeditation()); - const uint32_t* power = brain.readPowerArray(); - uint32_t max_value=0; - for (int x=0;x<8;++x) { - if (power[x] > max_value) { - max_value = power[x]; + const uint32_t* power = brain.readPowerArray(); + uint32_t max_value=0; + for (int x=0;x<8;++x) { + if (power[x] > max_value) { + max_value = power[x]; + } } - } - for (int x=0;x<8;++x) { - int x0 = (hist_width + hist_pad) * x; - // we have 30 vertical pixels - int height = power[x] * 30 / max_value; - for (int o=0;o 5) { - keepon.pan(attention); - keepon.tilt(meditation); - samples_delay=0; - uView.setCursor(5,0);uView.print("sent"); + if (quality < 10 && ++samples_delay > 5) { + keepon.pan(attention); + keepon.tilt(meditation); + samples_delay=0; + + uView.setCursor(5,0);uView.print("sent"); + } + } + if (ku) { + keepon.send(); } - uView.display(); } - keepon.send(); } \ No newline at end of file -- cgit v1.2.3