Revision 24:baa43caa2f3d, committed 2017-06-03

Comitter:

AidasL

Date:

Sat Jun 03 21:14:42 2017 +0000

Parent:

23:708cc5ef2604

Commit message:

Saturday updates

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gloves.cpp	Sat Jun 03 21:14:42 2017 +0000
@@ -0,0 +1,160 @@
+#include "mbed.h"
+#include "ble/BLE.h"
+#include "sensors.h"
+
+
+
+DigitalOut led(LED1);
+uint16_t customServiceUUID  = 0xABCD;
+uint16_t readCharUUID       = 0xABCE;
+uint16_t writeCharUUID      = 0xABCF;
+
+static volatile bool triggerSensorPolling = false;
+
+//Serial pc(USBTX, USBRX);
+//    pc.baud(9600);
+//    pc.printf("IIC Demo Start \r\n");
+// For debugging into PC terminal
+
+
+
+const static char DEVICE_NAME[]        = "WorkGloves";     // change this
+static const uint16_t uuid16_list[]        = {0xFFFF}; //Custom UUID, FFFF is reserved for development
+
+/* Set Up custom Characteristics */
+static uint16_t readValue[26] = {0x55, 0x33};
+ReadOnlyArrayGattCharacteristic<uint16_t, sizeof(readValue)> readChar(readCharUUID, readValue);
+
+static uint8_t writeValue[8] = {0x00};
+WriteOnlyArrayGattCharacteristic<uint8_t, sizeof(writeValue)> writeChar(writeCharUUID, writeValue);
+
+
+/* Set up custom service */
+GattCharacteristic *characteristics[] = {&readChar, &writeChar};
+GattService customService(customServiceUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic *));
+
+void datapackettoarray(Datapacket todump, uint16_t *dest)
+{       //Need a uint16_t array[26] input for correct functioning
+        uint8_t j,k;
+
+        for (j=0; j<12; j++)
+        {
+                *dest=todump.flex[j];
+                dest++;
+        }
+        for(k=0; k<7; k++)
+        {
+                *dest=todump.acc[0][k];
+                dest++;
+        }
+
+        for(k=0; k<7; j++, k++)
+        {
+                *dest=todump.acc[1][k];
+                dest++;
+        }
+
+
+
+}
+
+
+/*
+ *  Restart advertising when phone app disconnects
+ */
+void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *)
+{
+        BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising();
+}
+
+void periodicCallback(void)
+{
+        led = !led; /* Do blinky on LED1 while we're waiting for BLE events */
+
+        /* Note that the periodicCallback() executes in interrupt context, so it is safer to do
+         * heavy-weight sensor polling from the main thread. */
+        triggerSensorPolling = true;
+}
+
+/*
+ *  Handle writes to writeCharacteristic for screen data from phone
+ */
+void writeCharCallback(const GattWriteCallbackParams *params)
+{
+        /* Check to see what characteristic was written, by handle */
+        if(params->handle == writeChar.getValueHandle()) {
+                /* Update the readChar with the value of writeChar */
+//                BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(readChar.getValueHandle(), params->data, params->len);
+        }
+}
+/*
+ * Initialization callback
+ */
+void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
+{
+        BLE &ble          = params->ble;
+        ble_error_t error = params->error;
+
+        if (error != BLE_ERROR_NONE) {
+                return;
+        }
+
+        ble.gap().onDisconnection(disconnectionCallback);
+        ble.gattServer().onDataWritten(writeCharCallback); // TODO: update to flush screen !!!
+
+        /* Setup advertising */
+        ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE); // BLE only, no classic BT
+        ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); // advertising type
+        ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME)); // add name
+        ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list)); // UUID's broadcast in advertising packet
+        ble.gap().setAdvertisingInterval(250); // 250ms.
+
+        /* Add our custom service */
+        ble.addService(customService);
+
+        /* Start advertising */
+        ble.gap().startAdvertising();
+}
+
+/*
+ *  Main loop
+ */
+int main(void)
+{
+        /* initialize stuff */
+//        printf("\n\r********* Starting Main Loop *********\n\r");
+        Datapacket readings;
+        setupI2C();
+        setupacc(ACC_LEFT);
+        setupacc(ACC_RIGHT);
+        led = 1;
+        Ticker ticker;
+        ticker.attach(periodicCallback, 0.2); // blink LED every 0.2 second
+        BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
+        ble.init(bleInitComplete);
+        uint16_t array[26];
+//        uint8_t data[52]={0,0,0,0,0,0,0,0,0,0,0,0,0};
+        /* 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 */ }
+//        uint16_t data[24] = {0x11, 0x22};
+        /* Infinite loop waiting for BLE interrupt events */
+        while (1) {
+                // check for trigger// from periodicCallback()
+                if (triggerSensorPolling && ble.getGapState().connected) {
+                        triggerSensorPolling = false;
+//                        data[0] = data[0]+1;//
+                        readflexs(&readings);
+                        readacc(&readings, ACC_LEFT );
+                        readacc(&readings, ACC_RIGHT );
+                        datapackettoarray(readings, array);
+                        BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(readChar.getValueHandle(), (uint8_t*)array, 52 );
+//                      BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(readChar.getValueHandle(), (uint8_t*)data, 52 );
+
+
+                }
+                else {
+                        ble.waitForEvent(); // low power wait for event
+                }
+        }
+}

