Cristi Stoican
PID
Dependencies: BLE_API mbed nRF51822
Diff: main.cpp
- Revision:
- 1:d3e12393b71d
- Parent:
- 0:1f4d5c5491b8
--- a/main.cpp Mon Oct 24 16:10:48 2016 +0000
+++ b/main.cpp Thu Jan 12 16:04:37 2017 +0000
@@ -26,13 +26,17 @@
//#include "mbed.h"
#include "PID.h"
+#include "RST.h"
#include "ble/BLE.h"
+#include "StateMachine.h"
+#include <Gap.h>
#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. */
+//#define MyASSERT(cond , serialpc, errVal) assert_error_app((bool)cond, serialpc, (uint16_t)errVal, __LINE__, __FILE__)
#define TXRX_BUF_LEN 20
BLE ble;
@@ -49,10 +53,28 @@
uint8_t txPayload[TXRX_BUF_LEN] = {0,};
uint8_t rxPayload[TXRX_BUF_LEN] = {0,};
+//static uint32_t GlobalTime = 1;
+Ticker periodicCommandRST;
+Ticker periodicCommandPID;
+static float PIDGlobalCommand = 0;
+static float RSTGlobalCommand = 0;
+static const float GlobalSamplingTime = 0.5;
+//static const float DecodeTime = 0.1;
static uint8_t rx_buf[TXRX_BUF_LEN];
static uint8_t rx_len=0;
+static States sm_states;
+bool g_bIsConnected = false;
+bool g_bIsAdvertising = false;
+bool g_bConnDisabled = false;
+
+bool apply_PIDCommand = false;
+bool apply_RSTCommand = false;
+
+DigitalOut myled(LED1);
+AnalogIn input(P0_4);
+PwmOut output(D4);
GattCharacteristic txCharacteristic (uart_tx_uuid, txPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
@@ -62,13 +84,239 @@
GattService uartService(uart_base_uuid, uartChars, sizeof(uartChars) / sizeof(GattCharacteristic *));
-
+void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
+{
+ pc.printf("Connected \r\n");
+ g_bIsConnected = true;
+ g_bIsAdvertising = false;
+ g_bConnDisabled = false;
+}
void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
pc.printf("Disconnected \r\n");
pc.printf("Restart advertising \r\n");
ble.startAdvertising();
+ g_bIsConnected = false;
+ g_bConnDisabled = true;
+}
+
+void connectionUpdate(conn_update option)
+{
+ switch(option)
+ {
+ case start_advertising:
+ {
+ g_bIsAdvertising = true;
+ g_bIsConnected = true;
+ g_bConnDisabled = false;
+ pc.printf("Start advertising \n");
+ ble.startAdvertising();
+ break;
+ }
+ case stop_advertising:
+ {
+ g_bIsAdvertising = false;
+ g_bIsConnected = true;
+ g_bConnDisabled = false;
+ pc.printf("Stop advertising \n");
+ break;
+ }
+ case stop_connection:
+ {
+ pc.printf("All stop \n");
+ g_bIsAdvertising = false;
+ g_bConnDisabled = true;
+ g_bIsConnected = false;
+ break;
+ }
+ case connected:
+ {
+ pc.printf("Connected \n");
+ g_bIsConnected = true;
+ g_bConnDisabled = false;
+ g_bIsAdvertising = false;
+
+ }
+ }
+
+}
+
+void sendMsg(const uint8_t *buf, uint16_t length)
+{
+ ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, length);
+}
+
+void WriteBuffer(char* myBuf)
+{
+ uint16_t bytesRead = 20;
+ sendMsg((uint8_t *)myBuf, bytesRead);
+}
+
+
+void computeCommand()
+{
+ static PIDClass PID(0.05,0.02,0.01,0.6);
+ //float out;
+
+ if(apply_PIDCommand){
+ float out = PID.ComputeCommand(input.read());
+ PIDGlobalCommand = out;
+ }else
+ {
+ PIDGlobalCommand = 0;
+ }
+}
+
+void computeRSTCommand()
+{
+ static RST_aditionalData PolinomialGrades;
+ PolinomialGrades.gradR = 3;
+ PolinomialGrades.gradT = 4;
+ PolinomialGrades.gradS = 5;
+ PolinomialGrades.setpoint = 0.8;
+ static uint8_t R_pol[10] = {1,2,3,3,0,0,0,0,0,0};
+ static uint8_t T_pol[10] = {1,3,2,3,5,0,0,0,0,0};
+ static uint8_t S_pol[10] = {1,5,3,2,1,5,0,0,0,0};
+ static RST new_RST((float*)R_pol, (float*)T_pol, (float*)S_pol, PolinomialGrades);
+ if(apply_RSTCommand)
+ {
+ float out = new_RST.ComputeCommand(input.read());
+ RSTGlobalCommand = out;
+ }else
+ {
+ RSTGlobalCommand = 0;
+ }
+}
+
+void decode(uint8_t* buf)
+{
+ char myBuf[TXRX_BUF_LEN];
+ switch(buf[0])
+ {
+ case 'P':
+ {
+ sprintf(myBuf,"command=%2f", PIDGlobalCommand);
+ WriteBuffer(myBuf);
+ }
+ case 'm':
+ {
+ sprintf(myBuf,"ReadADC=%2f",input.read());
+ WriteBuffer(myBuf);
+ }
+ case 'R':
+ {
+ sprintf(myBuf,"RSTcommand=%2f", RSTGlobalCommand);
+ WriteBuffer(myBuf);
+ }
+ case 's':
+ {
+ sprintf(myBuf,"Safe mode=%2f", PIDGlobalCommand);
+ WriteBuffer(myBuf);
+ }
+ default:
+ {
+ sprintf(myBuf,"Default Case %d", 0);
+ WriteBuffer(myBuf);
+ }
+ }
+}
+
+void decode_state_machine()
+{
+ char buf;
+ switch(sm_states)
+ {
+ case shut_down:
+ {
+ //do nothing
+ apply_PIDCommand = false;
+ apply_RSTCommand = false;
+ break;
+ }
+ case safe_state:
+ {
+ apply_PIDCommand = false;
+ apply_RSTCommand = false;
+ buf = 's';
+ decode((uint8_t*)buf);
+ break;
+ }
+ case monitoring:
+ {
+ apply_PIDCommand = false;
+ apply_RSTCommand = false;
+ buf = 'm';
+ decode((uint8_t*)buf);
+ break;
+ }
+ case local_control_PID:
+ {
+ apply_PIDCommand = true;
+ apply_RSTCommand = false;
+ buf = 'P';
+ decode((uint8_t*)buf);
+ break;
+ }
+ case local_control_RST:
+ {
+ apply_PIDCommand = false;
+ apply_RSTCommand = true;
+ buf = 'R';
+ decode((uint8_t*)buf);
+ break;
+ }
+ }
+}
+
+void conn_state_machine(uint8_t *letter, uint8_t *letter_2)
+{
+ switch(letter[0]){
+ case 'a':
+ {
+ connectionUpdate(start_advertising);
+ sm_states = safe_state;
+ break;
+ }
+ case 'c':
+ {
+ connectionUpdate(connected);
+ switch(letter_2[0])
+ {
+ case 'm':
+ {
+ sm_states = monitoring;
+ break;
+ }
+ case 'P':
+ {
+ sm_states = local_control_PID;
+ break;
+ }
+ case 'R':
+ {
+ sm_states = local_control_RST;
+ break;
+ }
+ case 'r':
+ {
+ sm_states = remote_control;
+ break;
+ }
+ }
+ break;
+ }
+ case 's':
+ {
+ connectionUpdate(stop_advertising);
+ sm_states = safe_state;
+ }
+ case 'd':
+ {
+ connectionUpdate(stop_connection);
+ sm_states = shut_down;
+ }
+ }
}
void WrittenHandler(const GattWriteCallbackParams *Handler)
@@ -80,17 +328,10 @@
{
ble.readCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(), buf, &bytesRead);
memset(txPayload, 0, TXRX_BUF_LEN);
- memcpy(txPayload, buf, TXRX_BUF_LEN);
- pc.printf("WriteHandler \r\n");
- pc.printf("Length: ");
- pc.putc(bytesRead);
- pc.printf("\r\n");
- pc.printf("Data: ");
- for(index=0; index<bytesRead; index++)
- {
- pc.putc(txPayload[index]);
- }
- pc.printf("\r\n");
+ memcpy(txPayload, buf, TXRX_BUF_LEN);
+ //decode();
+ //WriteBuffer(GlobalCommand);
+ conn_state_machine(&buf[0], &buf[1]);
}
}
@@ -102,24 +343,17 @@
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);
- pc.printf("RecHandler \r\n");
- pc.printf("Length: ");
- pc.putc(rx_len);
- pc.printf("\r\n");
rx_len = 0;
break;
}
}
}
-DigitalOut myled(LED1);
-AnalogIn input(P0_4);
-PwmOut output(D4);
-
int main(void)
{
ble.init();
ble.onDisconnection(disconnectionCallback);
+ ble.onConnection(connectionCallback);
ble.onDataWritten(WrittenHandler);
pc.baud(9600);
@@ -139,35 +373,12 @@
ble.addService(uartService);
ble.startAdvertising();
+ //periodicActions.attach(&returnCommand, GlobalTime);
+ periodicCommandPID.attach(&computeCommand, GlobalSamplingTime);
pc.printf("Advertising Start \r\n");
- float out = 0;
- PIDClass PID(5,2,1,100);
while(1)
{
- myled = 1;
- wait(1);
- myled = 0;
- wait(1);
- out = PID.ComputeCommand(input.read());
- output.write(out);
- //ble.waitForEvent();
- pc.printf("Gigel");
- pc.putc(out);
+ decode_state_machine();
+ ble.waitForEvent();
}
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+}
\ No newline at end of file