 #include "freeram.h"
-
 #include "mpu.h"
 #include "I2Cdev.h"
+#include "RS485_protocol.h"
+
+const byte ENABLE_PIN = 4;
+const byte SLAVE_ID = 1;
+union float2bytes { float f; byte b[sizeof(float)]; };
+
+
+void fWrite (const byte content)
+  {
+  Serial.write (content);
+  }
+
+int fAvailable ()
+  {
+  return Serial.available ();
+  }
+
+ int fRead ()
+  {
+  return Serial.read ();
+  }
+
 
 int ret;
 void setup() {
     Fastwire::setup(400,0);
     Serial.begin(38400);
     ret = mympu_open(200);
+    pinMode (ENABLE_PIN, OUTPUT);  // driver output enable
+    digitalWrite (ENABLE_PIN, LOW);
+
+#ifdef DMP_DEBUG
     Serial.print("MPU init: "); Serial.println(ret);
     Serial.print("Free mem: "); Serial.println(freeRam());
-
+#endif
 }
 
 unsigned int c = 0; //cumulative number of successful MPU/DMP reads

unsigned int err_o = 0; //cumulative number of MPU/DMP reads that had overflow bit set

void loop() {
    ret = mympu_update();

    switch (ret) {
	case 0: c++; break;
	case 1: np++; return;

	    Serial.print(" a2: "); Serial.print(mympu.accel[1]);
	    Serial.print(" a3: "); Serial.println(mympu.accel[2]);
    }
#endif
}
