Diff: HC05.cpp

Revision:

0:e0c6e95761d2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HC05.cpp	Thu Feb 04 20:18:06 2016 +0000
@@ -0,0 +1,82 @@
+#include "mbed.h"
+#include "HC05.h"
+
+HC05::HC05(PinName tx, PinName rx): myBT(tx, rx)
+{
+myBT.baud(9600);
+
+index = 0;
+
+started = false;
+ended = false;   
+}
+
+void HC05::getValue(int16_t* x, int16_t* y, int16_t* c)
+{      
+    if(!myBT.readable())
+    {
+        *x = 0;
+        *y = 0;
+        *c = 0;
+    }  
+    while(myBT.readable())
+    {
+        btChar = myBT.getc();
+        
+        if(btChar == SOP)
+        {
+            index = 0;
+            btData[index] = '\0';
+            started = true;
+            ended = false;
+        }
+        else if(btChar == EOP)
+        {
+            ended = true;
+            break;
+        }
+        else
+        {
+            if(index < 19)
+            {
+                btData[index] = btChar;
+                index++;
+                btData[index] = '\0';
+            }
+        }
+    }
+    
+    if(started && ended)
+    {
+        char *name = strtok(btData, "=");
+                    
+        while(name)
+        {                  
+            char *valToken = strtok(NULL, ",");
+            if(valToken)
+            {                    
+                int val = atoi(valToken);
+                                
+                if(strcmp(name, "X") == 0)
+                {
+                    *x = val;            
+                }
+                else if(strcmp(name, "Y") == 0) 
+                {
+                    *y = val;
+                }
+                else if(strcmp(name, "C") == 0) 
+                {
+                    *c = val;
+                }
+            }
+            name = strtok(NULL, "=");
+        }
+            
+        // Reset for the next packet
+        started = false;
+        ended = false;
+        index = 0;
+        btData[index] = '\0';             
+    }
+}
\ No newline at end of file