Revision 8:c6345e8d607c, committed 2015-06-11

Comitter:

dkester

Date:

Thu Jun 11 20:59:22 2015 +0000

Parent:

7:bb6eb41a29da

Commit message:

working without Iic

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BLE_API.lib	Thu Jun 11 20:59:22 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/Bluetooth-Low-Energy/code/BLE_API/#c4436674db7b

--- a/Controller.cpp	Sun Jun 07 21:59:02 2015 +0000
+++ b/Controller.cpp	Thu Jun 11 20:59:22 2015 +0000
@@ -1,80 +1,135 @@
 #include "Controller.h"
+#include "InterruptManager.h"
 
+InterruptIn   imu(p16);
 InterruptIn   button(p23);
-InterruptIn   imu(p16);
-Serial pc(USBTX, USBRX);
+
+
+//Ticker ticker;
+//Ticker tickerData;
 
 Controller::Controller()
 {
-    this->sensors = new Sensors();
-    this->storage = new Storage();
-    
-    this->modeList.push_back(new TrainingCommand(sensors));
-    this->modeList.push_back(new OfflineCommand(sensors, storage));
-    this->modeList.push_back(new ReadCommand(storage));
-    this->modeList.push_back(new IdleCommand(sensors));
-    this->modeList.push_back(new SleepCommand(sensors));
+    buttonIntFlag = false;
+    imuIntFlag = false;
+    tickerIntFlag = false;
+    currentModeSelector = 3;
+    tickerIntDataFlag = false;
+
+    angle = 0;
+    peak = false;
+
+    sensors = new Sensors();
+    storage = new Storage();
+    gonioService = new GonioService();
+
+    //Set up IMU with sample frequency
+    sensors->setupIMU(0x4f);
+
+    modeList.push_back(new TrainingCommand(sensors, gonioService));
+    modeList.push_back(new OfflineCommand(sensors, storage));
+    modeList.push_back(new ReadCommand(storage, gonioService));
+    modeList.push_back(new IdleCommand(sensors, gonioService));
+    modeList.push_back(new SleepCommand(sensors));
+
+    wait(0.01);
+
+    //sensors->disableIMU();
 
     button.fall(this, &Controller::buttonInt);
     imu.fall(this, &Controller::imuInt);
 
     //Set idle command at startup
-    this->currentMode = 3;
-    
-    sensors->setupIMU();
-    sensors->disableIMU();
-    wait(0.1);
-    this->modeList[currentMode]->initialize();
-    
+    currentMode()->initialize();
 }
 
 void Controller::setCommand(int n)
 {
-    this->modeList[currentMode]->finish();
-    this->currentMode = n;
-    this->modeList[currentMode]->initialize();
+    currentMode()->finish();
+    currentModeSelector = n;
+    currentMode()->initialize();
+}
+
+Command* Controller::currentMode()
+{
+    return modeList[currentModeSelector];
 }
 
 void Controller::imuInt()
 {
-    this->modeList[currentMode]->execute();
+     
+     imuIntFlag = true;
 }
 
 void Controller::buttonInt()
 {
-    wait(0.25);
-    this->modeList[currentMode]->button();
+    wait(0.25); //wait for debounce
+    buttonIntFlag = true;
+}
+
+void Controller::tickerInt()
+{
+    tickerIntFlag = true;
+}
+
+void Controller::tickerDataInt()
+{
+    tickerIntDataFlag = true;
 }
 
 void Controller::run()
 {
-    pc.baud(19200);
+    /*
+    ticker.attach(this, &Controller::tickerInt, 0.1);
+    wait(0.05);
+    tickerData.attach(this, &Controller::tickerDataInt, 0.1);
+    */
+
 
     while(1) {
-
-        if(pc.readable()) {
-
-            char command = pc.getc();
-
+        
+        if(gonioService->isConnected() & gonioService->newValue()){
+            
+            int8_t command = gonioService->getWriteValue() - 1;
+            printf("%d", command);
             switch(command) {
-                case '1':
+                case 0 :
+                    printf("een\n");
                     setCommand(0);
                     break;
-                case '2':
+                case 1 :
+                    printf("twee\n");
+                    gonioService->disconnect();
                     setCommand(1);
                     break;
-                case '3':
+                case 2 :
+                    printf("drie\n");
                     setCommand(2);
                     break;
-                case '4':
+                case 3 :
+                    printf("vier\n");
                     setCommand(3);
                     break;
-                case '5':
-                    setCommand(4);
-                    break;
+                }
             }
+        
+        
+        if(buttonIntFlag) {
+            buttonIntFlag = false;
+            currentMode()->button();
+        }
+
+        if(tickerIntDataFlag) {
+            tickerIntDataFlag = false;
+        }
+
+        if(tickerIntFlag) {
+            tickerIntFlag = false;
+        }
+
+        if(imuIntFlag) {
+            imuIntFlag = false;
+            currentMode()->execute();
         }
     }
-
-
 }
\ No newline at end of file

--- a/Controller.h	Sun Jun 07 21:59:02 2015 +0000
+++ b/Controller.h	Thu Jun 11 20:59:22 2015 +0000
@@ -4,21 +4,35 @@
 #include "Sensors.h"
 #include "Storage.h"
 
+#include "BLEDevice.h"
+#include "GonioService.h"
+
 
 class Controller {
   
   std::vector<Command*> modeList;
-  int currentMode;
+  int currentModeSelector;
+  bool buttonIntFlag;
+  bool imuIntFlag;
+  bool tickerIntFlag;
+  bool tickerIntDataFlag;
+  bool peak;
+  
+  int16_t angle;
   Sensors* sensors;
   Storage* storage;
+  GonioService* gonioService;
   
   public:
   
   Controller();
-  
+  void disconnect();
   void setCommand(int);
   void buttonInt();
-  void imuInt();    
-  
+  void imuInt();  
+  void tickerInt();    
+  void tickerDataInt();
+
+  Command* currentMode();
   void run();
 };
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GonioService.cpp	Thu Jun 11 20:59:22 2015 +0000
@@ -0,0 +1,135 @@
+#include "GonioService.h"
+
+BLEDevice  _ble;
+Gap::ConnectionParams_t fast;
+
+void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
+{
+    printf("\n\r disconnect %d:\n\r",reason);
+    if(reason == Gap::LOCAL_HOST_TERMINATED_CONNECTION) {
+        printf("\n\r local host terminated connection \n\r");
+    }else{
+    _ble.startAdvertising(); // restart advertising
+    }
+}
+
+GonioService::GonioService():
+    ble(_ble),
+    G_ServiceUUID(0x9000),
+    G_CharUUID(0x9001),
+    G_ControlUUID(0x9002),
+    gValueBytes(0,0,0),
+    dataChar(G_CharUUID, gValueBytes.getPointer(),
+             gValueBytes.getNumValueBytes(), GonioValueBytes::MAX_VALUE_BYTES,
+             GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
+    batteryChar(GattCharacteristic::UUID_BATTERY_LEVEL_CHAR, &batteryValue,
+                GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY),
+    controlPoint(G_ControlUUID, &controlPointValue)
+{
+    setupService();
+    //;
+}
+
+bool GonioService::isConnected()
+{
+    return ble.getGapState().connected;
+}
+
+uint8_t GonioService::getWriteValue()
+{
+    return writeValue;
+}
+
+void GonioService::disconnect(){
+    ble.disconnect( Gap::LOCAL_HOST_TERMINATED_CONNECTION);
+    }
+
+bool GonioService::newValue()
+{
+    if(newValueFlag == 1) {
+        newValueFlag = 0;
+        return true;
+    } else {
+        return false;
+    }
+}
+
+void GonioService::setupService()
+{
+    static bool serviceAdded = false; /* We should only ever need to add the heart rate service once. */
+    if (serviceAdded) {
+        return;
+    }
+    init();
+    //batteryLevel(GattCharacteristic::UUID_BATTERY_LEVEL_CHAR, &batteryValue, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
+    GattCharacteristic *charTable[] = {&dataChar, &controlPoint,&batteryChar};
+    GattService         GonioService(G_ServiceUUID, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
+
+    ble.addService(GonioService);
+    serviceAdded = true;
+
+    ble.onDataWritten(this, &GonioService::onDataWritten);
+    setupAdvertising();
+}
+
+void GonioService::updateBattery(uint8_t newLevel)
+{
+    batteryValue = newLevel;
+    ble.updateCharacteristicValue(batteryChar.getValueAttribute().getHandle(), &batteryValue, 1);
+}
+
+uint8_t GonioService::getBatteryLevel()
+{
+    return batteryValue;
+}
+
+void GonioService::updateGonio(uint16_t params, uint16_t acc, uint16_t gyro)
+{
+    gValueBytes.updateGonio(params,acc, gyro);
+    ble.updateCharacteristicValue(dataChar.getValueAttribute().getHandle(), gValueBytes.getPointer(), gValueBytes.getNumValueBytes());
+    //waitForEvent();
+}
+
+void GonioService::onDataWritten(const GattCharacteristicWriteCBParams *params)
+{
+    if (params->charHandle == controlPoint.getValueAttribute().getHandle()) {
+        writeValue = params->data[0];
+        newValueFlag = true;
+    }
+    //waitForEvent();    
+}
+
+void GonioService::waitForEvent(){
+    ble.waitForEvent();
+    }
+
+uint8_t GonioService::getGServiceUUID()
+{
+    return G_ServiceUUID;
+}
+
+void GonioService::init()
+{
+    ble.init();
+    ble.setTxPower(4);
+    ble.onDisconnection(disconnectionCallback);
+    ble.getPreferredConnectionParams(&fast);
+    fast.minConnectionInterval = 6; // 7.5 ms (6*1.25)
+    fast.maxConnectionInterval = 8; // 10 ms (8*1.25)
+    fast.slaveLatency = 0;
+    ble.setPreferredConnectionParams(&fast);
+}
+
+void GonioService::setupAdvertising()
+{
+    const static char DEVICE_NAME[] = "GonioMeter";
+    static const uint16_t uuid16_list[] = {G_ServiceUUID};
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
+    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
+    ble.setAdvertisingInterval(50);
+    ble.startAdvertising();
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GonioService.h	Thu Jun 11 20:59:22 2015 +0000
@@ -0,0 +1,110 @@
+/* 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.
+ */
+
+#ifndef __GONIOSERVICE_H__
+#define __GONIOSERVICE_H__
+
+#include "BLEDevice.h"
+#include "mbed.h"
+
+class GonioService {
+public:
+    GonioService();
+
+    void updateGonio(uint16_t params, uint16_t acc, uint16_t gyro);
+    void updateBattery(uint8_t newLevel);
+    void onDataWritten(const GattCharacteristicWriteCBParams *params);
+    uint8_t getGServiceUUID();
+    uint8_t getWriteValue();
+    uint8_t getBatteryLevel();
+    void setupService();
+    bool isConnected();
+    bool newValue();
+    void waitForEvent();
+    void disconnect();
+    
+private:
+    //void disconnectionCallback();
+    //void periodicCallback();
+    void init();
+    void setupAdvertising();
+    bool newValueFlag;
+    
+    struct GonioValueBytes {
+        static const unsigned MAX_VALUE_BYTES  = 6; /* FLAGS + up to two bytes for heart-rate */
+        static const unsigned FLAGS_BYTE_INDEX = 0;
+        static const unsigned VALUE_FORMAT_BITNUM = 0;
+        static const uint8_t  VALUE_FORMAT_FLAG   = (1 << VALUE_FORMAT_BITNUM);
+
+
+        GonioValueBytes(uint16_t params, uint16_t acc, uint16_t gyro) : valueBytes() {
+            updateGonio(params,acc,gyro);
+        }
+
+        void updateGonio(uint16_t hoek, uint16_t acc, uint16_t gyro) {
+            //valueBytes[FLAGS_BYTE_INDEX]    |= VALUE_FORMAT_FLAG;
+           valueBytes[FLAGS_BYTE_INDEX ]    = (uint8_t)(hoek >> 8);
+            valueBytes[FLAGS_BYTE_INDEX + 1] = (uint8_t)(hoek & 0xFF);
+            valueBytes[FLAGS_BYTE_INDEX + 2] = (uint8_t)(acc >> 8);
+            valueBytes[FLAGS_BYTE_INDEX + 3] = (uint8_t)(acc & 0xFF);
+            valueBytes[FLAGS_BYTE_INDEX + 4] = (uint8_t)(gyro >> 8);
+            valueBytes[FLAGS_BYTE_INDEX + 5] = (uint8_t)(gyro & 0xFF);
+        }
+
+        uint8_t       *getPointer(void) {
+            return valueBytes;
+        }
+
+        const uint8_t *getPointer(void) const {
+            return valueBytes;
+        }
+
+        unsigned getNumValueBytes(void) const {
+            return MAX_VALUE_BYTES;
+        }
+        
+        uint8_t getValue(int index){
+            if(index >=0 && index<MAX_VALUE_BYTES ){
+                return valueBytes[index];
+            } else{
+                return NULL;
+            }
+        }
+
+private:
+        uint8_t valueBytes[MAX_VALUE_BYTES];
+    };
+    
+private:
+    //const static char DEVICE_NAME[];
+    //uint16_t uuid16_list[];    
+
+    BLEDevice           &ble;
+    uint16_t            G_ServiceUUID;
+    uint16_t            G_CharUUID;
+    uint16_t            G_ControlUUID;
+
+    GonioValueBytes     gValueBytes;
+    uint8_t             controlPointValue;
+    uint8_t             writeValue;
+    uint8_t             batteryValue;
+
+    GattCharacteristic                      dataChar;
+    GattCharacteristic                      batteryChar;
+    WriteOnlyGattCharacteristic<uint8_t>    controlPoint;
+};
+
+#endif /* #ifndef __GONIOSERVICE_H__*/
\ No newline at end of file

--- a/Sensors.cpp	Sun Jun 07 21:59:02 2015 +0000
+++ b/Sensors.cpp	Thu Jun 11 20:59:22 2015 +0000
@@ -7,17 +7,26 @@
 
 Sensors::Sensors()
 {
-    i2c.frequency(100000);
+    i2c.frequency(350000);
     
     for(int i = 0; i < 12; i++){
         imuData[i] = 0;
         }
         
     angle[0] = angle[1] = 0;
+    angleDummy[0] = angleDummy[1] = 0;
     
-    setupAngle();
+    //setupAngle();
+    setupIMU(0xfe);
 }
 
+
+int8_t Sensors::getAngleDummy(int n){
+    angleDummy[n]++;
+    return angleDummy[n];
+    
+    }
+
 void Sensors::setupAngle()
 {
     cmd[0] = 0x0C;
@@ -40,15 +49,12 @@
 }
 
 
-void Sensors::setupIMU()
-{
-    //ResetIMU
-    //writeRegister(0x6B, readRegister(0x6B) | (1<<7));
-    //wait(0.1);
-    
+void Sensors::setupIMU(int8_t sampleFrequencyIMU)
+{   
+
     
     //Set sample rate to 8000/1+79 = 100Hz
-    writeRegister(0x19,0x4f); //4f = 100 hz, 9f = 50 hz
+    writeRegister(0x19,sampleFrequencyIMU); //4f = 100 hz, 9f = 50 hz
 
     //Disable Frame Synchronization (FSYNC)
     writeRegister(0x1A,0x0);
@@ -64,22 +70,22 @@
     writeRegister(0x1C,(0x3 << 3));
 
     //Set Motion Free Fall Threshold
-    writeRegister(0x1D,0x01);
+    writeRegister(0x1D,0x0a);
 
     //Set Motion Free Fall Dur
     writeRegister(0x1E,0xff);
 
     //Set Motion Detection Threshold
-    writeRegister(0x1F,0x5);
+    writeRegister(0x1F,0xf);
 
     //Set Motion Detection Dur
-    writeRegister(0x20,0x01);
+    writeRegister(0x20,0x0a);
 
     //Set Zero Motion Threshold
     writeRegister(0x21,0xf1);
 
     //Set Zero Motion Dur
-    writeRegister(0x22,0x01);
+    writeRegister(0x22,0x0a);
 
     //Disable FIFO buffer
     writeRegister(0x23,0x00);
@@ -91,14 +97,16 @@
 
     //Interrupt Enable
     //[7] FF_EN [6] MOT_EN  [5] ZMOT_EN [4] FIFO_OFLOW_EN   [3] I2C_MST_INT_EN  [2] PLL_RDY_INT_EN  [1] DMP_INT_EN  [0] RAW_RDY_EN
-    writeRegister(0x38,0xe1); 
+    //writeRegister(0x38,0xe1); 
+    //writeRegister(0x38,0x1); 
+    writeRegister(0x38, 0xe1);
 
     //Motion detection control
     writeRegister(0x69,(1<<4));
 
     //User control
     //[7] DMP_EN    [6] FIFO_EN [5] I2C_MST_EN  [4] I2C_IF_DIS  [3] DMP_RESET   [2] FIFO_RESET  [1] I2C_MST_RESET   [0] SIG_COND_RESET
-    writeRegister(0x6A,0x00);
+    writeRegister(0x6A,0x80);
 
     //Set power managenemt 1
     //[7] DEVICE_RESET    [6] SLEEP   [5] CYCLE       [3] TEMP_DIS    [2:0] CLK_SEL
@@ -115,6 +123,8 @@
     i2c.write(AS5600,cmd,1);
     i2c.read(AS5600,out,2);
     
+    
+    
     angle[0] = out[0];
     angle[1] = out[1];
 }
@@ -145,6 +155,11 @@
     return readRegister(0x3A);
 }
 
+int8_t Sensors::getMotionDetectionStatus(){
+    //[7] MOT_XNEG    [6] MOT_XPOS    [5] MOT_YNEG    [4] MOT_YPOS    [3] MOT_ZNEG    [2] MOT_ZPOS
+    return readRegister(0x61);
+    }
+
 uint8_t Sensors::getAngle(int i){
     return angle[i];
     }

--- a/Sensors.h	Sun Jun 07 21:59:02 2015 +0000
+++ b/Sensors.h	Thu Jun 11 20:59:22 2015 +0000
@@ -10,11 +10,16 @@
 
     int8_t imuData[12];
     uint8_t angle[2];
+    int8_t angleDummy[2];
+    
+    int8_t sampleFrequencyIMU;
 
 public:
     Sensors ();
 
-    void    setupIMU();
+    int8_t getAngleDummy(int);
+
+    void    setupIMU(int8_t);
     void    setupAngle ();
 
     void    wakeIMU();
@@ -28,6 +33,7 @@
     uint8_t  getAngle(int);
     int8_t   getIMU(int);
     int8_t getInterruptStatus();
+    int8_t getMotionDetectionStatus();
 
     //Read and write register function for MPU
     int8_t readRegister(int8_t addr);

--- a/commands.lib	Sun Jun 07 21:59:02 2015 +0000
+++ b/commands.lib	Thu Jun 11 20:59:22 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/dkester/code/commands/#32afe87d4b62
+http://developer.mbed.org/users/dkester/code/commands/#47cb37923f58

--- a/main.cpp	Sun Jun 07 21:59:02 2015 +0000
+++ b/main.cpp	Thu Jun 11 20:59:22 2015 +0000
@@ -4,9 +4,9 @@
 
 int main(void)
 {   
-    wait(2);
+    
     Controller* goniometer = new Controller();
-    
+    wait(0.5);
     while(true) {
         goniometer->run();
     }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/nRF51822.lib	Thu Jun 11 20:59:22 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/dkester/code/nRF51822/#02500109ccb8