Revision 15:d117591084ff, committed 2017-05-02

Comitter:

nkosarek

Date:

Tue May 02 04:16:38 2017 +0000

Parent:

14:700ba072d1a0

Commit message:

Working i2c to ble to pi without serial

Changed in this revision

--- a/ButtonService.h	Wed Apr 19 01:47:36 2017 +0000
+++ b/ButtonService.h	Tue May 02 04:16:38 2017 +0000
@@ -13,32 +13,32 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
-
+ 
 #ifndef __BLE_BUTTON_SERVICE_H__
 #define __BLE_BUTTON_SERVICE_H__
-
+ 
 class ButtonService {
 public:
     const static uint16_t BUTTON_SERVICE_UUID              = 0xA000;
     const static uint16_t BUTTON_STATE_CHARACTERISTIC_UUID = 0xA001;
-
-    ButtonService(BLE &_ble, bool buttonPressedInitial) :
-        ble(_ble), buttonState(BUTTON_STATE_CHARACTERISTIC_UUID, &buttonPressedInitial, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY)
+ 
+    ButtonService(BLE &_ble, uint8_t* start) :
+        ble(_ble), buttonState(BUTTON_STATE_CHARACTERISTIC_UUID, start, 2, 20, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY)
     {
         GattCharacteristic *charTable[] = {&buttonState};
         GattService         buttonService(ButtonService::BUTTON_SERVICE_UUID, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
         ble.gattServer().addService(buttonService);
     }
-
+ 
     //void updateButtonState(uint8_t newState) {
     void updateButtonState(uint8_t *newState) {
         //ble.gattServer().write(buttonState.getValueHandle(), (uint8_t *)&newState, sizeof(uint8_t));
-        ble.gattServer().write(buttonState.getValueHandle(), newState, sizeof(uint8_t)*16);
+        ble.updateCharacteristicValue(buttonState.getValueHandle(), newState, 20);
     }
-
+ 
 private:
     BLE                              &ble;
-    ReadOnlyGattCharacteristic<bool>  buttonState;
+    GattCharacteristic  buttonState;
 };
-
-#endif /* #ifndef __BLE_BUTTON_SERVICE_H__ */
+ 
+#endif /* #ifndef __BLE_BUTTON_SERVICE_H__ */
\ No newline at end of file

--- a/main.cpp	Wed Apr 19 01:47:36 2017 +0000
+++ b/main.cpp	Tue May 02 04:16:38 2017 +0000
@@ -1,51 +1,165 @@
-/* mbed Microcontroller Library
- * Copyright (c) 2006-2013 ARM Limited
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
+/*
+ 
+Copyright (c) 2012-2014 RedBearLab
+ 
+Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+and associated documentation files (the "Software"), to deal in the Software without restriction,
+including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
+subject to the following conditions:
+The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+ 
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
+INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
+PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
+FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ 
+*/
+ 
 #include "mbed.h"
 #include "ble/BLE.h"
+#include "wire.h"
 #include "ButtonService.h"
-#include <time.h>
-
+ 
+#define BLE_Nano
+//#define nRF_51822
+ 
+#define LIS331HH
+ 
+#ifdef nRF_51822
+#define SCL         28
+#define SDA         29
+#endif
+ 
+#ifdef BLE_Nano
+#define SCL         P0_8
+#define SDA         P0_10
+#endif
+ 
+ 
+#ifdef LIS3DH
+#define ADDR_ONE    0x30
+#define ADDR_TWO    0x32
+#define AXIS_X      0x00
+#define AXIS_Y      0x01
+#define AXIS_Z      0x02
+#define REG_OUT_X_L 0x28
+#define REG_CTRL1   0x20
+#define REG_CTRL4   0x23
+#define REG_WHOAMI  0x0F
+#define RANGE_2G    0x00
+#define DEVICE_ID   0x33
+ 
+ 
+#define DATARATE_400HZ          0b0111 // 400Hz
+#define DATARATE_200HZ          0b0110 // 200Hz
+#define DATARATE_100HZ          0b0101 // 100Hz
+#define DATARATE_50HZ           0b0100 // 50Hz
+#define DATARATE_25HZ           0b0011 // 25Hz
+#define DATARATE_10HZ           0b0010 // 10Hz
+#define DATARATE_1HZ            0b0001 // 1Hz
+#define DATARATE_POWERDOWN      0      // Power down
+#define DATARATE_LOWPOWER_1K6HZ 0b1000 // Low power mode (1.6KHz)
+#define DATARATE_LOWPOWER_5KHZ  0b1001 // Low power mode (5KHz) / Normal power mode (1.25KHz)
+#endif
+ 
+ 
+#ifdef LIS331HH
+#define ADDR_ONE    0x30
+#define ADDR_TWO    0x32
+#define AXIS_X      0x00
+#define AXIS_Y      0x01
+#define AXIS_Z      0x02
+#define REG_OUT_X_L 0x28
+#define REG_CTRL1   0x20
+#define REG_CTRL4   0x23
+#define REG_WHOAMI  0x0F
+#define RANGE_2G    0x00
+#define DEVICE_ID   0x33
+ 
+ 
+#define DATARATE_1KHZ           0b11 // 1000Hz
+#define DATARATE_400HZ          0b10 // 400Hz
+#define DATARATE_100HZ          0b01 // 100Hz
+#define DATARATE_50HZ           0b00 // 50Hz
+#define DATARATE_POWERDOWN      0      // Power down
+#define DATARATE_NORMAL_MODE    0b001
+#define DATARATE_LOWPOWER_0.5HZ 0b010
+#define DATARATE_LOWPOWER_1HZ   0b011
+#define DATARATE_LOWPOWER_2HZ   0b100
+#define DATARATE_LOWPOWER_5HZ   0b101
+#define DATARATE_LOWPOWER_10HZ  0b110
+#endif
+ 
+#define PACKET_SIZE 20
+#define QUEUE_SIZE 20
+ 
+ 
 const static char     DEVICE_NAME[] = "LUMBERJACK_NANO";
 static const uint16_t uuid16_list[] = {ButtonService::BUTTON_SERVICE_UUID};
-
-Serial pc(USBTX, USBRX);
-
-
+ 
+struct packetQueue
+{
+    uint16_t size;
+    uint16_t nextPacketToSend;
+    uint16_t nextSampleToSave;
+    uint16_t liveSamples;
+    uint8_t packets[QUEUE_SIZE][PACKET_SIZE];
+};
+ 
+packetQueue pq;
+ 
+void addToQueue(uint8_t* packet) {
+    for (int i = 0; i < PACKET_SIZE; i++) {
+        pq.packets[pq.nextSampleToSave][i] = packet[i];
+    }
+    if (pq.nextPacketToSend == pq.nextSampleToSave && pq.liveSamples > 0) {
+        pq.nextSampleToSave = (pq.nextSampleToSave + 1) % QUEUE_SIZE;
+        pq.nextPacketToSend = (pq.nextPacketToSend + 1) % QUEUE_SIZE;
+    } else {
+        pq.liveSamples += 1;
+        pq.nextSampleToSave = (pq.nextSampleToSave + 1) % QUEUE_SIZE;
+    }
+    return;
+}
+ 
+uint8_t* removeFromQueue() {
+    if (pq.nextSampleToSave != pq.nextPacketToSend && pq.liveSamples > 0) {
+        pq.liveSamples -= 1;
+        uint8_t* old = pq.packets[pq.nextPacketToSend];
+        pq.nextPacketToSend = (pq.nextPacketToSend + 1) % QUEUE_SIZE;
+        return old;
+    } else {
+        return NULL;
+    }
+}
+   
+ 
+//Serial pc(USBTX, USBRX);
+TwoWire Wire = TwoWire(NRF_TWI0);
+ 
 static ButtonService *buttonServicePtr;
 bool isThereAConnection = false;
-
+ 
 void sleep(unsigned int mseconds)
 {
     clock_t goal = mseconds + clock();
     while (goal > clock());
 }
-
+ 
 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
 {
     BLE::Instance().gap().startAdvertising();
     isThereAConnection = false;
 }
-
+ 
 void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
 {
-    pc.printf("Connection recieved!\r\n");
+    //pc.printf("Connection recieved!\r\n");
     isThereAConnection = true;
 }
-
+ 
 /**
  * This function is called when the ble initialization process has failled
  */
@@ -53,7 +167,7 @@
 {
     /* Initialization error handling should go here */
 }
-
+ 
 /**
  * Callback triggered when the ble initialization process has finished
  */
@@ -61,66 +175,282 @@
 {
     BLE&        ble   = params->ble;
     ble_error_t error = params->error;
-
+ 
     if (error != BLE_ERROR_NONE) {
         /* In case of error, forward the error handling to onBleInitError */
         onBleInitError(ble, error);
         return;
     }
-
+ 
     /* Ensure that it is the default instance of BLE */
     if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
         return;
     }
-
+ 
     ble.gap().onDisconnection(disconnectionCallback);
     ble.gap().onConnection(connectionCallback);
-
+ 
     /* Setup primary service */
-    buttonServicePtr = new ButtonService(ble, false /* initial value for button pressed */);
-
+    uint8_t initial_value[20] = {0, 0, 0, 0, 0,
+                                 0, 0, 0, 0, 0,
+                                 0, 0, 0, 0, 0,
+                                 0, 0, 0, 0, 0};
+    buttonServicePtr = new ButtonService(ble, initial_value);
+ 
     /* setup advertising */
     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
     ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
     ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
-    pc.printf("start advertising now \r\n");
+    //pc.printf("start advertising now \r\n");
     ble.gap().startAdvertising();
 }