--- a/main.cpp	Wed May 31 22:13:36 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,140 +0,0 @@
-#include "mbed.h"
-#include "ble/BLE.h"
-#include "sensors.h"
-
-
-
-DigitalOut led(LED1);
-uint16_t customServiceUUID  = 0xABCD;
-uint16_t readCharUUID       = 0xABCE;
-uint16_t writeCharUUID      = 0xABCF;
-
-static volatile bool triggerSensorPolling = false;
-
-
-const static char DEVICE_NAME[]        = "WorkGloves";     // change this
-static const uint16_t uuid16_list[]        = {0xFFFF}; //Custom UUID, FFFF is reserved for development
-
-/* Set Up custom Characteristics */
-static uint16_t readValue[24] = {0x55, 0x33};
-ReadOnlyArrayGattCharacteristic<uint16_t, sizeof(readValue)> readChar(readCharUUID, readValue);
-
-static uint8_t writeValue[8] = {0x00};
-WriteOnlyArrayGattCharacteristic<uint8_t, sizeof(writeValue)> writeChar(writeCharUUID, writeValue);
-
-
-/* Set up custom service */
-GattCharacteristic *characteristics[] = {&readChar, &writeChar};
-GattService customService(customServiceUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic *));
-
-void dump(Datapacket todump)
-{
-        uint8_t j;
-        for (j=0; j<12; j++)
-        {
-                readValue[j]=todump.flex[j];
-        }
-        for(j=12; j<18; j++)
-        {
-                readValue[j]=todump.acc[0][j%6];
-        }
-
-        for(j=18; j<24; j++)
-        {
-                readValue[j]=todump.acc[1][j%6];
-        }
-
-
-
-}
-
-
-/*
- *  Restart advertising when phone app disconnects
- */
-void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *)
-{
-        BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising();
-}
-
-void periodicCallback(void)
-{
-        led = !led; /* Do blinky on LED1 while we're waiting for BLE events */
-
-        /* Note that the periodicCallback() executes in interrupt context, so it is safer to do
-         * heavy-weight sensor polling from the main thread. */
-        triggerSensorPolling = true;
-}
-
-/*
- *  Handle writes to writeCharacteristic for screen data from phone
- */
-void writeCharCallback(const GattWriteCallbackParams *params)
-{
-        /* Check to see what characteristic was written, by handle */
-        if(params->handle == writeChar.getValueHandle()) {
-                /* Update the readChar with the value of writeChar */
-//                BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(readChar.getValueHandle(), params->data, params->len);
-        }
-}
-/*
- * Initialization callback
- */
-void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
-{
-        BLE &ble          = params->ble;
-        ble_error_t error = params->error;
-
-        if (error != BLE_ERROR_NONE) {
-                return;
-        }
-
-        ble.gap().onDisconnection(disconnectionCallback);
-        ble.gattServer().onDataWritten(writeCharCallback); // TODO: update to flush screen !!!
-
-        /* Setup advertising */
-        ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE); // BLE only, no classic BT
-        ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); // advertising type
-        ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME)); // add name
-        ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list)); // UUID's broadcast in advertising packet
-        ble.gap().setAdvertisingInterval(500); // 500ms.
-
-        /* Add our custom service */
-        ble.addService(customService);
-
-        /* Start advertising */
-        ble.gap().startAdvertising();
-}
-
-/*
- *  Main loop
- */
-int main(void)
-{
-        /* initialize stuff */
-//        printf("\n\r********* Starting Main Loop *********\n\r");
-
-        led = 1;
-        Ticker ticker;
-        ticker.attach(periodicCallback, 0.1); // blink LED every 0.5 second
-        BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
-        ble.init(bleInitComplete);
-
-        /* 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 */ }
-        uint16_t data[24] = {0x11, 0x22};
-        /* Infinite loop waiting for BLE interrupt events */
-        while (1) {
-                // check for trigger// from periodicCallback()
-                if (triggerSensorPolling && ble.getGapState().connected) {
-                        triggerSensorPolling = false;
-                        data[1] = data[1]+1;
-                        BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(readChar.getValueHandle(), (uint8_t*)data , 48 );
-                        
-                        }
-                 else {
-                        ble.waitForEvent(); // low power wait for event
-                }
-        }
-}

