Bugs fixed

3 files changed, 686 insertions, 5 deletions

diff --git a/Source/libmodelPlugFirmata.so b/Source/libmodelPlugFirmata.so
index 22d3c01..3e71a82 100755
--- a/Source/libmodelPlugFirmata.so
+++ b/Source/libmodelPlugFirmata.so
diff --git a/Source/modelPlugFirmata.cpp b/Source/modelPlugFirmata.cpp
index cd4441e..8ad53cf 100755..100644
--- a/Source/modelPlugFirmata.cpp
+++ b/Source/modelPlugFirmata.cpp
@@ -21,6 +21,7 @@
 #include <stdint.h>
 #include <algorithm>
 #include <vector>
+#include <cmath>
 
 #include "serial.h"
 #include "modelPlugFirmata.h"
@@ -81,7 +82,7 @@ public:
 	void writeDigitalPin(uint32_t pin,uint32_t value);
 	void writeAnalogPin(uint32_t pin, uint32_t value);
 	void writeServoPin(uint32_t pin, uint32_t value, int min, int max);
-	double readAnalogPin(uint32_t pin, double min, double max, double init);
+	double readAnalogPin(uint32_t pin, double min, double max, double init, int adcResolution);
 	uint32_t readDigitalPin(uint32_t pin, int init);
 	void setServoConfig(uint32_t pin,uint32_t min,uint32_t max);
 
@@ -244,12 +245,12 @@ void firmataBoard::writeServoPin(uint32_t pin, uint32_t value, int min, int max)
 	}
 }
 
