diff --git a/I2CFunctions.h b/I2CFunctions.h
index f08831b..49d8836 100644
--- a/I2CFunctions.h
+++ b/I2CFunctions.h
@@ -2,7 +2,14 @@
 #define I2C_FUNCTIONS_H
 
 #include <Arduino.h>
+
+// For more defines for Teensy (granularity), see: 
+// https://arduino.stackexchange.com/questions/21137/arduino-how-to-get-the-board-type-in-code 
+#if defined(TEENSYDUINO)
+#include <i2c_t3.h>
+#else
 #include <Wire.h>
+#endif
 
 #define STOP_CONDITION_I2C true
 class I2CFunctions {
diff --git a/LidarController.h b/LidarController.h
index d6c537d..32ef87a 100644
--- a/LidarController.h
+++ b/LidarController.h
@@ -3,8 +3,6 @@
 
 #include "I2CFunctions.h"
 #include "LidarObject.h"
-#include <Wire.h>
-
 // Wait between I2C transactions in µs
 // One bit every 10µs (2.5µs in 400kHz)
 // Wait at least 5 bits to wait for slave answer
@@ -312,13 +310,15 @@ class LidarController {
       if(biasCorrection) nack = I2C.write(lidars[Lidar]->address, REG_ACQ_COMMAND, DATA_MEASURE_WITH_BIAS);
       else nack = I2C.write(lidars[Lidar]->address, REG_ACQ_COMMAND, DATA_MEASURE_WITHOUT_BIAS);
       shouldIncrementNack(Lidar, nack);
+	  return nack;
     };
 
     /*******************************************************************************
       distance:
         - Read the measured value from data registers
     *******************************************************************************/
-    uint8_t distance(uint8_t Lidar, int * data) {
+    //uint8_t distance(uint8_t Lidar, int * data) {
+	uint8_t distance(uint8_t Lidar, int16_t * data) {
       uint8_t distanceArray[2];
       uint8_t nackCatcher = I2C.readWord(lidars[Lidar]->address, MEASURED_VALUE_REGISTER, distanceArray);
       shouldIncrementNack(Lidar, nackCatcher);
@@ -353,7 +353,7 @@ class LidarController {
         This has to be worked on, this is the original implementation without the 
           blocking architecture
     *******************************************************************************/
-    int velocity(uint8_t Lidar, int * data) {
+    int16_t velocity(uint8_t Lidar, int16_t * data) {
       // Set in velocity mode
       I2C.write(lidars[Lidar]->address, REG_ACQ_CONFIG, DATA_VELOCITY_MODE_DATA);
       //  Write 0x04 to register 0x00 to start getting distance readings
@@ -362,7 +362,7 @@ class LidarController {
       uint8_t velocityArray[1];
       uint8_t nack = I2C.readByte(lidars[Lidar]->address, REG_VELOCITY, velocityArray);
 
-      return((int)((char)velocityArray[0]));
+      return((int16_t)((char)velocityArray[0]));
     };
 
     /*******************************************************************************
@@ -402,7 +402,7 @@ class LidarController {
       We could use the async() method in ACQUISITION_DONE, but it would need to spin
       one time more before starting the acquisition again
     *******************************************************************************/
-    uint8_t distanceAndAsync(uint8_t Lidar, int * data) {
+    uint8_t distanceAndAsync(uint8_t Lidar, int16_t * data) {
       uint8_t nackCatcher = distance(Lidar, data);
       // if error reading the value, try ONCE again
       if (nackCatcher)
diff --git a/LidarObject.h b/LidarObject.h
index ee54a7c..e4a71c2 100644
--- a/LidarObject.h
+++ b/LidarObject.h
@@ -1,5 +1,4 @@
 #include <Arduino.h>
-#include <Wire.h>
 #include "I2CFunctions.h"
 // We got a Lidar object per laser. 
 
@@ -128,6 +127,7 @@ class LidarObject {
 *******************************************************************************/
     bool resetNacksCount(){
       nacksCount = 0;
+	  return 1;
     };
 
 /*******************************************************************************
diff --git a/README.md b/README.md
index d9c0460..04d3a6a 100644
--- a/README.md
+++ b/README.md
@@ -5,6 +5,8 @@ Introduction
 --------------
 This library tends to improve the **efficiency** and the **robustness** of the poor original acquisition library. It is originally released for the LidarLite v2 (deprecated) and compatible with the LidarLite v3 (release from Garmin) lasermeter.
 
+The Teensy 3.x (3.5) 32-bit board support has been added by [adam-sampson](http://github.com/adam-sampson). If you use it, replace `#include <Wire.h>` by `#include <i2c_t3.h>` in the examples and install the [i2c_t3 library](https://github.com/nox771/i2c_t3)
+
 Improvements over the original library
 --------------------
 - *Asynchronous acquisition*
@@ -444,3 +446,4 @@ Pull requests are welcome :D
 Credits 
 ------
 * Alexis Paques (alexis[dot]paques[at]gmail[dot]com)
+* Adam Sampson
diff --git a/example/Teensy/Teensy.ino b/example/Teensy/Teensy.ino
new file mode 100644
index 0000000..90fbaf4
--- /dev/null
+++ b/example/Teensy/Teensy.ino
@@ -0,0 +1,47 @@
+#include <Arduino.h>
+#include <i2c_t3.h>
+#include <I2CFunctions.h> 
+#include <LidarObject.h>
+#include <LidarController.h>
+
+#define WIRE400K true
+// Trigger pin, can be unplugged
+#define Z1_LASER_TRIG 11
+// Enable pin, IMPORTANT
+#define Z1_LASER_EN 12
+// Mode pin, can be unplugged
+#define Z1_LASER_PIN 13
+//Define address of lasers
+//Thoses are written during initialisation
+// default address : 0x62
+#define Z1_LASER_AD 0x6E
+
+#define NUMBER_OF_LASERS 1
+
+// Create lasers
+static LidarController Controller;
+static LidarObject LZ1;
+
+void setup()
+{
+  Serial.begin(115200);
+  // Configure lasers
+  LZ1.begin(Z1_LASER_EN, Z1_LASER_PIN, Z1_LASER_TRIG, Z1_LASER_AD, 2, DISTANCE, 'A');
+  LZ1.setCallbackDistance(&distance_callback);
+  // Add the laser to the Controller
+  Controller.add(&LZ1, 0);
+
+  delay(100);
+  Controller.begin(WIRE400K);
+  delay(100);
+}
+
+void distance_callback(LidarObject* self){
+   Serial.println(self->distance);
+}
+
+void loop()
+{
+  Controller.spinOnce();
+  // Rest of your non blocking application. 
+}