dsf
Dependencies: BLE_API mbed nRF51822
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
diff -r 000000000000 -r b5906c81772b BLE_API.lib --- /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
diff -r 000000000000 -r b5906c81772b BTDevice.cpp --- /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); +} +
diff -r 000000000000 -r b5906c81772b BTDevice.hpp --- /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
diff -r 000000000000 -r b5906c81772b CmdEnums.hpp --- /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
diff -r 000000000000 -r b5906c81772b Controller.cpp --- /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() {}
diff -r 000000000000 -r b5906c81772b Controller.hpp --- /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
diff -r 000000000000 -r b5906c81772b ControllerFactory.cpp --- /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; + } + } +} +
diff -r 000000000000 -r b5906c81772b ControllerFactory.hpp --- /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
diff -r 000000000000 -r b5906c81772b ControllerParams.hpp --- /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
diff -r 000000000000 -r b5906c81772b PIDController.cpp --- /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() {}
diff -r 000000000000 -r b5906c81772b PIDController.hpp --- /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
diff -r 000000000000 -r b5906c81772b RST.cpp --- /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; +}
diff -r 000000000000 -r b5906c81772b RST.hpp --- /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
diff -r 000000000000 -r b5906c81772b main.cpp --- /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(); + } +} + + + + + + + + + + + + + + + +
diff -r 000000000000 -r b5906c81772b mbed.bld --- /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
diff -r 000000000000 -r b5906c81772b nRF51822.lib --- /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