Diff: moppy.cpp

Revision:

0:84c336a81482

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/moppy.cpp	Wed May 24 11:58:43 2017 +0000
@@ -0,0 +1,100 @@
+#include "mbed.h"
+#include "moppy.h"
+
+#define SILENCE_ALL     250
+#define RESET_ALL       251
+#define RESET_COMPLETE  252
+#define ERROR_STATE     253
+#define RESERVED        254
+#define SYNC            255
+
+void Moppy::consumeAndCheck(int event) {
+    while(expectedSyncs-->0) {
+        if(str_buffer[expectedSyncs]!=0)
+            error("Moppy Flush Error");
+    }
+}
+
+Moppy::Moppy(PinName tx, PinName rx, int baud) : serial(tx,rx,baud) {
+    
+    wait_ms(50);
+    
+    //Synchronise
+    serial.putc(SYNC);
+    if(serial.getc()!=SYNC)
+        error("Moppy Initialisation Error");
+    
+    //Reset all
+    serial.putc(RESET_ALL);
+    if(serial.getc()!=RESET_ALL)
+        error("Failed to reset all drives");
+    serial.putc(RESET_COMPLETE);
+    if(serial.getc()!=RESET_COMPLETE)
+        error("Failed to reset all drives");
+    
+    //Count
+    serial.putc(249);
+    driveCount = (249-serial.getc())*2;
+    serial.putc(SYNC);
+    if(serial.getc()!=SYNC)
+        error("Moppy Initialisation Error");
+    
+    str_buffer = (char*)calloc(driveCount/2, sizeof(uint8_t)*6);
+    
+    freqs = (uint16_t*)calloc(driveCount, sizeof(uint16_t));
+    
+    pair_changed = (bool*)calloc(driveCount/2, sizeof(bool));
+    
+    expectedSyncs = 0;
+    read_callback.attach(this, &Moppy::consumeAndCheck);
+    
+}
+
+void Moppy::setFrequency(int drive, int frequency) {
+    
+    freqs[drive] = frequency;
+    pair_changed[drive>>1] = true;
+    
+}
+
+void Moppy::flush() {
+    
+    //build message
+    int i=driveCount>>1;
+    int j=0;
+    while(i-->0) {
+        if(pair_changed[i]) {
+            int freq1 = freqs[i<<1];
+            int freq2 = freqs[(i<<1)+1];
+            
+            str_buffer[j++] = i;
+            str_buffer[j++] = freq1/250;
+            str_buffer[j++] = freq1%250;
+            str_buffer[j++] = freq2/250;
+            str_buffer[j++] = freq2%250;
+            str_buffer[j++] = SYNC;
+            
+            pair_changed[i] = false; //reset changed flag
+        }
+    }
+    
+    //send message asynchronously
+    if(j>0) {
+        
+        for(int i=0; i<j; i++) {
+            serial.putc(str_buffer[i]);
+            if(str_buffer[i]==SYNC)
+                serial.getc();
+        }
+        
+//        serial.write((uint8_t*)str_buffer, j, NULL);
+//        expectedSyncs = j/6;
+//        serial.read((uint8_t*)str_buffer, expectedSyncs, read_callback);
+    }
+    
+}
+
+bool Moppy::silence() {
+    serial.putc(250);
+    return serial.getc()==250;
+}
\ No newline at end of file