--- a/sensors.cpp	Wed May 31 22:13:36 2017 +0000
+++ b/sensors.cpp	Sat Jun 03 21:14:42 2017 +0000
@@ -1,12 +1,203 @@
 #include "sensors.h"
 #include "mbed.h"
 #include "wire.h"
- 
+
 #define BLE_Nano
- 
-#ifdef BLE_Nano
-#define SCL         7  
-#define SDA         6
-#endif
- 
-#define DEV_ADDR    0xA0
\ No newline at end of file
+//
+// #ifdef BLE_Nano
+// #define SCL         7
+// #define SDA         6
+// #endif
+
+// #ifdef BLE_Nano
+// #define SCL         8 // D2
+// #define SDA         10 // D3
+// #endif
+
+
+// #define ACC2 0x69 //    0b01101001
+
+
+
+
+AnalogIn analogL(P0_4); // Analog senson input polling A3; Left In
+AnalogIn analogR(P0_5); // Analog senson input polling A4; Right In
+
+DigitalOut flexA(P0_7); // Left Hand Finger Selectors:  A
+DigitalOut flexB(P0_6); // B
+DigitalOut flexC(P0_15); //  C
+
+// DigitalOut flexRA(P0_28); //  Right hand finder selectors : A
+// DigitalOut flexRB(P0_29); //  B
+// DigitalOut flexRC(P0_11); //  C
+
+// TwoWire Wire = TwoWire(NRF_TWI0);
+I2C i2c(p8, p10);
+
+
+
+void WriteBytes(uint8_t addr, uint8_t *pbuf, uint16_t length, uint8_t DEV_ADDR)
+{
+        // Wire.beginTransmission(DEV_ADDR<<1);
+        // Wire.write( (uint8_t)addr);
+        // Wire.write(pbuf, length);
+        // Wire.endTransmission();
+        i2c.start();
+        i2c.write( DEV_ADDR <<1 );
+        i2c.write( addr );
+        for (int i =0; i<length; i++) {
+                i2c.write( *pbuf );
+                pbuf++;
+        }
+        i2c.stop();
+}
+
+void ReadBytes(uint8_t addr, uint8_t *pbuf, uint16_t length, uint8_t DEV_ADDR)
+{
+        // Wire.beginTransmission((DEV_ADDR<<1)+1);
+        // Wire.write( (uint8_t)addr);
+        // Wire.endTransmission();
+        // Wire.requestFrom(DEV_ADDR, length);
+        // while( Wire.available() > 0 )
+        i2c.start();
+        i2c.write( (DEV_ADDR <<1) +1);
+        i2c.write( addr );
+        i2c.read( addr, (char*) pbuf, length);
+        // {
+        //         *pbuf = Wire.read();
+        //         pbuf++;
+        // }
+}
+
+void WriteByte(uint8_t addr, uint8_t buf, uint8_t DEV_ADDR)
+{
+        // Wire.beginTransmission(DEV_ADDR<<1);
+        // Wire.write( (uint8_t)addr );
+        // Wire.write((uint8_t)buf);
+        // Wire.endTransmission();
+        i2c.start();
+        i2c.write( DEV_ADDR <<1 );
+        i2c.write( addr );
+        i2c.write( buf );
+        i2c.stop();
+}
+
+//
+//void Clear(){
+//  Wire.twi_master_clear_bus()
+//}
+
+
+void setupI2C(void){
+        // Wire.begin(SCL, SDA, TWI_FREQUENCY_100K);
+}
+
+void setupacc(uint8_t ADDRESS){
+        WriteByte(SMPRT_DIV, 0x13, ADDRESS); // Set rate divider to 19, so sample rate is 50hz
+        WriteByte(CONFIG, 0x05, ADDRESS); // Set the digital low pass filter to 10Hz Cutoff for both Gyro and acc
+        WriteByte(GYRO_CONFIG, 0x08, ADDRESS); // Set the gyro range to \pm 500 degrees/sec
+        WriteByte(ACCEL_CONFIG, 0x00, ADDRESS); // Set the acc range to \pm 2g
+        WriteByte(FIFO_EN, 0x00, ADDRESS); // Disable FIFO
+        WriteByte(I2C_MST_CTRL, 0x00, ADDRESS); // Disabling external I2C
+        WriteByte(INT_PIN_CFG, 0x30, ADDRESS); // Active high, push-pull, high until cleared, cleared on any read,
+        WriteByte(INT_ENABLE, 0x00, ADDRESS); // DataRDY is the last bit if needed
+        WriteByte(USER_CTRL, 0x00, ADDRESS); // No FIFO or I2C Master set
+        WriteByte(PWR_MGMT_1, 0x00, ADDRESS); // TODO: Sleepmode is here; Disabled now; Cycle -> LP_WAKE_CTRL
+        WriteByte(PWR_MGMT_2, 0x00, ADDRESS); // No lowpower mode, all sensors active
+
+}
+
+void readacc(Datapacket *data, uint8_t accadr){
+        uint8_t rorl = 1; //
+        if(accadr == ACC_RIGHT) {rorl=0; };
+        uint8_t buffer[14]={0};
+        uint16_t temp;
+        ReadBytes(0x3B, buffer, 14, accadr);
+        temp = ((buffer[0]<<8)+buffer[1]);
+        for (int i=0; i<7; i++) {
+
+                temp=0;
+                temp = ((buffer[2*i]<<8)+buffer[2*i + 1]);
+                data->acc[rorl][i]= (uint16_t) temp;
+        }
+        // Acc data structure: ACC X, Y, Z; Temperature measurement; GYRO X, Y, Z;
+
+}
+
+
+void readflexs(Datapacket *data){
+
+        // Order of the flex sensors:
+        // 0 - right thumb, 4 - right pinky, 5 - R elbow;
+        // 6 - left thumb, 10 left pinky, 11 - L elbow;
+        // Read right hand in:
+
+        // AnalogIn analogL(P0_4); // Analog senson input polling A3; Left In
+        // AnalogIn analogR(P0_5); // Analog senson input polling A4; Right In
+        //
+        // DigitalOut flexLA(P0_7); // Left Hand Finger Selectors:  A
+        // DigitalOut flexLB(P0_6); // B
+        // DigitalOut flexLC(P0_15); //  C
+        //
+        // DigitalOut flexRA(P0_28); //  Right hand finder selectors : A
+        // DigitalOut flexRB(P0_29); //  B
+        // DigitalOut flexRC(P0_11); //  C
+        // Address is CBA
+        // Read thumbs:
+        flexC = 0;
+        flexB = 0;
+        flexA = 0;
+        data->flex[6] = (uint16_t) (analogL.read()*65535);
+        // flexRC = 0;
+        // flexRB = 0;
+        // flexRA = 0;
+        data->flex[0] = (uint16_t) (analogR.read()*65535);
+
+        flexC = 0;
+        flexB = 0;
+        flexA = 1;
+        data->flex[7] = (uint16_t) (analogL.read()*65535);
+        // flexRC = 0;
+        // flexRB = 0;
+        // flexRA = 1;
+        data->flex[1] = (uint16_t) (analogR.read()*65535);
+
+        flexC = 0;
+        flexB = 1;
+        flexA = 0;
+        data->flex[8] = (uint16_t) (analogL.read()*65535);
+        // flexRC = 0;
+        // flexRB = 1;
+        // flexRA = 0;
+        data->flex[2] = (uint16_t) (analogR.read()*65535);
+
+        flexC = 0;
+        flexB = 1;
+        flexA = 1;
+        data->flex[9] = (uint16_t) (analogL.read()*65535);
+        // flexRC = 0;
+        // flexRB = 1;
+        // flexRA = 1;
+        data->flex[3] = (uint16_t) (analogR.read()*65535);
+
+        flexC = 1;
+        flexB = 0;
+        flexA = 0;
+        data->flex[10] = (uint16_t) (analogL.read()*65535);
+        // flexRC = 1;
+        // flexRB = 0;
+        // flexRA = 0;
+        data->flex[4] = (uint16_t) (analogR.read()*65535);
+
+
+        flexC = 1;
+        flexB = 0;
+        flexA = 1;
+        data->flex[11] = (uint16_t) (analogL.read()*65535);
+        // flexRC = 1;
+        // flexRB = 0;
+        // flexRA = 1;
+        data->flex[5] = (uint16_t) (analogR.read()*65535);
+
+
+}

