1 files changed, 453 insertions, 0 deletions
diff --git a/libraries/Firmata/Firmata.cpp b/libraries/Firmata/Firmata.cpp
new file mode 100644
index 0000000..f2688b9
--- /dev/null
+++ b/libraries/Firmata/Firmata.cpp
@@ -0,0 +1,453 @@
+/*
+  Firmata.cpp - Firmata library
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+ 
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+*/
+
+/*
+ * TODO generalized SysEx support
+ * TODO firmware name/version reporting (i.e. some firmwares will use the Firmata
+ *       protocol, but will only support specific devices, like ultrasound 
+ *       rangefinders or servos)
+ * TODO implement v2 protocol using digital ports
+ */
+
+//******************************************************************************
+//* Includes
+//******************************************************************************
+
+#include "WProgram.h"
+#include "HardwareSerial.h"
+#include "Firmata.h"
+
+extern "C" {
+#include <string.h>
+#include <stdlib.h>
+}
+
+//******************************************************************************
+//* Support Functions
+//******************************************************************************
+
+void sendValueAsTwo7bitBytes(int value)
+{
+    Serial.print(value & B01111111, BYTE); // LSB
+    Serial.print(value >> 7 & B01111111, BYTE); // MSB
+}
+
+void startSysex(void)
+{
+    Serial.print(START_SYSEX, BYTE);
+}
+
+void endSysex(void)
+{
+    Serial.print(END_SYSEX, BYTE);
+}
+
+//******************************************************************************
+//* Constructors
+//******************************************************************************
+
+FirmataClass::FirmataClass(void)
+{
+    firmwareVersionCount = 0;
+    systemReset();
+}
+
+//******************************************************************************
+//* Public Methods
+//******************************************************************************
+
+/* begin method for overriding default serial bitrate */
+void FirmataClass::begin(void)
+{
+    Serial.begin(115200);
+    blinkVersion();
+    delay(300);
+    printVersion();
+}
+
+/* begin method for overriding default serial bitrate */
+void FirmataClass::begin(long speed)
+{
+    blinkVersion();
+#if defined(__AVR_ATmega128__)  // Wiring
+    Serial.begin((uint32_t)speed);
+#else
+    Serial.begin(speed);
+#endif
+    delay(300);
+    printVersion();
+    printFirmwareVersion();
+}
+
+// output the protocol version message to the serial port
+void FirmataClass::printVersion(void) {
+    Serial.print(REPORT_VERSION, BYTE);
+    Serial.print(FIRMATA_MAJOR_VERSION, BYTE);
+    Serial.print(FIRMATA_MINOR_VERSION, BYTE);
+}
+
+void FirmataClass::blinkVersion(void)
+{
+    // flash the pin with the protocol version
+    pinMode(VERSION_BLINK_PIN,OUTPUT);
+    pin13strobe(FIRMATA_MAJOR_VERSION, 200, 400);
+    delay(300);
+    pin13strobe(2,1,4); // separator, a quick burst
+    delay(300);
+    pin13strobe(FIRMATA_MINOR_VERSION, 200, 400);
+}
+
+void FirmataClass::printFirmwareVersion(void)
+{
+    byte i;
+
+    if(firmwareVersionCount) { // make sure that the name has been set before reporting
+        startSysex();
+        Serial.print(REPORT_FIRMWARE, BYTE);
+        Serial.print(firmwareVersionVector[0]); // major version number
+        Serial.print(firmwareVersionVector[1]); // minor version number
+        for(i=2; i<firmwareVersionCount; ++i) {
+            sendValueAsTwo7bitBytes(firmwareVersionVector[i]);
+        }
+        endSysex();
+    }
+}
+
+void FirmataClass::setFirmwareNameAndVersion(const char *name, byte major, byte minor)
+{
+    const char *filename;
+    char *extension;
+
+// parse out ".cpp" and "applet/" that comes from using __FILE__
+    extension = strstr(name, ".cpp");
+    filename = strrchr(name, '/') + 1; //points to slash, +1 gets to start of filename
+    // add two bytes for version numbers
+    if(extension && filename) {
+        firmwareVersionCount = extension - filename + 2;
+    } else {
+        firmwareVersionCount = strlen(name) + 2;
+        filename = name;
+    }
+    firmwareVersionVector = (byte *) malloc(firmwareVersionCount);
+    firmwareVersionVector[firmwareVersionCount] = 0;
+    firmwareVersionVector[0] = major;
+    firmwareVersionVector[1] = minor;
+    strncpy((char*)firmwareVersionVector + 2, filename, firmwareVersionCount - 2);
+// alas, no snprintf on Arduino
+//    snprintf(firmwareVersionVector, MAX_DATA_BYTES, "%c%c%s", 
+//             (char)major, (char)minor, firmwareVersionVector);
+}
+
+//------------------------------------------------------------------------------
+// Serial Receive Handling
+
+int FirmataClass::available(void)
+{
+    return Serial.available();
+}
+
+
+void FirmataClass::processSysexMessage(void)
+{
+    switch(storedInputData[0]) { //first byte in buffer is command
+    case REPORT_FIRMWARE:
+        printFirmwareVersion();
+        break;
+    case FIRMATA_STRING:
+        if(currentStringCallback) {
+            byte bufferLength = (sysexBytesRead - 1) / 2;
+            char *buffer = (char*)malloc(bufferLength * sizeof(char));
+            byte i = 1;
+            byte j = 0;
+            while(j < bufferLength) {
+                buffer[j] = (char)storedInputData[i];
+                i++;
+                buffer[j] += (char)(storedInputData[i] << 7);
+                i++;
+                j++;
+            }
+            (*currentStringCallback)(buffer);
+        }
+        break;
+    default:
+        if(currentSysexCallback)
+        (*currentSysexCallback)(storedInputData[0], sysexBytesRead - 1, storedInputData + 1);
+    }
+}
+
+void FirmataClass::processInput(void)
+{
+    int inputData = Serial.read(); // this is 'int' to handle -1 when no data
+    int command;
+    
+    // TODO make sure it handles -1 properly
+
+    if (parsingSysex) {
+        if(inputData == END_SYSEX) {
+            //stop sysex byte      
+            parsingSysex = false;
+            //fire off handler function
+            processSysexMessage();
+        } else {
+            //normal data byte - add to buffer
+            storedInputData[sysexBytesRead] = inputData;
+            sysexBytesRead++;
+        }
+    } else if( (waitForData > 0) && (inputData < 128) ) {  
+        waitForData--;
+        storedInputData[waitForData] = inputData;
+        if( (waitForData==0) && executeMultiByteCommand ) { // got the whole message
+            switch(executeMultiByteCommand) {
+            case ANALOG_MESSAGE:
+                if(currentAnalogCallback) {
+                    (*currentAnalogCallback)(multiByteChannel,
+                                             (storedInputData[0] << 7)
+                                             + storedInputData[1]);
+                }
+                break;
+            case DIGITAL_MESSAGE:
+                if(currentDigitalCallback) {
+                    (*currentDigitalCallback)(multiByteChannel,
+                                              (storedInputData[0] << 7)
+                                              + storedInputData[1]);
+                }
+                break;
+            case SET_PIN_MODE:
+                if(currentPinModeCallback)
+                    (*currentPinModeCallback)(storedInputData[1], storedInputData[0]);
+                break;
+            case REPORT_ANALOG:
+                if(currentReportAnalogCallback)
+                    (*currentReportAnalogCallback)(multiByteChannel,storedInputData[0]);
+                break;
+            case REPORT_DIGITAL:
+                if(currentReportDigitalCallback)
+                    (*currentReportDigitalCallback)(multiByteChannel,storedInputData[0]);
+                break;
+            }
+            executeMultiByteCommand = 0;
+        }	
+    } else {
+        // remove channel info from command byte if less than 0xF0
+        if(inputData < 0xF0) {
+            command = inputData & 0xF0;
+            multiByteChannel = inputData & 0x0F;
+        } else {
+            command = inputData;
+            // commands in the 0xF* range don't use channel data
+        }
+        switch (command) {
+        case ANALOG_MESSAGE:
+        case DIGITAL_MESSAGE:
+        case SET_PIN_MODE:
+            waitForData = 2; // two data bytes needed
+            executeMultiByteCommand = command;
+            break;
+        case REPORT_ANALOG:
+        case REPORT_DIGITAL:
+            waitForData = 1; // two data bytes needed
+            executeMultiByteCommand = command;
+            break;
+        case START_SYSEX:
+            parsingSysex = true;
+            sysexBytesRead = 0;
+            break;
+        case SYSTEM_RESET:
+            systemReset();
+            break;
+        case REPORT_VERSION:
+            Firmata.printVersion();
+            break;
+        }
+    }
+}
+
+//------------------------------------------------------------------------------
+// Serial Send Handling
+
+// send an analog message
+void FirmataClass::sendAnalog(byte pin, int value) 
+{
+	// pin can only be 0-15, so chop higher bits
+	Serial.print(ANALOG_MESSAGE | (pin & 0xF), BYTE);
+    sendValueAsTwo7bitBytes(value);
+}
+
+// send a single digital pin in a digital message
+void FirmataClass::sendDigital(byte pin, int value) 
+{
+	/* TODO add single pin digital messages to the protocol, this needs to
+     * track the last digital data sent so that it can be sure to change just
+     * one bit in the packet.  This is complicated by the fact that the
+     * numbering of the pins will probably differ on Arduino, Wiring, and
+     * other boards.  The DIGITAL_MESSAGE sends 14 bits at a time, but it is
+     * probably easier to send 8 bit ports for any board with more than 14
+     * digital pins.
+     */
+
+	// TODO: the digital message should not be sent on the serial port every
+	// time sendDigital() is called.  Instead, it should add it to an int
+	// which will be sent on a schedule.  If a pin changes more than once
+	// before the digital message is sent on the serial port, it should send a
+	// digital message for each change.
+
+//    if(value == 0)
+//        sendDigitalPortPair();
+}
+
+
+// send 14-bits in a single digital message (protocol v1)
+// send an 8-bit port in a single digital message (protocol v2)
+void FirmataClass::sendDigitalPort(byte portNumber, int portData)
+{
+	Serial.print(DIGITAL_MESSAGE | (portNumber & 0xF),BYTE);
+	Serial.print(portData % 128, BYTE); // Tx bits 0-6
+	Serial.print(portData >> 7, BYTE);  // Tx bits 7-13
+}
+
+
+void FirmataClass::sendSysex(byte command, byte bytec, byte* bytev) 
+{
+    byte i;
+    startSysex();
+    Serial.print(command, BYTE);
+    for(i=0; i<bytec; i++) {
+        sendValueAsTwo7bitBytes(bytev[i]);        
+    }
+    endSysex();
+}
+
+void FirmataClass::sendString(byte command, const char* string) 
+{
+    sendSysex(command, strlen(string), (byte *)string);
+}
+
+
+// send a string as the protocol string type
+void FirmataClass::sendString(const char* string) 
+{
+    sendString(FIRMATA_STRING, string);
+}
+
+
+// Internal Actions/////////////////////////////////////////////////////////////
+
+// generic callbacks
+void FirmataClass::attach(byte command, callbackFunction newFunction)
+{
+    switch(command) {
+    case ANALOG_MESSAGE: currentAnalogCallback = newFunction; break;
+    case DIGITAL_MESSAGE: currentDigitalCallback = newFunction; break;
+    case REPORT_ANALOG: currentReportAnalogCallback = newFunction; break;
+    case REPORT_DIGITAL: currentReportDigitalCallback = newFunction; break;
+    case SET_PIN_MODE: currentPinModeCallback = newFunction; break;
+    }
+}
+
+void FirmataClass::attach(byte command, systemResetCallbackFunction newFunction)
+{
+    switch(command) {
+    case SYSTEM_RESET: currentSystemResetCallback = newFunction; break;
+    }
+}
+
+void FirmataClass::attach(byte command, stringCallbackFunction newFunction)
+{
+    switch(command) {
+    case FIRMATA_STRING: currentStringCallback = newFunction; break;
+    }
+}
+
+void FirmataClass::attach(byte command, sysexCallbackFunction newFunction)
+{
+    currentSysexCallback = newFunction;
+}
+
+void FirmataClass::detach(byte command)
+{
+    switch(command) {
+    case SYSTEM_RESET: currentSystemResetCallback = NULL; break;
+    case FIRMATA_STRING: currentStringCallback = NULL; break;
+    case START_SYSEX: currentSysexCallback = NULL; break;
+    default:
+        attach(command, (callbackFunction)NULL);
+    }
+}
+
+// sysex callbacks
+/*
+ * this is too complicated for analogReceive, but maybe for Sysex?
+ void FirmataClass::attachSysex(sysexFunction newFunction)
+ {
+ byte i;
+ byte tmpCount = analogReceiveFunctionCount;
+ analogReceiveFunction* tmpArray = analogReceiveFunctionArray;
+ analogReceiveFunctionCount++;
+ analogReceiveFunctionArray = (analogReceiveFunction*) calloc(analogReceiveFunctionCount, sizeof(analogReceiveFunction));
+ for(i = 0; i < tmpCount; i++) {
+ analogReceiveFunctionArray[i] = tmpArray[i];
+ }
+ analogReceiveFunctionArray[tmpCount] = newFunction;
+ free(tmpArray);
+ }
+*/
+
+//******************************************************************************
+//* Private Methods
+//******************************************************************************
+
+
+
+// resets the system state upon a SYSTEM_RESET message from the host software
+void FirmataClass::systemReset(void)
+{
+    byte i;
+
+    waitForData = 0; // this flag says the next serial input will be data
+    executeMultiByteCommand = 0; // execute this after getting multi-byte data
+    multiByteChannel = 0; // channel data for multiByteCommands
+
+
+    for(i=0; i<MAX_DATA_BYTES; i++) {
+        storedInputData[i] = 0;
+    }
+
+    parsingSysex = false;
+    sysexBytesRead = 0;
+
+    if(currentSystemResetCallback)
+                    (*currentSystemResetCallback)();
+
+    //flush(); //TODO uncomment when Firmata is a subclass of HardwareSerial
+}
+
+
+
+// =============================================================================
+// used for flashing the pin for the version number
+void FirmataClass::pin13strobe(int count, int onInterval, int offInterval) 
+{
+    byte i;
+    pinMode(VERSION_BLINK_PIN, OUTPUT);
+    for(i=0; i<count; i++) {
+        delay(offInterval);
+        digitalWrite(VERSION_BLINK_PIN, HIGH);
+        delay(onInterval);
+        digitalWrite(VERSION_BLINK_PIN, LOW);
+    }
+}
+
+
+// make one instance for the user to use
+FirmataClass Firmata;
+
+