-double firmataBoard::readAnalogPin(uint32_t pin,double min, double max, double init){
+double firmataBoard::readAnalogPin(uint32_t pin,double min, double max, double init, int adcResolution){
 	if(ready){
 		if(pin_info[pin].mode!=MODE_ANALOG)
 			setPinMode(pin,MODE_ANALOG);
 		if(pin_info[pin].ready)
-			return min+((pin_info[pin].value/1023.0)*(max-min));
+			return min+((pin_info[pin].value/(pow(2, adcResolution)-1))*(max-min));
 		else
 			return init;
 	}
@@ -652,9 +653,9 @@ EXPORT int getBoardId(void* object){
 		return -1;
 }
 
-EXPORT double readAnalogPin(int pin, double min, double max, double init, int id){
+EXPORT double readAnalogPin(int pin, double min, double max, double init, int id, int adcResolution){
 	firmataBoard* board = firmataBoardHandler::instance()->getBoard(id);
-	double value = board->readAnalogPin(pin,min,max,init);
+	double value = board->readAnalogPin(pin,min,max,init, adcResolution);
 	return value;
 }
 EXPORT int readDigitalPin(int pin, int init, int id){
diff --git a/Source/modelPlugFirmata.cpp~ b/Source/modelPlugFirmata.cpp~
new file mode 100755
index 0000000..011ca40
--- /dev/null
+++ b/Source/modelPlugFirmata.cpp~
@@ -0,0 +1,680 @@
+
+/*  ModelPlug: Modelica connection to Firmata
+ *  Author: Leonardo Laguna Ruiz
+ *  Based on Firmata GUI-friendly queries test by Paul Stoffregen (paul@pjrc.com)
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <algorithm>
+#include <vector>
+
+#include "serial.h"
+#include "modelPlugFirmata.h"
+
+
+typedef struct {
+	uint8_t  mode;
+	uint8_t  analog_channel;
+	uint64_t supported_modes;
+	uint32_t value;
+	uint32_t next_value;
+	uint8_t  ready;
+} pin_t;
+
+#define MODE_INPUT    0x00
+#define MODE_OUTPUT   0x01
+#define MODE_ANALOG   0x02
+#define MODE_PWM      0x03
+#define MODE_SERVO    0x04
+#define MODE_SHIFT    0x05
+#define MODE_I2C      0x06
+
+#define START_SYSEX             0xF0 // start a MIDI Sysex message
+#define END_SYSEX               0xF7 // end a MIDI Sysex message
+#define PIN_MODE_QUERY          0x72 // ask for current and supported pin modes
+#define PIN_MODE_RESPONSE       0x73 // reply with current and supported pin modes
+
+#define RESERVED_COMMAND        0x00 // 2nd SysEx data byte is a chip-specific command (AVR, PIC, TI, etc).
+#define ANALOG_MAPPING_QUERY    0x69 // ask for mapping of analog to pin numbers
+#define ANALOG_MAPPING_RESPONSE 0x6A // reply with mapping info
+#define CAPABILITY_QUERY        0x6B // ask for supported modes and resolution of all pins
+#define CAPABILITY_RESPONSE     0x6C // reply with supported modes and resolution
+#define PIN_STATE_QUERY         0x6D // ask for a pin's current mode and value
+#define PIN_STATE_RESPONSE      0x6E // reply with a pin's current mode and value
+#define EXTENDED_ANALOG         0x6F // analog write (PWM, Servo, etc) to any pin
+#define SERVO_CONFIG            0x70 // set max angle, minPulse, maxPulse, freq
+#define STRING_DATA             0x71 // a string message with 14-bits per char
+#define SHIFT_DATA              0x75 // shiftOut config/data message (34 bits)
+#define I2C_REQUEST             0x76 // I2C request messages from a host to an I/O board
+#define I2C_REPLY               0x77 // I2C reply messages from an I/O board to a host
+#define I2C_CONFIG              0x78 // Configure special I2C settings such as power pins and delay times
+#define REPORT_FIRMWARE         0x79 // report name and version of the firmware
+#define SAMPLING_INTERVAL       0x7A // sampling interval
+#define SYSEX_NON_REALTIME      0x7E // MIDI Reserved for non-realtime messages
+#define SYSEX_REALTIME          0x7F // MIDI Reserved for realtime messages
+
+class firmataBoard
+{
+public:
+	firmataBoard(std::string port = "dummy",bool showCapabilitites=false,int samplingMs=10,int baudRate=57600,bool fake=true);
+	~firmataBoard(){};
+
+	std::string getPortName();
+	int openPort();
+	void closePort();
+	std::vector<std::string> getPortList();
+	void setPinMode(uint32_t pin, uint32_t mode);
+	void writeDigitalPin(uint32_t pin,uint32_t value);
+	void writeAnalogPin(uint32_t pin, uint32_t value);
+	void writeServoPin(uint32_t pin, uint32_t value, int min, int max);
+	double readAnalogPin(uint32_t pin, double min, double max, double init);
+	uint32_t readDigitalPin(uint32_t pin, int init);
+	void setServoConfig(uint32_t pin,uint32_t min,uint32_t max);
+
+public:
+	void initialize(bool dtr);
+	void updateBoard(int timeout);
+	void reportFirmware();
+	void showPinCapabilities();
+	void setSamplingInterval();
+	void setAsFakePort();
+private:
+
+	int write(const void *ptr, int len);
+	int read(void *ptr, int count,int timeout);
+	void Parse(const uint8_t *buf, int len);
+	void DoMessage(void);
+
+	pin_t pin_info[128];
+	std::string port_name;
+	bool show_capabilitites;
+	int sampling;
+	int baudrate;
+	Serial serial_port;
+	bool ready;
+	bool is_fake_port;
+
+
+	std::string firmata_name;
+	int parse_count;
+	int parse_command_len;
+	uint8_t parse_buf[4096];
+};
+
+// This is an external object that we want to use in our modelica code.
+class firmataBoardHandler  {
+	firmataBoardHandler(){
+		try
+		{
+			std::vector<std::string> available_ports = dummyPort.getPortList();
+			printf("[ModelPlug.Firmata]: Available ports:\n"); fflush(stdout);
+			for (std::vector<std::string>::iterator it = available_ports.begin() ; it != available_ports.end(); ++it){
+				printf("- %s\n",(*it).c_str());fflush(stdout);
+			}
+		}
+		catch(std::exception&)
+		{
+			printf("[ModelPlug.Firmata]: Failed to show the port names\n");fflush(stdout);
+		}
+	}
+
+public:
+	static firmataBoardHandler *instance()
+    {
+        if (!s_instance)
+          s_instance = new firmataBoardHandler;
+        return s_instance;
+    }
+    firmataBoard* registerBoard(char* port,bool showCapabilitites,int samplingMs,int baudRate,bool dtr);
+    int getPortIndex(firmataBoard* elem);
+
+    firmataBoard* getBoard(uint32_t id);
+
+private:
+	std::vector<firmataBoard*> ports;
+
+	firmataBoard dummyPort;
+
+	static firmataBoardHandler *s_instance;
+
+};
+
+firmataBoardHandler *firmataBoardHandler::s_instance = 0;
+
+int firmataBoard::write(const void *ptr, int len){
+	if(!is_fake_port) return serial_port.Write(ptr,len);
+	return 0;
+}
+
+int firmataBoard::read(void *ptr, int count, int timeout){
+	if(!is_fake_port){
+		if (serial_port.Is_open() && (serial_port.Input_wait(timeout)>0))
+			return serial_port.Read(ptr, count);
+		return 0;
+	}
+	return 0;
+}
+
+void firmataBoard::setAsFakePort(){
+	is_fake_port = true;
+}
+
+firmataBoard::firmataBoard(std::string port, bool showCapabilitites, int samplingMs, int baudRate, bool fake)
+	:port_name(port),show_capabilitites(showCapabilitites),sampling(samplingMs),baudrate(baudRate),is_fake_port(fake){
+	// Initialize the pin information
+	for (int i=0; i < 128; i++) {
+		pin_info[i].mode = 255;
+		pin_info[i].analog_channel = 127;
+		pin_info[i].supported_modes = 0;
+		pin_info[i].value = 0;
+		pin_info[i].next_value = 0;
+		pin_info[i].ready = 0;
+	}
+	ready = false;
+}
+
+void firmataBoard::setPinMode(uint32_t pin, uint32_t mode){
+
+	if (mode != pin_info[pin].mode && ready) {
+		// send the mode change message
+		uint8_t buf[4];
+		buf[0] = 0xF4;
+		buf[1] = pin;
+		buf[2] = mode;
+		write(buf, 3);
+		pin_info[pin].mode = mode;
+		pin_info[pin].value = 0;
+		pin_info[pin].next_value = 0;
+		printf("[ModelPlug.Firmata]: %s setting pin %i to mode %i\n",port_name.c_str(),pin,mode);
+	}
+}
+
+void firmataBoard::setServoConfig(uint32_t pin,uint32_t min,uint32_t max){
+	if(ready){
+		uint8_t buf[8];
+		buf[0] = START_SYSEX;
+		buf[1] = SERVO_CONFIG;
+		buf[2] = pin;
+		buf[3] = min & 0x7F;
+		buf[4] = (min >> 7) & 0x7F;
+		buf[5] = max & 0x7F;
+		buf[6] = (max >> 7) & 0x7F;
+		buf[7] = END_SYSEX;
+		write(buf, 8);
+	}
+}
+
+void firmataBoard::writeDigitalPin(uint32_t pin,uint32_t value){
+	if(ready){
+		if(pin_info[pin].mode!=MODE_OUTPUT)
+			setPinMode(pin,MODE_OUTPUT);
+		pin_info[pin].next_value = value;
+	}
+}
+
+void firmataBoard::writeAnalogPin(uint32_t pin, uint32_t value){
+	if(ready){
+		if(pin_info[pin].mode!=MODE_PWM)
+			setPinMode(pin,MODE_PWM);
+		pin_info[pin].next_value = value;
+	}
+}
+
+void firmataBoard::writeServoPin(uint32_t pin, uint32_t value, int min, int max){
+	if(ready){
+		if(pin_info[pin].mode!=MODE_SERVO){
+			setPinMode(pin,MODE_SERVO);
+			setServoConfig(pin,min,max);
+		}
+		pin_info[pin].next_value = value;
+	}
+}
+
+double firmataBoard::readAnalogPin(uint32_t pin,double min, double max, double init, int adcResolution){
+	if(ready){
+		if(pin_info[pin].mode!=MODE_ANALOG)
+			setPinMode(pin,MODE_ANALOG);
+		if(pin_info[pin].ready)
+			return min+((pin_info[pin].value/pow(2, adcResolution)-1)*(max-min));
+		else
+			return init;
+	}
+	return init;
+}
+
+uint32_t firmataBoard::readDigitalPin(uint32_t pin, int init){
+	if(ready){
+		if(pin_info[pin].mode!=MODE_INPUT)
+			setPinMode(pin,MODE_INPUT);
+		if(pin_info[pin].ready)
+			return pin_info[pin].value;
+		else
+			return init;
+	}
+	return init;
+}
+
+void firmataBoard::reportFirmware(){
+	uint8_t buf[3];
+	buf[0] = START_SYSEX;
+	buf[1] = REPORT_FIRMWARE; // read firmata name & version
+	buf[2] = END_SYSEX;
+	write(buf, 3);
+}
+
+void firmataBoard::setSamplingInterval(){
+	uint8_t buf[5];
+	buf[0] = START_SYSEX;
+	buf[1] = SAMPLING_INTERVAL; // read firmata name & version
+	buf[2] = sampling & 0x7F;
+	buf[3] = (sampling >> 7) & 0x7F;
+	buf[4] = END_SYSEX;
+	write(buf, 5);
+	printf("[ModelPlug.Firmata]: Setting sampling interval to %i ms for board %s\n",sampling,port_name.c_str()); fflush(stdout);
+}
+
+void firmataBoard::showPinCapabilities(){
+	printf("[ModelPlug.Firmata]: Board %s capabilities\n", port_name.c_str());
+	for (int pin=0; pin<128; pin++) {
+		if(pin_info[pin].supported_modes!=0){
+			printf("- Pin %i supports: ",pin);
+			if (pin_info[pin].supported_modes & (1<<MODE_INPUT))  printf(" DigitalInput");
+			if (pin_info[pin].supported_modes & (1<<MODE_OUTPUT)) printf(" - DigitalOutput");
+			if (pin_info[pin].supported_modes & (1<<MODE_ANALOG)) printf(" - AnalogInput(A%d)",pin_info[pin].analog_channel);
+			if (pin_info[pin].supported_modes & (1<<MODE_PWM))    printf(" - AnalogOutput");
+			if (pin_info[pin].supported_modes & (1<<MODE_SERVO))  printf(" - Servo");
+			printf("\n");
+		}
+	}
+	fflush(stdout);
+}
+
+void firmataBoard::updateBoard(int timeout){
+	uint8_t buf[1024];
+	int r;
+	// write the output values
+	if(ready)
+	for (int pin=0; pin<128; pin++) {
+		if(pin_info[pin].value!=pin_info[pin].next_value && (pin_info[pin].mode==MODE_OUTPUT || pin_info[pin].mode==MODE_PWM || pin_info[pin].mode==MODE_SERVO)){
+
+			pin_info[pin].value=pin_info[pin].next_value;
+
+			// Digital output
+			if(pin_info[pin].mode==MODE_OUTPUT){
+				int port_num = pin / 8;
+				int port_val = 0;
+				for (int i=0; i<8; i++) {
+					int p = port_num * 8 + i;
+					if (pin_info[p].mode == MODE_OUTPUT || pin_info[p].mode == MODE_INPUT) {
+						if (pin_info[p].value) {
+							port_val |= (1<<i);
+						}
+					}
+				}
+				uint8_t buf[3];
+				buf[0] = 0x90 | port_num;
+				buf[1] = port_val & 0x7F;
+				buf[2] = (port_val >> 7) & 0x7F;
+				write(buf, 3);
+			}
+			// Analog output or servo
+			if(pin_info[pin].mode==MODE_PWM || pin_info[pin].mode==MODE_SERVO){
+				uint32_t value = pin_info[pin].value;
+
+				if (pin <= 15 && value <= 16383) {
+					uint8_t buf[3];
+					buf[0] = 0xE0 | pin;
+					buf[1] = value & 0x7F;
+					buf[2] = (value >> 7) & 0x7F;
+					write(buf, 3);
+				} else {
+					uint8_t buf[12];
+					int len=4;
+					buf[0] = 0xF0;
+					buf[1] = 0x6F;
+					buf[2] = pin;
+					buf[3] = value & 0x7F;
+					if (value > 0x00000080) buf[len++] = (value >> 7) & 0x7F;
+					if (value > 0x00004000) buf[len++] = (value >> 14) & 0x7F;
+					if (value > 0x00200000) buf[len++] = (value >> 21) & 0x7F;
+					if (value > 0x10000000) buf[len++] = (value >> 28) & 0x7F;
+					buf[len++] = 0xF7;
+					write(buf, len);
+				}
+			}
+		}
+	}
+	// receieve bytes from the serial port
+	r = read(buf, sizeof(buf),timeout);
+	if (r) {
+		Parse(buf, r);
+	}
+}
+
+void firmataBoard::Parse(const uint8_t *buf, int len)
+{
+	const uint8_t *p, *end;
+
+	p = buf;
+	end = p + len;
+	for (p = buf; p < end; p++) {
+		uint8_t msn = *p & 0xF0;
+		if (msn == 0xE0 || msn == 0x90 || *p == 0xF9) {
+			parse_command_len = 3;
+			parse_count = 0;
+		} else if (msn == 0xC0 || msn == 0xD0) {
+			parse_command_len = 2;
+			parse_count = 0;
+		} else if (*p == START_SYSEX) {
+			parse_count = 0;
+			parse_command_len = sizeof(parse_buf);
+		} else if (*p == END_SYSEX) {
+			parse_command_len = parse_count + 1;
+		} else if (*p & 0x80) {
+			parse_command_len = 1;
+			parse_count = 0;
+		}
+		if (parse_count < (int)sizeof(parse_buf)) {
+			parse_buf[parse_count++] = *p;
+		}
+		if (parse_count == parse_command_len) {
+			DoMessage();
+			parse_count = parse_command_len = 0;
+		}
+	}
+}
+
+void firmataBoard::DoMessage(void)
+{
+	uint8_t cmd = (parse_buf[0] & 0xF0);
+
+	if (cmd == 0xE0 && parse_count == 3) {
+		int analog_ch = (parse_buf[0] & 0x0F);
+		int analog_val = parse_buf[1] | (parse_buf[2] << 7);
+		for (int pin=0; pin<128; pin++) {
+			if (pin_info[pin].analog_channel == analog_ch) {
+				pin_info[pin].value = analog_val;
+				pin_info[pin].ready = 1;
+				return;
+			}
+		}
+		return;
+	}
+	if (cmd == 0x90 && parse_count == 3) {
+		int port_num = (parse_buf[0] & 0x0F);
+		int port_val = parse_buf[1] | (parse_buf[2] << 7);
+		int pin = port_num * 8;
+		//printf("port_num = %d, port_val = %d\n", port_num, port_val);
+		for (int mask=1; mask & 0xFF; mask <<= 1, pin++) {
+			if (pin_info[pin].mode == MODE_INPUT) {
+				uint32_t val = (port_val & mask) ? 1 : 0;
+				if (pin_info[pin].value != val) {
+					pin_info[pin].value = val;
+					pin_info[pin].ready = 1;
+				}
+			}
+		}
+		return;
+	}
+
+
+	if (parse_buf[0] == START_SYSEX && parse_buf[parse_count-1] == END_SYSEX) {
+		// Sysex message
+		if (parse_buf[1] == REPORT_FIRMWARE) {
+			char name[140];
+			int len=0;
+			for (int i=4; i < parse_count-2; i+=2) {
+				name[len++] = (parse_buf[i] & 0x7F)
+				  | ((parse_buf[i+1] & 0x7F) << 7);
+			}
+			name[len++] = '-';
+			name[len++] = parse_buf[2] + '0';
+			name[len++] = '.';
+			name[len++] = parse_buf[3] + '0';
+			name[len++] = 0;
+			firmata_name = name;
+			printf("[ModelPlug.Firmata]: %s %s\n",port_name.c_str(),name);
+			ready = true;
+			// query the board's capabilities only after hearing the
+			// REPORT_FIRMWARE message.  For boards that reset when
+			// the port open (eg, Arduino with reset=DTR), they are
+			// not ready to communicate for some time, so the only
+			// way to reliably query their capabilities is to wait
+			// until the REPORT_FIRMWARE message is heard.
+			uint8_t buf[80];
+			len=0;
+			buf[len++] = START_SYSEX;
+			buf[len++] = ANALOG_MAPPING_QUERY; // read analog to pin # info
+			buf[len++] = END_SYSEX;
+			buf[len++] = START_SYSEX;
+			buf[len++] = CAPABILITY_QUERY; // read capabilities
+			buf[len++] = END_SYSEX;
+			for (int i=0; i<16; i++) {
+				buf[len++] = 0xC0 | i;  // report analog
+				buf[len++] = 1;
+				buf[len++] = 0xD0 | i;  // report digital
+				buf[len++] = 1;
+			}
+			write(buf, len);
+			setSamplingInterval();
+		} else if (parse_buf[1] == CAPABILITY_RESPONSE) {
+			int pin, i, n;
+			for (pin=0; pin < 128; pin++) {
+				pin_info[pin].supported_modes = 0;
+			}
+			for (i=2, n=0, pin=0; i<parse_count; i++) {
+				if (parse_buf[i] == 127) {
+					pin++;
+					n = 0;
+					continue;
+				}
+				if (n == 0) {
+					// first byte is supported mode
+					pin_info[pin].supported_modes |= (1<<parse_buf[i]);
+				}
+				n = n ^ 1;
+			}
+			if(show_capabilitites) {
+				showPinCapabilities();
+				show_capabilitites = false;
+			}
+			// send a state query for for every pin with any modes
+			for (pin=0; pin < 128; pin++) {
+				uint8_t buf[512];
+				int len=0;
+				if (pin_info[pin].supported_modes) {
+					buf[len++] = START_SYSEX;
+					buf[len++] = PIN_STATE_QUERY;
+					buf[len++] = pin;
+					buf[len++] = END_SYSEX;
+				}
+				write(buf, len);
+			}
+		} else if (parse_buf[1] == ANALOG_MAPPING_RESPONSE) {
+			int pin=0;
+			for (int i=2; i<parse_count-1; i++) {
+				pin_info[pin].analog_channel = parse_buf[i];
+				pin++;
+			}
+			return;
+		} else if (parse_buf[1] == PIN_STATE_RESPONSE && parse_count >= 6) {
+			int pin = parse_buf[2];
+			pin_info[pin].mode = parse_buf[3];
+			pin_info[pin].value = parse_buf[4];
+			if (parse_count > 6) pin_info[pin].value |= (parse_buf[5] << 7);
+			if (parse_count > 7) pin_info[pin].value |= (parse_buf[6] << 14);
+			//add_pin(pin);
+		}
+		return;
+	}
+}
+
+std::vector<std::string> firmataBoard::getPortList(){
+	return serial_port.port_list();
+}
+
+firmataBoard* firmataBoardHandler::getBoard(uint32_t id){
+	if(id<0)
+		return &dummyPort;
+	else
+		return ports[id];
+}
+
+firmataBoard* firmataBoardHandler::registerBoard(char* port,bool showCapabilitites,int samplingMs,int baudRate,bool dtr){
+
+	// Check if the port exists
+	bool port_exists = false;
+	bool fake_port = false;
+	std::string port_name(port);
+	std::vector<std::string> available_ports = dummyPort.getPortList();
+	for (std::vector<std::string>::iterator it = available_ports.begin() ; it != available_ports.end(); ++it){
+		if((*it).compare(port_name)==0){
+			port_exists = true;
+			break;
+		}
+	}
+
+	if(!port_exists) {
+		printf("[ModelPlug.Firmata]: ERROR The port %s does not exist\n",port); fflush(stdout);
+		fake_port |= true;
+	}
+
+	// Check if the port is already used
+	for (std::vector<firmataBoard*>::iterator it = ports.begin() ; it != ports.end(); ++it){
+		if ((*it)->getPortName().compare(port_name)==0)
+		{
+			// Print duplicated port, report error
+			printf("[ModelPlug.Firmata]: ERROR: Trying to use two boards with the port %s\n",port);fflush(stdout);
+			fake_port |= true;
+		}
+	}
+
+	firmataBoard* new_port = new firmataBoard(port_name,showCapabilitites,samplingMs,baudRate,fake_port);
+
+	if(new_port->openPort()==0){
+		ports.push_back(new_port);
+		new_port->initialize(dtr);
+		return new_port;
+	}
+	else{
+		// Could not open the port return error
+		printf("[ModelPlug.Firmata]: ERROR Failed to open the port %s\n",port); fflush(stdout);
+		new_port->setAsFakePort();
+		return new_port;
+	}
+	return new_port;
+}
+
+int firmataBoardHandler::getPortIndex(firmataBoard* elem) {
+	unsigned pos = std::find(ports.begin(), ports.end(), elem) - ports.begin();
+	if(pos >= ports.size()){ // not found
+		return -1;
+	}
+	return pos;
+}
+
+std::string firmataBoard::getPortName(){
+	return port_name;
+}
+
+int firmataBoard::openPort(){
+	if(is_fake_port){
+		return 0;
+	} else {
+		serial_port.Open(port_name);
+		serial_port.Set_baud(baudrate);
+		if(serial_port.Is_open()){
+			printf("[ModelPlug.Firmata]: Using port %s with baud rate %i\n",port_name.c_str(),baudrate); fflush(stdout);
+			return 0;
+		}
+		else
+			return -1;
+	}
+	return -1;
+}
+
+void firmataBoard::initialize(bool dtr){
+	serial_port.Set_control(dtr?1:0,-1);
+	updateBoard(5);
+	reportFirmware();
+	updateBoard(5);
+}
+
+void firmataBoard::closePort(){
+	if(is_fake_port) return;
+	else
+		if(serial_port.Is_open())
+			serial_port.Close();
+}
+
+extern "C"
+
+{
+
+
+EXPORT void* boardConstructor(char* port,bool showCapabilitites,int samplingMs,int baudRate,bool dtr){
+	firmataBoardHandler* boards = firmataBoardHandler::instance();
+	void* object = (void*)boards->registerBoard(port,showCapabilitites,samplingMs,baudRate,dtr);
+	return object;
+}
+EXPORT void boardDestructor(void* object){
+	if(object!=NULL){
+		firmataBoard* board = (firmataBoard*)object;
+		board->closePort();
+		delete board;
+	}
+}
+
+EXPORT void updateBoard(int id){
+	firmataBoard* board = firmataBoardHandler::instance()->getBoard(id);
+	board->updateBoard(0);
+}
+
+EXPORT int getBoardId(void* object){
+	if(object!=NULL){
+		return firmataBoardHandler::instance()->getPortIndex((firmataBoard*)object);
+	}
+	else
+		return -1;
+}
+
+EXPORT double readAnalogPin(int pin, double min, double max, double init, int id, int adcResolution){
+	firmataBoard* board = firmataBoardHandler::instance()->getBoard(id);
+	double value = board->readAnalogPin(pin,min,max,init, adcResolution);
+	return value;
+}
+EXPORT int readDigitalPin(int pin, int init, int id){
+	firmataBoard* board = firmataBoardHandler::instance()->getBoard(id);
+	int value = board->readDigitalPin(pin,init);
+	return value;
+}
+EXPORT void writeAnalogPin(int pin, int id,double value){
+	double croped = value>1.0?1.0:(value<0.0?0.0:value);
+	firmataBoard* board = firmataBoardHandler::instance()->getBoard(id);
+	board->writeAnalogPin(pin,(uint32_t)(croped*1023));
+}
+EXPORT void writeDigitalPin(int pin, int id,int value){
+	firmataBoard* board = firmataBoardHandler::instance()->getBoard(id);
+	board->writeDigitalPin(pin,value);
+}
+EXPORT void writeServoPin(int pin, int id,double value,int min,int max){
+	double croped = value>1.0?1.0:(value<0.0?0.0:value);
+	firmataBoard* board = firmataBoardHandler::instance()->getBoard(id);
+	board->writeServoPin(pin,(int)(croped*180),min,max);
+}
+
+} // end extern "C"