Bugs fixed

1 files changed, 6 insertions, 5 deletions

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){