-
+ 
+void AT24C512_WriteBytes(uint16_t addr, uint8_t *pbuf, uint16_t length, uint16_t i2cAddr)
+{
+    Wire.beginTransmission(i2cAddr);
+    int err = Wire.write( (uint8_t)addr );
+    Wire.write(pbuf, length);
+    if (err != 0) {
+        //pc.printf("error on write write! %d\r\n", err);
+    }
+    uint8_t err8 = Wire.endTransmission();
+    if (err8 != 0) {
+        //pc.printf("error on write end transmission! %d\r\n", err8);
+    }
+}
+ 
+void AT24C512_ReadBytes(uint16_t addr, uint8_t *pbuf, uint16_t length, uint16_t i2cAddr)
+{
+    Wire.beginTransmission(i2cAddr);
+    int err= Wire.write( (uint8_t)addr );
+    if (err != 0) {
+        //pc.printf("error on read write! %d\r\n", err);
+    }
+    uint8_t err8 = Wire.endTransmission();
+    if (err8 != 0) {
+        //pc.printf("error on read end transmission! %d\r\n", err8);
+    }
+       
+    err8 = Wire.requestFrom(i2cAddr+1, length);
+    if (err != 0) {
+        //pc.printf("error on read request from! %d\r\n", err8);
+    }
+    while( Wire.available() > 0 )
+    {
+        *pbuf = Wire.read();
+        pbuf++;
+    }
+}
+ 
+//Set the bit at index 'bit' to 'value' on 'input' and return
+uint8_t setBit(uint8_t input, uint8_t bit, uint8_t value) {
+    uint8_t mask = 1 << bit;
+    input &= ~mask;
+    if (value == 1) {
+        input |= mask;
+    }
+    return input;
+}
+ 
+uint16_t getAxis(uint16_t axis, uint16_t i2cAddr)
+{
+    uint8_t base = REG_OUT_X_L + (2 * axis);
+    uint8_t* low = new uint8_t[1];
+    uint8_t* high = new uint8_t[1];
+    AT24C512_ReadBytes(base, low, 1, i2cAddr);
+    AT24C512_ReadBytes(base + 1, high, 1, i2cAddr);
+    uint16_t res = low[0] | (high[0] << 8);
+    delete[] low;
+    delete[] high;
+    return res;
+}
+ 
+void setRange(uint8_t range, uint16_t i2cAddr) {
+    uint8_t* val = new uint8_t[1];
+    AT24C512_ReadBytes(REG_CTRL4, val, 1, i2cAddr);//get value from the register
+    val[0] &= ~(0b110000); //zero out lowest 2 bits of top 4 bits
+    val[0] |= (range << 4); // write in our new range
+    //pc.printf("REG_CTRL4 after setRange: 0x%x\r\n", *val);
+    AT24C512_WriteBytes(REG_CTRL4, val, 1, i2cAddr);
+    delete[] val;
+}
+ 
+//Set whether we want to use high resolution or not
+void setHighResolution(bool highRes, uint16_t i2cAddr) {
+    uint8_t* val = new uint8_t[1];
+    AT24C512_ReadBytes(REG_CTRL4, val, 1, i2cAddr);//get value from the register
+    uint8_t final;
+    if (highRes) {
+        final = setBit(val[0], 3, 1);
+    } else {
+        final = setBit(val[0], 3, 0);
+    }
+    val[0] = final;
+    //pc.printf("REG_CTRL4 after setHiRes: 0x%x\r\n", *val);
+    AT24C512_WriteBytes(REG_CTRL4, val, 1, i2cAddr);
+    delete[] val;
+}
+ 
+void setAxisStatus(uint8_t axis, bool enable, uint16_t i2cAddr) {
+    uint8_t* current = new uint8_t[1];
+    AT24C512_ReadBytes(REG_CTRL1, current, 1, i2cAddr);//get value from the register
+    uint8_t final;
+    if (enable == 1) {
+        final = setBit(current[0], axis, 1);
+    } else {
+        final = setBit(current[0], axis, 0);
+    }
+    current[0] = final;
+    AT24C512_WriteBytes(REG_CTRL1, current, 1, i2cAddr);
+   
+    AT24C512_ReadBytes(REG_CTRL1, current, 1, i2cAddr);
+    //pc.printf("REG_CTRL1 after setAxisStatus: 0x%x\r\n", *current);
+    delete[] current;
+}
+ 
+void setDataRate(uint8_t dataRate, uint16_t i2cAddr) {
+    uint8_t* val = new uint8_t[1];
+    AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
+    //pc.printf("REG_CTRL1 before data rate set: 0x%x\r\n", *val);
+    val[0] &= 0b11100111; //d
+    val[0] |= (dataRate << 3);
+    AT24C512_WriteBytes(REG_CTRL1, val, 1, i2cAddr);
+   
+    AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
+    //pc.printf("REG_CTRL1 after data rate set: 0x%x\r\n", *val);
+    delete[] val;
+}
+ 
+void setPowerMode(uint8_t powerMode, uint16_t i2cAddr) {
+    uint8_t* val = new uint8_t[1];
+    AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
+    val[0] &= 0b11111;
+    val[0] |= (powerMode << 5);
+    //pc.printf("writing this to REG_CTRL1: 0x%x\r\n", *val);
+    AT24C512_WriteBytes(REG_CTRL1, val, 1, i2cAddr);
+   
+    AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
+    //pc.printf("REG_CTRL1 after power mode set: 0x%x\r\n", *val);
+    delete[] val;
+}
+   
+void setBDU(bool bdu, uint16_t i2cAddr)
+{
+    uint8_t* val = new uint8_t[1];
+    AT24C512_ReadBytes(REG_CTRL4, val, 1, i2cAddr);//get value from the register
+    //pc.printf("REG_CTRL4: 0x%x\r\n", *val);
+    uint8_t final;
+    if (bdu) {
+        final = setBit(val[0], 7, 1);
+    } else {
+        final = setBit(val[0], 7, 0);
+    }
+    val[0] = final;
+    //pc.printf("REG_CTRL4 after setBDU: 0x%x\r\n", *val);
+    AT24C512_WriteBytes(REG_CTRL4, val, 1, i2cAddr);
+    delete[] val;
+}
+ 
+uint16_t getX(uint16_t i2cAddr)
+{
+    return getAxis(AXIS_X, i2cAddr);
+}
+ 
+uint16_t getY(uint16_t i2cAddr)
+{
+    return getAxis(AXIS_Y, i2cAddr);
+}
+ 
+uint16_t getZ(uint16_t i2cAddr)
+{
+    return getAxis(AXIS_Z, i2cAddr);
+}
+ 
 int main(void)
 {
-    pc.printf("entering main!! \r\n");
-    //uint8_t counter = 65; //'A'
-    uint8_t counter[16] = {65, 65, 65, 65, 65,
-                           65, 65, 65, 65, 65,
-                           65, 65, 65, 65, 65,
-                           0};
-
+    //pc.baud(9600);
+    wait(5);
+    //Wire.begin();
+    Wire.begin(SCL, SDA, TWI_FREQUENCY_100K);
+ 
+    //pc.printf("\r\n\r\n\r\nStarting...\r\n");
+ 
+    wait(5);
+   
+    setAxisStatus(AXIS_X, true, ADDR_ONE);
+    setAxisStatus(AXIS_Y, true, ADDR_ONE);
+    setAxisStatus(AXIS_Z, true, ADDR_ONE);
+    setDataRate(DATARATE_400HZ, ADDR_ONE);
+    setPowerMode(DATARATE_NORMAL_MODE, ADDR_ONE);
+    //setHighResolution(true, ADDR_ONE);
+    setBDU(true, ADDR_ONE);
+    //setRange(RANGE_2G, ADDR_ONE);
+   
+   
+    setAxisStatus(AXIS_X, true, ADDR_TWO);
+    setAxisStatus(AXIS_Y, true, ADDR_TWO);
+    setAxisStatus(AXIS_Z, true, ADDR_TWO);
+    setDataRate(DATARATE_400HZ, ADDR_TWO);
+    setPowerMode(DATARATE_NORMAL_MODE, ADDR_TWO);
+    //setHighResolution(true, ADDR_TWO);
+    setBDU(true, ADDR_TWO);
+    //setRange(RANGE_2G, ADDR_TWO);
+ 
+    uint8_t* val = new uint8_t[1];
+    *val = 0x80;
+    AT24C512_WriteBytes(REG_CTRL4, val, 1, ADDR_ONE);
+    AT24C512_WriteBytes(REG_CTRL4, val, 1, ADDR_TWO);
+    AT24C512_ReadBytes(REG_CTRL4, val, 1, ADDR_ONE);
+    //pc.printf("REG_CTRL4, should be 0x80: 0x%x\r\n", *val);
+ 
+    uint8_t* whoami = new uint8_t[1];
+    AT24C512_ReadBytes(REG_WHOAMI, whoami, 1, ADDR_ONE);
+    //pc.printf("REG_WHOAMI should be 0x32: 0x%x\r\n", *whoami);
+    AT24C512_ReadBytes(REG_WHOAMI, whoami, 1, ADDR_TWO);
+    //pc.printf("REG_WHOAMI should be 0x32: 0x%x\r\n", *whoami);
+    AT24C512_ReadBytes(0x1F, whoami, 1, ADDR_ONE);
+ 
+   
     BLE &ble = BLE::Instance();
     ble.init(bleInitComplete);
-    
-    pc.printf("entering spin loop\r\n");
+   
+    //pc.printf("entering spin loop\r\n");
     /* SpinWait for initialization to complete. This is necessary because the
      * BLE object is used in the main loop below. */
     while (ble.hasInitialized()  == false) { /* spin loop */ }
-    pc.printf("leaving spin loop\r\n");
-    
-    while (true) {
-        pc.printf("loop!\r\n");
+    //pc.printf("leaving spin loop\r\n");
+   
+    pq.size = QUEUE_SIZE;
+    pq.nextPacketToSend = 0;
+    pq.nextSampleToSave = 0;
+    pq.liveSamples = 0;
+   
+    while(1)
+    {
+        //pc.printf("Read data from AT24C512\r\n");
+        uint16_t x1 = getX(ADDR_ONE);
+        uint16_t y1 = getY(ADDR_ONE);
+        uint16_t z1 = getZ(ADDR_ONE);
+       
+        uint16_t x2 = getX(ADDR_TWO);
+        uint16_t y2 = getY(ADDR_TWO);
+        uint16_t z2 = getZ(ADDR_TWO);
+       
+        //pc.printf("Accel one: x %d y %d z %d\r\n", (int16_t)x1, (int16_t)y1, (int16_t)z1);
+        //pc.printf("Accel two: x %d y %d z %d\r\n", (int16_t)x2, (int16_t)y2, (int16_t)z2);
+        //pc.printf("\r\n");
+       
         if(isThereAConnection) {
-            //pc.printf("sending Notification: %d\r\n", counter);
-            pc.printf("sending Notification: %d\r\n", counter[0]);
-            buttonServicePtr->updateButtonState(counter);
-            //counter++;
-            for( int i = 0; i < 16; i++ ) {
-                counter[i] = counter[i]+1;
-            }
-        } else {
-            //
+            //pc.printf("sending Notification\r\n");
+            uint8_t values[20] = {(uint8_t)x1, (uint8_t)(x1 >> 8), (uint8_t)y1, (uint8_t)(y1 >> 8), (uint8_t)z1, (uint8_t)(z1 >> 8),
+                                  (uint8_t)x2, (uint8_t)(x2 >> 8), (uint8_t)y2, (uint8_t)(y2 >> 8), (uint8_t)z2, (uint8_t)(z2 >> 8),
+                                  0, 0, 0, 0, 0, 0, 0, 0};
+            buttonServicePtr->updateButtonState(values);
         }
-        //ble.waitForEvent();
-        sleep(1000);
+ 
+        wait(1);
     }
-}
+ 
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wire.cpp	Tue May 02 04:16:38 2017 +0000
@@ -0,0 +1,527 @@
+/*
+
+    Copyright (c) 2014 RedBearLab, 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.
+
+    This library 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 Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public
+    License along with this library; if not, write to the Free Software
+    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+    MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+    IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+    CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+    TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+    SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+*/
+
+#include "wire.h"
+#include "nrf_soc.h"
+#include "nrf_sdm.h"
+
+//extern Serial pc;
+/**********************************************************************
+name :
+function : return: 0--SUCCESS, 1--FAIL
+**********************************************************************/
+bool TwoWire::twi_master_clear_bus(void)
+{
+    bool bus_clear;
+    uint32_t twi_state;
+    uint32_t sck_pin_config;
+    uint32_t sda_pin_config;
+    
+    twi_state = twi->ENABLE;
+    twi->ENABLE = TWI_ENABLE_ENABLE_Disabled << TWI_ENABLE_ENABLE_Pos;
+    
+    sck_pin_config = NRF_GPIO->PIN_CNF[SCL_Pin]; 
+    NRF_GPIO->PIN_CNF[SCL_Pin] = (GPIO_PIN_CNF_SENSE_Disabled << GPIO_PIN_CNF_SENSE_Pos) 
+                                  | (GPIO_PIN_CNF_DRIVE_H0D1     << GPIO_PIN_CNF_DRIVE_Pos) 
+                                  | (GPIO_PIN_CNF_PULL_Pullup    << GPIO_PIN_CNF_PULL_Pos)  
+                                  | (GPIO_PIN_CNF_INPUT_Connect  << GPIO_PIN_CNF_INPUT_Pos) 
+                                  | (GPIO_PIN_CNF_DIR_Output     << GPIO_PIN_CNF_DIR_Pos);
+
+    sda_pin_config = NRF_GPIO->PIN_CNF[SDA_Pin];    
+    NRF_GPIO->PIN_CNF[SDA_Pin] = (GPIO_PIN_CNF_SENSE_Disabled << GPIO_PIN_CNF_SENSE_Pos) 
+                                  | (GPIO_PIN_CNF_DRIVE_H0D1     << GPIO_PIN_CNF_DRIVE_Pos) 
+                                  | (GPIO_PIN_CNF_PULL_Pullup    << GPIO_PIN_CNF_PULL_Pos)  
+                                  | (GPIO_PIN_CNF_INPUT_Connect  << GPIO_PIN_CNF_INPUT_Pos) 
+                                  | (GPIO_PIN_CNF_DIR_Output     << GPIO_PIN_CNF_DIR_Pos);  
+                                  
+    NRF_GPIO->OUTSET = ( 1 << SCL_Pin );
+    NRF_GPIO->OUTSET = ( 1 << SDA_Pin );
+    TWI_DELAY(4);
+    
+    if( ( (NRF_GPIO->IN >> SCL_Pin) & 0X1UL ) && ( (NRF_GPIO->IN >> SDA_Pin) & 0x1UL ) )
+    {
+        bus_clear = 0;
+    }
+    else
+    {
+        uint_fast8_t index;
+        bus_clear = 1;
+        for( index=18; index--;)
+        {
+            NRF_GPIO->OUTCLR = ( 1 << SCL_Pin );
+            TWI_DELAY(4);
+            NRF_GPIO->OUTSET = ( 1 << SDA_Pin );
+            TWI_DELAY(4);
+            
+            if( (NRF_GPIO->IN >> SDA_Pin) & 0x1UL == 1 )
+            {
+                bus_clear = 0;
+                break;
+            }
+        }
+    }
+    
+    NRF_GPIO->PIN_CNF[SCL_Pin] = sck_pin_config;
+    NRF_GPIO->PIN_CNF[SDA_Pin]  = sda_pin_config;
+
+    twi->ENABLE = twi_state;    
+    
+    return bus_clear;
+}
+
+/**********************************************************************
+name :
+function : return: 0--SUCCESS, 1--FAIL
+**********************************************************************/
+bool TwoWire::twi_master_init(void)
+{   
+    uint8_t softdevice_enabled;
+    //uint32_t err_code = NRF_SUCCESS;
+
+    NRF_GPIO->PIN_CNF[SCL_Pin] = (GPIO_PIN_CNF_SENSE_Disabled << GPIO_PIN_CNF_SENSE_Pos)
+                                  | (GPIO_PIN_CNF_DRIVE_H0D1     << GPIO_PIN_CNF_DRIVE_Pos)
+                                  | (GPIO_PIN_CNF_PULL_Pullup    << GPIO_PIN_CNF_PULL_Pos)
+                                  | (GPIO_PIN_CNF_INPUT_Connect  << GPIO_PIN_CNF_INPUT_Pos)
+                                  | (GPIO_PIN_CNF_DIR_Input      << GPIO_PIN_CNF_DIR_Pos);  
+                                  
+    NRF_GPIO->PIN_CNF[SDA_Pin] = (GPIO_PIN_CNF_SENSE_Disabled << GPIO_PIN_CNF_SENSE_Pos)
+                                  | (GPIO_PIN_CNF_DRIVE_H0D1     << GPIO_PIN_CNF_DRIVE_Pos)
+                                  | (GPIO_PIN_CNF_PULL_Pullup    << GPIO_PIN_CNF_PULL_Pos)
+                                  | (GPIO_PIN_CNF_INPUT_Connect  << GPIO_PIN_CNF_INPUT_Pos)
+                                  | (GPIO_PIN_CNF_DIR_Input      << GPIO_PIN_CNF_DIR_Pos);
+                                  
+    twi->EVENTS_RXDREADY = 0;
+    twi->EVENTS_TXDSENT  = 0;
+    twi->PSELSCL = SCL_Pin;
+    twi->PSELSDA = SDA_Pin;
+    twi->FREQUENCY = twi_frequency; //TWI_FREQUENCY_FREQUENCY_K100 << TWI_FREQUENCY_FREQUENCY_Pos;
+    
+    sd_softdevice_is_enabled(&softdevice_enabled);
+    //APP_ERROR_CHECK(err_code);
+    if (softdevice_enabled == 0)
+    {   
+        NRF_PPI->CH[7].EEP  =   (uint32_t)&twi->EVENTS_BB;
+        NRF_PPI->CH[7].TEP  =   (uint32_t)&twi->TASKS_SUSPEND;
+        NRF_PPI->CHEN &= ~(1 << 7);
+    }
+    else
+    {
+        sd_ppi_channel_assign(7, &twi->EVENTS_BB, &twi->TASKS_SUSPEND);
+        //APP_ERROR_CHECK(err_code);
+        sd_ppi_channel_enable_clr(1 << 7);
+        //APP_ERROR_CHECK(err_code);
+    }
+    
+    twi->ENABLE = TWI_ENABLE_ENABLE_Enabled << TWI_ENABLE_ENABLE_Pos;  
+    
+    return twi_master_clear_bus();
+}
+/**********************************************************************
+name :
+function : return: 0--SUCCESS, 1--FAIL
+**********************************************************************/
+uint8_t TwoWire::twi_master_write(uint8_t *data, uint8_t data_length, uint8_t issue_stop_condition)
+{
+    uint32_t timeout = MAX_TIMEOUT_LOOPS;
+
+    if(data_length == 0)
+    {
+        return 1;
+    }
+    twi->TXD = *data++;
+    twi->TASKS_STARTTX = 1;
+    while(1)
+    {   
+        while( (twi->EVENTS_TXDSENT == 0) && (--timeout) );//&& (twi->EVENTS_ERROR == 0) );
+        
+        if( 0 == timeout )//|| twi->EVENTS_ERROR != 0)
+        {   
+            twi->EVENTS_ERROR = 0;
+            twi->ENABLE     = TWI_ENABLE_ENABLE_Disabled << TWI_ENABLE_ENABLE_Pos; 
+            twi->POWER        = 0; 
+            TWI_DELAY(5);
+            twi->POWER        = 1; 
+            twi->ENABLE     = TWI_ENABLE_ENABLE_Enabled << TWI_ENABLE_ENABLE_Pos;
+            
+            twi_master_init();
+            return 1;
+        }
+        twi->EVENTS_TXDSENT = 0;
+        if( --data_length == 0)
+        {   
+            break;
+        }
+        
+        twi->TXD = *data++;
+    }
+    if(issue_stop_condition)
+    {   
+        twi->EVENTS_STOPPED = 0;
+        twi->TASKS_STOP     = 1;
+        while(twi->EVENTS_STOPPED == 0)
+        {
+            //do nothing, wait for stop sequence is sent
+        }       
+    }
+    return 0;
+}
+/**********************************************************************
+name :
+function : return: 0--SUCCESS, 1--FAIL
+**********************************************************************/
+uint8_t TwoWire::twi_master_read(uint8_t *data, uint8_t data_length, uint8_t issue_stop_condition)
+{   
+    uint8_t softdevice_enabled;
+    uint32_t timeout = MAX_TIMEOUT_LOOPS;// err_code = NRF_SUCCESS;
+
+    sd_softdevice_is_enabled(&softdevice_enabled);
+    //APP_ERROR_CHECK(err_code);
+    if( 0 == data_length )
+    {
+        return 1;
+    }
+    else if( 1== data_length )//&& issue_stop_condition == 1)
+    {
+        if (softdevice_enabled == 0)
+        {   
+            NRF_PPI->CH[7].EEP = (uint32_t)&twi->EVENTS_BB;
+            NRF_PPI->CH[7].TEP = (uint32_t)&twi->TASKS_STOP;
+            NRF_PPI->CHEN |= (1 << 7);
+        }
+        else
+        {
+            sd_ppi_channel_assign(7, &twi->EVENTS_BB, &twi->TASKS_STOP);
+            //APP_ERROR_CHECK(err_code);
+            sd_ppi_channel_enable_set(1 << 7);
+            //APP_ERROR_CHECK(err_code);
+        }
+    }
+    else
+    {   
+        if (softdevice_enabled == 0)
+        {   
+            NRF_PPI->CH[7].EEP = (uint32_t)&twi->EVENTS_BB;
+            NRF_PPI->CH[7].TEP = (uint32_t)&twi->TASKS_SUSPEND;
+            NRF_PPI->CHEN |= (1 << 7);
+        }
+        else
+        {
+            sd_ppi_channel_assign(7, &twi->EVENTS_BB, &twi->TASKS_SUSPEND);
+            //APP_ERROR_CHECK(err_code);
+            sd_ppi_channel_enable_set(1 << 7);
+            //APP_ERROR_CHECK(err_code);          
+        }
+    }
+    
+    twi->EVENTS_RXDREADY = 0;
+    twi->TASKS_STARTRX = 1;
+    
+    while(1)
+    {   
+        while( twi->EVENTS_RXDREADY == 0 && (--timeout) )
+        {
+            //do nothing, just wait
+        }
+    
+        if( timeout == 0 )
+        {   
+            twi->EVENTS_ERROR = 0;
+            twi->ENABLE       = TWI_ENABLE_ENABLE_Disabled << TWI_ENABLE_ENABLE_Pos; 
+            twi->POWER        = 0; 
+            TWI_DELAY(5);
+            twi->POWER        = 1; 
+            twi->ENABLE       = TWI_ENABLE_ENABLE_Enabled << TWI_ENABLE_ENABLE_Pos;
+    
+            twi_master_init();          
+          
+            return 1;           
+        }
+        
+        twi->EVENTS_RXDREADY = 0;
+        *data++ = twi->RXD;
+    
+        if( --data_length == 1 )
+        {   
+            if (softdevice_enabled == 0)
+            {   
+                //NRF_PPI->CH[7].EEP = (uint32_t)&twi->EVENTS_BB;
+                NRF_PPI->CH[7].TEP = (uint32_t)&twi->TASKS_STOP;
+            }
+            else
+            {
+                sd_ppi_channel_assign(7, &twi->EVENTS_BB, &twi->TASKS_STOP);
+                //APP_ERROR_CHECK(err_code);
+            }
+        }
+        
+        if( data_length == 0 )
+        {   
+            twi->TASKS_STOP = 1;
+            break;
+        }
+        TWI_DELAY(20);
+        twi->TASKS_RESUME = 1;
+    }
+    while( twi->EVENTS_STOPPED == 0 )
+    {
+        //do nothing
+    }
+
+    twi->EVENTS_STOPPED = 0;
+    
+    if (softdevice_enabled == 0)
+    {       
+        NRF_PPI->CHEN &= ~(1 << 7);
+    }
+    else
+    {
+        sd_ppi_channel_enable_clr( 1 << 7 );
+        //APP_ERROR_CHECK(err_code);
+    }
+    return 0;
+}
+
+/**********************************************************************
+name :
+function : 
+**********************************************************************/
+TwoWire::TwoWire(NRF_TWI_Type *twi_use)
+{
+    twi = twi_use;
+    
+    RX_BufferIndex  = 0;
+    RX_BufferLength = 0;
+    TX_BufferIndex  = 0;
+    TX_BufferLength = 0;
+    
+    Transform_Addr  = 0;
+    
+    twi_status      = UNINITIALIZED;
+}
+/**********************************************************************
+name :
+function : 
+**********************************************************************/
+void TwoWire::begin(uint32_t scl, uint32_t sda, uint8_t speed)
+{     
+    if( speed == 2 )
+    {
+        twi_frequency = TWI_FREQUENCY_FREQUENCY_K400;
+    }
+    else if( speed == 1 )
+    {
+        twi_frequency = TWI_FREQUENCY_FREQUENCY_K250;
+    }
+    else
+    {
+        twi_frequency = TWI_FREQUENCY_FREQUENCY_K100;   
+    }
+    
+    SCL_Pin = scl;
+    SDA_Pin = sda;
+    twi_master_init();
+
+    twi_status = MASTER_IDLE;   
+}
+/**********************************************************************
+name :
+function : 
+**********************************************************************/
+void TwoWire::begin()
+{
+    begin(TWI_SCL, TWI_SDA, 0);
+}
+/**********************************************************************
+name :
+function : 
+**********************************************************************/
+void TwoWire::beginTransmission( uint8_t address )
+{
+    Transform_Addr = address;
+    TX_BufferIndex = 0;
+    twi_status = MASTER_SEND;
+}
+/**********************************************************************
+name :
+function : 
+**********************************************************************/
+void TwoWire::beginTransmission( int address )
+{
+    beginTransmission( (uint8_t)address );
+}
+/**********************************************************************
+name :
+function :  return: 0--SUCCESS, 1--FAIL
+**********************************************************************/
+uint8_t TwoWire::endTransmission( uint8_t stop)
+{
+    uint8_t twi_flag = 1;
+
+    if(TX_BufferLength > 0 && !(twi_master_clear_bus()) )
+    {   
+        twi->ADDRESS = ( Transform_Addr >> 1);
+        twi_flag = twi_master_write(TX_Buffer, TX_BufferLength, stop);
+    }
+    
+    TX_BufferLength = 0;
+    twi_status = MASTER_IDLE;
+
+    return twi_flag;
+}
+/**********************************************************************
+name :
+function : return: 0--SUCCESS, 1--FAIL
+**********************************************************************/
+uint8_t TwoWire::endTransmission(void)
+{
+    uint8_t twi_flag;
+    
+    twi_flag = endTransmission(1);
+    
+    return twi_flag;
+}
+/**********************************************************************
+name :
+function : return: 0--SUCCESS, -1--FAIL,
+**********************************************************************/
+int TwoWire::write(uint8_t data)
+{
+    if(twi_status == MASTER_SEND)
+    {   
+        if(TX_BufferLength >= BUFF_MAX_LENGTH)
+        {
+            return -1;
+        }
+        TX_Buffer[TX_BufferLength++] = data;
+        return 0;
+    }
+    else
+    {
+        return -1;
+    }
+}
+/**********************************************************************
+name :
+function : return: -1--FAIL, else--the length of data stored
+**********************************************************************/
+int TwoWire::write(const uint8_t *data, size_t quantity )
+{
+    if( twi_status == MASTER_SEND )
+    {
+        for( size_t i=0; i<quantity; ++i )
+        {   
+            if(TX_BufferLength >= BUFF_MAX_LENGTH)
+            {
+                return i;
+            }
+            TX_Buffer[TX_BufferLength++] = data[i];
+        }
+    }
+    else
+    {
+        return -1;
+    }
+    
+    return quantity;
+}
+/**********************************************************************
+name :
+function : return: 0--SUCCESS, 1--FAIL
+**********************************************************************/
+uint8_t TwoWire::requestFrom(uint8_t addr, uint8_t quantity, uint8_t stop)
+{
+    uint8_t twi_flag = 1;
+    
+    if(quantity > BUFF_MAX_LENGTH)
+    {   
+        quantity = BUFF_MAX_LENGTH;
+    }
+    if(quantity > 0 && !(twi_master_clear_bus()) )
+    {   
+        twi->ADDRESS = ( addr >> 1 );
+        twi_flag = twi_master_read(RX_Buffer, quantity, stop);
+    }
+    RX_BufferIndex = 0;
+    RX_BufferLength = quantity;
+    
+    return twi_flag;
+}
+/**********************************************************************
+name :
+function : 
+**********************************************************************/
+uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity)
+{
+    return requestFrom((uint8_t) address, (uint8_t) quantity, (uint8_t) true);
+}
+/**********************************************************************
+name :
+function : 
+**********************************************************************/
+uint8_t TwoWire::requestFrom(int address, int quantity) 
+{
+    return requestFrom((uint8_t) address, (uint8_t) quantity, (uint8_t) true);
+}
+/**********************************************************************
+name :
+function : 
+**********************************************************************/
+uint8_t TwoWire::requestFrom(int address, int quantity, int sendStop) 
+{
+    return requestFrom((uint8_t) address, (uint8_t) quantity, (uint8_t) sendStop);
+}
+/**********************************************************************
+name :
+function : return:-1--FAIL, else:the length of data that could be read
+**********************************************************************/
+int TwoWire::available(void)
+{
+    if(RX_BufferIndex <= RX_BufferLength)
+    {
+        return (RX_BufferLength - RX_BufferIndex);
+    }
+    else
+    {
+        return -1;
+    }
+}
+/**********************************************************************
+name :
+function : 
+**********************************************************************/
+int TwoWire::read(void)
+{
+    if(RX_BufferIndex < RX_BufferLength)
+    {
+        return RX_Buffer[RX_BufferIndex++];
+    }
+    else
+    {
+        return -1;
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wire.h	Tue May 02 04:16:38 2017 +0000
@@ -0,0 +1,103 @@
+/*
+
+    Copyright (c) 2014 RedBearLab, 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.
+
+    This library 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 Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public
+    License along with this library; if not, write to the Free Software
+    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+    MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+    IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+    CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+    TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+    SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+*/
+
+#ifndef _WIRE_H_
+#define _WIRE_H_
+
+#include "mbed.h"
+
+#define TWI_DELAY(x)                    wait_us(x);
+
+#define BUFF_MAX_LENGTH                 128
+
+#define MAX_TIMEOUT_LOOPS               (20000UL)
+
+#define TWI_FREQUENCY_100K              0 
+#define TWI_FREQUENCY_250K              1 
+#define TWI_FREQUENCY_400K              2 
+
+#define TWI_SCL                         28
+#define TWI_SDA                         29
+
+
+class TwoWire
+{  
+    public :
+        TwoWire(NRF_TWI_Type *twi_use);
+        void begin();
+        void begin(uint32_t scl_pin, uint32_t sda_pin, uint8_t speed);
+        void beginTransmission(uint8_t);
+        void beginTransmission(int);
+        uint8_t endTransmission(void);
+        uint8_t endTransmission(uint8_t);
+        uint8_t requestFrom(uint8_t, uint8_t);
+        uint8_t requestFrom(uint8_t, uint8_t, uint8_t);
+        uint8_t requestFrom(int, int);
+        uint8_t requestFrom(int, int, int);
+        int write(uint8_t);
+        int write(const uint8_t *, size_t);
+        int available(void);
+        int read(void);  
+        
+     private : 
+        uint8_t RX_Buffer[BUFF_MAX_LENGTH];
+        uint8_t RX_BufferIndex;
+        uint8_t RX_BufferLength;
+        
+        uint8_t TX_Buffer[BUFF_MAX_LENGTH];
+        uint8_t TX_BufferIndex;
+        uint8_t TX_BufferLength;
+        
+        NRF_TWI_Type *twi;
+        
+        uint8_t PPI_channel;
+        uint8_t Transform_Addr;
+        
+        uint32_t SDA_Pin;
+        uint32_t SCL_Pin;
+        
+        uint32_t twi_frequency;
+
+        enum TwoWireStatus {
+            UNINITIALIZED,
+            MASTER_IDLE,
+            MASTER_SEND,
+            MASTER_RECV,
+            SLAVE_IDLE,
+            SLAVE_RECV,
+            SLAVE_SEND
+        };
+        TwoWireStatus twi_status;
+        
+        bool twi_master_clear_bus(void);
+        bool twi_master_init(void);
+        uint8_t twi_master_read(uint8_t *data, uint8_t data_length, uint8_t issue_stop_condition);
+        uint8_t twi_master_write(uint8_t *data, uint8_t data_length, uint8_t issue_stop_condition);
+};
+
+#endif
\ No newline at end of file