Revision 0:b5906c81772b, committed 2017-02-05

Comitter:

stoicancristi

Date:

Sun Feb 05 16:31:58 2017 +0000

Commit message:

BLE

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BLE_API.lib	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/stoicancristi/code/BLE_API/#d912402c34ec

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BTDevice.cpp	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,35 @@
+#include "BTDevice.hpp"
+
+BTDevice::BTDevice() {
+    this->ctrl = NULL;
+}
+
+BTDevice::BTDevice(BLE _ble):ble(_ble){}
+
+BTDevice::~BTDevice() { 
+    delete this->ctrl; 
+} 
+
+void BTDevice::sendMsg(char *buf, uint16_t length) {
+    ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), (const uint8_t*)buf, length);
+}
+
+int BTDevice::readMsg(uint8_t buf[TXRX_BUF_LEN], uint16_t expectedLen) {
+    uint16_t bytesRead;
+    ble.readCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(), buf, &bytesRead);
+    
+    return (bytesRead == expectedLen)? bytesRead : -1;
+}
+
+void BTDevice::attachController(Controller *c) { 
+    this->ctrl = c; 
+}
+
+void BTDevice::setCtrlRef(float ref) {
+    this->ctrl->updateRef(ref);
+}
+
+void BTDevice::setCtrlParams(ControllerParams &cp) {
+    this->ctrl->updateParams(cp);
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BTDevice.hpp	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,63 @@
+#ifndef _BTdevice_
+#define _BTdevice_
+
+#include "ble/BLE.h"
+#include <Gap.h>
+#include "ControllerParams.hpp"
+#include "Controller.hpp"
+#include "CmdEnums.hpp"
+
+#define TXRX_BUF_LEN    20
+
+#define BLE_UUID_TXRX_SERVICE            0x0000 /**< The UUID of the Nordic UART Service. */
+#define BLE_UUID_TX_CHARACTERISTIC       0x0002 /**< The UUID of the TX Characteristic. */
+#define BLE_UUIDS_RX_CHARACTERISTIC      0x0003 /**< The UUID of the RX Characteristic. */
+
+// The Nordic UART Service
+static const uint8_t uart_base_uuid[] = {0x71, 0x3D, 0, 0, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
+static const uint8_t uart_tx_uuid[]   = {0x71, 0x3D, 0, 3, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
+static const uint8_t uart_rx_uuid[]   = {0x71, 0x3D, 0, 2, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
+static const uint8_t uart_base_uuid_rev[] = {0x1E, 0x94, 0x8D, 0xF1, 0x48, 0x31, 0x94, 0xBA, 0x75, 0x4C, 0x3E, 0x50, 0, 0, 0x3D, 0x71};
+
+
+uint8_t txPayload[TXRX_BUF_LEN] = {0};
+uint8_t rxPayload[TXRX_BUF_LEN] = {0};
+
+
+static uint8_t rx_buf[TXRX_BUF_LEN];
+static uint8_t rx_len=0;
+
+
+GattCharacteristic  txCharacteristic (uart_tx_uuid, txPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
+                                      
+GattCharacteristic  rxCharacteristic (uart_rx_uuid, rxPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
+                                      
+GattCharacteristic *uartChars[] = {&txCharacteristic, &rxCharacteristic};
+
+GattService         uartService(uart_base_uuid, uartChars, sizeof(uartChars) / sizeof(GattCharacteristic *));
+
+
+
+class BTDevice {
+    
+    public:
+        BTDevice(BLE _ble);
+        BTDevice();
+        void sendMsg(char *buf, uint16_t length);
+        int readMsg(uint8_t buf[TXRX_BUF_LEN], uint16_t expectedLen);
+
+        void attachController(Controller *c);
+        void setCtrlRef(float ref);
+        void setCtrlParams(ControllerParams &cp);
+
+        void interpretCmd(uint8_t cmd[TXRX_BUF_LEN]);
+        ~BTDevice();        
+
+    //friend void getTokens(uint8_t cmd[TXRX_BUF_LEN], BTDevive&, SysObj&, SysObjTypes&, Actions&, uint8_t actionParams[TXRX_BUF_LEN]);
+        
+    private:
+        BLE ble;
+        Controller *ctrl;
+        float out;
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CmdEnums.hpp	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,29 @@
+#ifndef CMDENUMS_H
+#define CMDENUMS_H
+
+enum SysObj {
+    CONTROLLER = 0x01,
+    MONITOR = 0x02
+};
+
+enum SysObjTypes {
+    P = 0x11,
+    PI = 0x12,
+    PID = 0x13,
+    RST = 0x14
+};
+
+enum Actions {
+    CREATE = 0x20,
+    START = 0x21,
+    UPDATE = 0x22,
+    STOP = 0x23
+};
+
+enum Separators {
+    LT = 0x3c,
+    GT = 0x3e,
+    USCORE = 0x5f
+};
+
+#endif 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,11 @@
+#include "Controller.hpp"
+
+void Controller::updateRef (float _ref) {
+    ref = _ref;
+}
+
+void Controller::updateOut (float _out) {
+    out = _out;
+}
+
+Controller::~Controller() {}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.hpp	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,22 @@
+#ifndef CONTROLLER_H
+#define CONTROLLER_H
+
+#include "ControllerParams.hpp"
+
+using namespace std;
+
+class Controller {
+    protected:  
+        float ref;
+        float out;
+
+    public:
+        virtual float calculateCmd(void) const = 0;
+        virtual void updateParams(ControllerParams&) = 0;
+        virtual ~Controller();  
+    
+        void updateRef(float);  
+        void updateOut(float);  
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ControllerFactory.cpp	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,72 @@
+
+#include "ControllerFactory.hpp"
+
+
+void ControllerFactory::createController (Controller *c, SysObjTypes &ctrlType) {
+    
+    switch (ctrlType) {
+        case P:         //fall-through
+        case PI:
+        case PID: {
+            c = new PIDController();
+            break; 
+        }
+        case RST: {
+            //c = new RST::RST();
+            break;
+        }
+        default: c = 0;
+    }
+}
+
+void ControllerFactory::createControllerParams (ControllerParams &cp, SysObjTypes &ctrlType, float *paramsList, int paramsListLen) {
+    /*
+        paramsList form: {
+            P: [kp] 
+            PI: [kp, ti]
+            PID: [kp, ti, td]
+            RST: [nR,r0,r1,...,r_nR,nS,s0,s1,...,s_nS,nT,t0,t1,...,t_nT]
+        }
+        cond: paramsListLen = nR + nS + nT + 6
+    */
+    switch (ctrlType) {
+        case P: {
+            cp.kp = *paramsList;
+            cp.ti = 0;
+            cp.td = 0;
+            break;
+        }
+        case PI: {
+            cp.kp = *paramsList;
+            cp.ti = paramsList[1];
+            cp.td = 0;
+            break;
+        }
+        case PID: {
+            cp.kp = *paramsList;
+            cp.ti = paramsList[1];
+            cp.td = paramsList[2];
+            break;
+        }
+        case RST: {
+            int i;
+            cp.ordR = *paramsList;
+            cp.R = new float[cp.ordR+1];
+            for (i=1; i<cp.ordR+2;i++) {
+                cp.R[i] = paramsList[i];
+            }
+            cp.ordS = paramsList[i];
+            cp.S = new float[cp.ordS+1];
+            for (i=cp.ordR+2; i<cp.ordR+cp.ordS+3; i++) {
+                cp.S[i] = paramsList[i];        
+            }
+            cp.ordT = paramsList[i];
+            cp.T = new float[cp.ordT+1];
+            for(i=cp.ordR+cp.ordS+3; i<paramsListLen; i++) {
+                cp.T[i] = paramsList[i];
+            }
+            break;
+        }
+    }       
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ControllerFactory.hpp	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,18 @@
+#ifndef CONTROLLERFACTORY_H
+#define CONTROLLERFACTORY_H
+
+#include "PIDController.hpp"
+/*#include "RST.hpp"                // to be decommented after correcting
+#include "CmdEnums.hpp"
+#include "Controller.hpp"*/
+#include "BTDevice.hpp"
+
+//typedef char uint8_t; //only for compiling
+
+class ControllerFactory {
+    public:
+        void createController (Controller*, SysObjTypes&);      
+        void createControllerParams (ControllerParams&, SysObjTypes&, float *, int );
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ControllerParams.hpp	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,23 @@
+
+#ifndef CONTROLLERPARAMS_H
+#define CONTROLLERPARAMS_H
+
+typedef struct{
+
+    float kp;
+    float ti;
+    float td;
+
+    float *R;
+    float *S;
+    float *T;
+    
+    int ordR;
+    int ordS;
+    int ordT;
+    
+    static const int MAX_ORD = 10;
+
+}ControllerParams;
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDController.cpp	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,58 @@
+#include "PIDController.hpp"
+
+//PIDController::PIDController() {}
+
+void PIDController::updateParams(ControllerParams &cp) {
+    setKp(cp.kp);
+    setTi(cp.ti);
+    setTd(cp.td);
+}
+
+float PIDController::calculateCmd() const {
+    float outputPWM;
+    
+    float Proportional = kp*dif(this->ref,this->out);
+    float Integral;
+    float Derivative = (kp*td/TE)*dif(this->ref,this->out);
+    
+    if(ti == 0) {
+        outputPWM = Proportional + Derivative;
+    } else {
+        Integral = ((kp*TE)/ti)*dif(this->ref,this->out);
+        outputPWM = Proportional + Integral + Derivative;
+    }
+    
+    if(outputPWM > 1) {
+        outputPWM = outputPWM - Integral;
+    } else if(outputPWM < 0) {
+        outputPWM = 0;
+    }
+    
+    return outputPWM;
+}
+
+void PIDController::setKp(float _kp) {
+    kp = _kp;   
+}
+
+void PIDController::setTi(float _ti) {
+    ti = _ti;
+}
+
+void PIDController::setTd(float _td) {
+    td = _td;
+}
+
+float PIDController::getKp() const {
+    return kp;
+}
+
+float PIDController::getTi() const {
+    return ti;
+}
+
+float PIDController::getTd() const {
+    return td;
+}
+
+PIDController::~PIDController() {}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDController.hpp	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,33 @@
+#ifndef PIDCONTROLLER_H
+#define PIDCONTROLLER_H
+
+#include "Controller.hpp"
+#include "ControllerParams.hpp"
+
+#define TE      0.1
+#define dif(a,b)    (a-b)
+
+class PIDController : public Controller {
+
+    private:
+        float kp;
+        float ti;
+        float td;
+    
+    public:
+    
+        virtual void updateParams(ControllerParams&);   
+        virtual float calculateCmd(void) const;
+        virtual ~PIDController();
+
+        void setKp(float);
+        float getKp() const;
+        
+        void setTi(float);
+        float getTi() const;
+    
+        void setTd(float);
+        float getTd() const;        
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RST.cpp	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,74 @@
+#include "RST.hpp"
+
+RST::RST() {
+    R = 0;
+    S = 0;
+    T = 0;
+}
+
+float RST::calculateCmd()
+{
+    static float last_references[ControllerParams::MAX_ORD];
+    static float last_outputs[ControllerParams::MAX_ORD];
+    static float last_commands[ControllerParams::MAX_ORD];
+    
+    float R_component;
+    float S_component;
+    float T_component;
+    float outputPWM;
+    
+    
+    for(int i=0;i<(ordR+1);i++)
+    {
+        last_references[0] = this->ref;
+        R_component = R_component + R[i]*last_references[i];
+    }
+    for(int i=0;i<ordR;i++)
+    {
+        last_references[i+1] = last_references[i];
+    }
+    for(int i=0;i<(ordT+1);i++)
+    {
+        last_outputs[0] = this->out;
+        T_component = T_component + T[i]*last_outputs[i];
+    }
+    for(int i=0;i<ordT;i++)
+    {
+        last_outputs[i+1] = last_outputs[i];
+    }
+    for(int i=1;i<(ordS+1);i++)
+    {
+        S_component = S_component + last_commands[i]*S[i];
+    }
+    
+    outputPWM = T_component - R_component - S_component;
+    
+    for(int i=0;i<ordS;i++)
+    {
+        last_commands[i+1] = last_commands[i];
+    }
+    
+    return outputPWM; 
+}
+
+void RST::updateParams(ControllerParams &cp) {
+    ordR = cp.ordR;
+    ordS = cp.ordS;
+    ordT = cp.ordT; 
+
+    R = new float[ordR+1];
+    S = new float[ordS+1];
+    T = new float[ordT+1];
+    
+    int i;
+    for (i=0; i<ordR+1; i++) R[i] = cp.R[i];
+    for (i=0; i<ordS+1; i++) S[i] = cp.S[i];
+    for (i=0; i<ordT+1; i++) T[i] = cp.T[i];
+}
+
+
+RST::~RST() {
+    delete [] R;
+    delete [] S;
+    delete [] T;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RST.hpp	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,25 @@
+#ifndef _RST_H_
+#define _RST_H_
+
+#include "Controller.hpp"
+#include "ControllerParams.hpp"
+
+class RST : public Controller {
+    
+    public:
+        RST();
+        virtual float calculateCmd();
+        virtual void updateParams(ControllerParams&);
+        ~RST();
+    
+    private:
+        float *R;
+        float *S;
+        float *T;
+        
+        int ordR;
+        int ordS;
+        int ordT;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,190 @@
+/*
+
+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.
+
+*/
+
+/*
+ *    The application works with the BLEController iOS/Android App.
+ *    Type something from the Terminal to send
+ *    to the BLEController App or vice verse.
+ *    Characteristics received from App will print on Terminal.
+ */
+ 
+#include "mbed.h"
+#include "BTDevice.hpp"
+#include "ControllerFactory.hpp"
+
+BLE  ble;
+Ticker periodicCommand;
+
+Serial pc(USBTX, USBRX);
+
+BTDevice Nano(ble);
+ControllerFactory Control_Law();
+//Controller c;
+char *old_receive = NULL;
+float R[3] = {0.1,0.4,0.6};
+float S[3] = {0.2,0.1,0.5};
+float T[4] = {0.1,0.7,0.5};
+ControllerParams Parameters;
+
+
+// The Nordic UART Service
+
+/*decode(uint8_t *receive, Controller *c);
+
+void state_machine(uint8_t *receive)
+{
+            switch((char *)receive)
+            {
+                case 'P': 
+                {
+                    Control_Law.createController(&c, P);
+                    Parameters.kp = 0.1;
+                    Control_Law.createControllerParams(&c, P, &Parameters, 0);
+                    c.updateRef(0.5);
+                } 
+                case 'PI':
+                {
+                    Control_Law.createController(&c, PI);    
+                    Parameters.kp = 0.1;
+                    Parameters.ti = 0.3;
+                    Control_Law.createControllerParams(&c, PI, &Parameters, 0);
+                    c.updateRef(0.7);
+                }
+                case 'PID':
+                {
+                    Control_Law.createController(&c, PID);  
+                    Parameters.kp = 0.1;
+                    Parameters.ti = 0.3; 
+                    Parameters.td = 0.15;
+                    Control_Law.createControllerParams(&c, PID, &Parameters, 0);
+                    c.updateRef(0.9);
+                }
+                case 'RST':
+                {
+                    Control_Law.createController(&c, RST);  
+                    Parameters.ordR = sizeof(R)/sizeof(*R);
+                    Parameters.ordS = sizeof(S)/sizeof(*S);
+                    Parameters.ordT = sizeof(T)/sizeof(*T);
+                    Parameters.R = R;
+                    Parameters.S = S;
+                    Parameters.T = T;
+                    Control_Law.createControllerParams(&c, RST, &Parameters, Parameters.ordT);
+                    c.updateRef(1);
+                }
+                case 'delete':
+                {
+                    delete c;  
+                }
+            }    
+        decode_message(&receive, &c);
+}
+
+decode_message(uint8_t *receive, Controller c)
+{
+      switch((char *)receive)
+      {
+            case 'Command':
+            {
+                  Nano.sendMsg((uint8_t *),c.calculateCmd, 16);  
+            }
+      }
+}*/
+
+
+void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
+{
+    pc.printf("Disconnected \r\n");
+    pc.printf("Restart advertising \r\n");
+    ble.startAdvertising();
+}
+
+void WrittenHandler(const GattWriteCallbackParams *Handler)
+{   
+    uint8_t buf[TXRX_BUF_LEN];
+    uint16_t bytesRead, index;
+    
+    if (Handler->handle == txCharacteristic.getValueAttribute().getHandle()) 
+    {
+        ble.readCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(), buf, &bytesRead);
+        memset(txPayload, 0, TXRX_BUF_LEN);
+        memcpy(txPayload, buf, TXRX_BUF_LEN);       
+        //state_machine(&buf);
+    }
+}
+
+void uartCB(void)
+{   
+    while(pc.readable())    
+    {
+        rx_buf[rx_len++] = pc.getc();    
+        if(rx_len>=20 || rx_buf[rx_len-1]=='\0' || rx_buf[rx_len-1]=='\n')
+        {
+            ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), rx_buf, rx_len); 
+            rx_len = 0;
+            break;
+        }
+    }
+}
+
+int main(void)
+{
+    ble.init();
+    ble.onDisconnection(disconnectionCallback);
+    ble.onDataWritten(WrittenHandler);  
+    
+    pc.baud(9600);
+    pc.printf("SimpleChat Init \r\n");
+    
+    pc.attach( uartCB , pc.RxIrq);
+   // setup advertising 
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
+    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
+                                    (const uint8_t *)"Gigel", sizeof("Biscuit") - 1);
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
+                                    (const uint8_t *)uart_base_uuid_rev, sizeof(uart_base_uuid));
+    // 100ms; in multiples of 0.625ms. 
+    ble.setAdvertisingInterval(160);
+
+    ble.addService(uartService);
+    
+    ble.startAdvertising(); 
+    pc.printf("Advertising Start \r\n");
+    
+    while(1)
+    {
+        ble.waitForEvent(); 
+    }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/176b8275d35d
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/nRF51822.lib	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/Nordic-Semiconductor/code/nRF51822/#c90ae1400bf2