Revision 1:d3e12393b71d, committed 2017-01-12

Comitter:

stoicancristi

Date:

Thu Jan 12 16:04:37 2017 +0000

Parent:

0:1f4d5c5491b8

Commit message:

v2

Changed in this revision

--- a/PID.cpp	Mon Oct 24 16:10:48 2016 +0000
+++ b/PID.cpp	Thu Jan 12 16:04:37 2017 +0000
@@ -1,13 +1,10 @@
 #include "PID.h"
 
-PIDClass::PIDClass(int _Kr,int _Ki, int _Kd,int _SetPoint):Kr(_Kr),Ki(_Ki),Kd(_Kd),SetPoint(_SetPoint)
+PIDClass::PIDClass(float _Kr,float _Ki, float _Kd,float _SetPoint):Kr(_Kr),Ki(_Ki),Kd(_Kd),SetPoint(_SetPoint)
 {
-    /*Kr = _Kr;
-    Ki = _Ki;
-    Kd = _Kd;*/
+    //
 }
 
-
 float PIDClass::ComputeCommand(float inputADC)
 {
     static PID_static param;

--- a/PID.h	Mon Oct 24 16:10:48 2016 +0000
+++ b/PID.h	Thu Jan 12 16:04:37 2017 +0000
@@ -5,7 +5,7 @@
 #include "math.h"
 
 #define Te 100
-#define SUPERIOR_MARGIN 100
+#define SUPERIOR_MARGIN 1
 #define INFERIOR_MARGIN 0 
 
 typedef struct
@@ -17,15 +17,21 @@
 class PIDClass
 {
      public:   
-        PIDClass(int _Kr, int _Ki, int _Kd, int _SetPoint);
+        PIDClass(float _Kr, float _Ki, float _Kd, float _SetPoint);
 
         float ComputeCommand(float inputADC);
+        float getKr(){return Kr;}
+        float getKi(){return Ki;}
+        float getKd(){return Kd;}
+        void setKr(float new_Kr){Kr = new_Kr;}
+        void setKi(float new_Ki){Ki = new_Ki;}
+        void setKd(float new_Kd){Kd = new_Kd;}
         
      private:
-        int Kr;
-        int Ki;
-        int Kd;   
-        int SetPoint;
+        float Kr;
+        float Ki;
+        float Kd;   
+        float SetPoint;
 };
 
 #endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RST.cpp	Thu Jan 12 16:04:37 2017 +0000
@@ -0,0 +1,53 @@
+#include "RST.h"
+
+RST::RST(float* R, float* S, float* T, RST_aditionalData SP):m_R(R), m_S(S), m_T(T), m_SP(SP)
+{
+    //
+}
+
+float RST::ComputeCommand(float inputADC)
+{
+    static float last_references[MAX_GRADE];
+    static float last_outputs[MAX_GRADE];
+    static float last_commands[MAX_GRADE];
+    float R_component = 0;
+    float T_component = 0;
+    float S_component = 0;
+    float outputPWM;
+    
+    for(int i=0;i<(m_SP.gradR+1);i++)
+    {
+        last_references[0] = m_SP.setpoint;
+        R_component = R_component + m_R[i]*last_references[i];
+    }
+    for(int i=0;i<m_SP.gradR;i++)
+    {
+        last_references[i+1] = last_references[i];
+    }
+    for(int i=0;i<(m_SP.gradT+1);i++)
+    {
+        last_outputs[0] = inputADC;
+        T_component = T_component + m_T[i]*last_outputs[i];
+    }
+    for(int i=0;i<m_SP.gradT;i++)
+    {
+        last_outputs[i+1] = last_outputs[i];
+    }
+    for(int i=1;i<(m_SP.gradS+1);i++)
+    {
+        S_component = S_component + last_commands[i]*m_S[i];
+    }
+    
+    outputPWM = T_component - R_component - S_component;
+    last_commands[0] = outputPWM;
+    
+    for(int i=0;i<m_SP.gradS;i++)
+    {
+        last_commands[i+1] = last_commands[i];
+    }
+    
+    return outputPWM;
+}
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RST.h	Thu Jan 12 16:04:37 2017 +0000
@@ -0,0 +1,39 @@
+#ifndef _RST_H
+#define _RST_H
+
+#include "mbed.h"
+#include "math.h"
+
+#define UPPER_LIMIT 1
+#define LOWER_LIMIT 0
+#define MAX_GRADE  10
+
+typedef struct
+{
+    float setpoint;
+    uint8_t gradR;
+    uint8_t gradS;
+    uint8_t gradT;
+} RST_aditionalData;
+
+class RST{
+    
+    public:
+        RST(float* R, float* S, float* T, RST_aditionalData SP);
+        float* getR(){return m_R;}
+        float* getS(){return m_S;}
+        float* getT(){return m_T;}
+        void setR(float *new_R){m_R = new_R;}
+        void setS(float *new_S){m_S = new_S;}
+        void setT(float *new_T){m_T = new_T;}
+        
+        float ComputeCommand(float inputADC);
+        
+    private:
+        float* m_R;
+        float* m_S;
+        float* m_T;
+        RST_aditionalData m_SP;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StateMachine.h	Thu Jan 12 16:04:37 2017 +0000
@@ -0,0 +1,25 @@
+#ifndef _StateMachine_
+#define _StateMachine
+
+typedef enum
+{
+    shut_down = 0,
+    safe_state,
+    monitoring,
+    local_control_PID,
+    local_control_RST,
+    remote_control   
+} States;
+
+typedef enum
+{
+    start_advertising = 0,
+    stop_advertising,
+    stop_connection,
+    connected
+} conn_update;
+
+void decode_state_machine(States st, uint8_t buf[20]);
+void conn_state_machine(uint8_t* buf1, uint8_t* buf2);
+
+#endif

--- 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