--- a/sensors.h	Wed May 31 22:13:36 2017 +0000
+++ b/sensors.h	Sat Jun 03 21:14:42 2017 +0000
@@ -3,29 +3,79 @@
 #ifndef SENSORS_H
 #define SENSORS_H
 
+
+// Register names, can look up in the datasheet
+
+#define SMPRT_DIV    0x19 //
+#define CONFIG       0x1A //
+#define GYRO_CONFIG  0x1B //
+#define ACCEL_CONFIG 0x1C //
+#define FIFO_EN      0x23 //
+#define I2C_MST_CTRL 0x24 //
+#define INT_PIN_CFG  0x37 //
+#define INT_ENABLE   0x38 //
+
+#define ACCX158      0x3B //
+#define ACCX70       0x3C //
+#define ACCY158      0x3D //
+#define ACCY70       0x3E //
+#define ACCZ158      0x3F //
+#define ACCZ70       0x40 //
+#define TEMP158      0x41 //
+#define TEMP70       0x42 // Temperature in C = ((signed int16_t) value)/340   + 36.53
+#define GYROX158     0x43 //
+#define GYROX70      0x44 //
+#define GYROY158     0x45 //
+#define GYROY70      0x46 //
+#define GYROZ158     0x47 //
+#define GYROZ70      0x48 //
+
+#define USER_CTRL    0x6A //
+#define PWR_MGMT_1   0x6B //
+#define PWR_MGMT_2   0x6C //
+#define WHO_AM_I     0x75 // Check device number to see if it works
+
+const uint8_t ACC_LEFT  = 0x68; //    0b01101000
+const uint8_t ACC_RIGHT = 0x69; //    0b01101001
+
+
+
+
 typedef struct {
-    
-    uint16_t flex[12];
-    uint16_t acc[2][6];    
-    
-    } Datapacket;
-    
+
+        uint16_t flex[12];
+        // Order of the flex sensors:
+        // 0 - right thumb, 4 - right pinky, 5 - R elbow;
+        // 6 - left thumb, 10 left pinky, 11 - L elbow;
+        uint16_t acc[2][7];
+        // 7th value is the temperature value
+        uint16_t temp;
+
+
+} Datapacket;
+
 typedef struct {
-    
-    uint8_t pixels[8];
-    
-    } Screen;
-    
-void readflex(Datapacket data);
+
+        uint8_t pixels[8];
+
+} Screen;
 
 
 void readacc(Datapacket data);
 
-void setup(void);
+void setupacc(uint8_t ADDRESS);
 
+void WriteBytes(uint8_t addr, uint8_t *pbuf, uint16_t length, uint8_t DEV_ADDR);
+
+void WriteByte(uint8_t addr, uint8_t buf, uint8_t DEV_ADDR);
 
+void ReadBytes(uint8_t addr, uint8_t *pbuf, uint16_t length, uint8_t DEV_ADDR);
 
-    
+void readflexs(Datapacket *data);
+
+void readacc(Datapacket *data, uint8_t accadr);
+
+void setupI2C(void);
 
 
 

--- a/wire.cpp	Wed May 31 22:13:36 2017 +0000
+++ b/wire.cpp	Sat Jun 03 21:14:42 2017 +0000
@@ -484,10 +484,10 @@
 name :
 function : 
 **********************************************************************/
-uint8_t TwoWire::requestFrom(int address, int quantity) 
-{
-    return requestFrom((uint8_t) address, (uint8_t) quantity, (uint8_t) true);
-}
+//uint8_t TwoWire::requestFrom(int address, int quantity) 
+//{
+//    return requestFrom((uint8_t) address, (uint8_t) quantity, (uint8_t) true);
+//}
 /**********************************************************************
 name :
 function :