Pull changes from Yue

diff --git a/vis/device.py b/vis/device.py
index a8963751975191bed1a7294d589cbe2d5d5f6383..34a8be483dff2e9ac1a8a83de8a5744561192093 100644 (file)
--- a/vis/device.py
+++ b/vis/device.py
@@ -3,8 +3,43 @@ import time
 from re       import compile
 from serial   import Serial
 from datetime import datetime
 from re       import compile
 from serial   import Serial
 from datetime import datetime

+from struct   import *

 
 

-class State:
+
+
+#buildingFrame = 0;               #if it is a start of new frame
+
+class Const:
+        HEADER = 0x02
+        SNS_BITS = 5
+        LGT_SNS = 0x00
+        ACC_SNS = 0x01
+        MAG_SNS = 0x02
+        TCH_SNS = 0x03
+        ADC_SNS_1 = 0x04
+
+        INT = 0
+        LONG = 1
+        FLOAT = 2
+        DOUBLE = 3
+        TAIL = 0x0A
+
+        START = 0
+        STOP = 1
+        SET_INT = 2
+        buildingFrame = 0
+
+        snsCode = {'light':LGT_SNS,'touch':TCH_SNS,'acc':ACC_SNS,'mag':MAG_SNS,'a2d':ADC_SNS_1}
+        cmdCode = {'start':START, 'stop':STOP, 'set':SET_INT}
+        sizeMap = {INT:2, FLOAT:4, LONG:4, DOUBLE:8}
+        typeMap = {0:INT, 1:LONG, FLOAT:2, DOUBLE:3}
+
+class Static:
+
+        count = 0
+
+
+class State:                                    #information stored

        acc   = [None]*3
        mag   = [None]*3
        touch = [None]*2
        acc   = [None]*3
        mag   = [None]*3
        touch = [None]*2


@@ -16,19 +51,19 @@ class State:
                self.time = datetime.utcnow()
 
 class Device:
                self.time = datetime.utcnow()
 
 class Device:

-       # Attributes
-       port = "/dev/ttyACM0"
-

        # Constructors
        def __init__(self, config):
        # Constructors
        def __init__(self, config):

+                print("IN")

                self.config = config
                self.serial = None
 
        # Methods
        def connect(self):
                self.config = config
                self.serial = None
 
        # Methods
        def connect(self):

+                print("C")
+                buildingFrame = 0

                try:
                        self.inbuf  = []
                try:
                        self.inbuf  = []

-                       self.serial = Serial(self.port,          \
+                       self.serial = Serial(self.config.device, \

                                baudrate = self.config.baudrate, \
                                parity   = self.config.parity,   \
                                bytesize = self.config.databits, \
                                baudrate = self.config.baudrate, \
                                parity   = self.config.parity,   \
                                bytesize = self.config.databits, \


@@ -40,94 +75,140 @@ class Device:
                        return str(ex)
 
        def disconnect(self):
                        return str(ex)
 
        def disconnect(self):

+                print("DC")

                if self.serial and self.serial.isOpen():
                        self.serial.close()
 
                if self.serial and self.serial.isOpen():
                        self.serial.close()
 

-       def running(self):
+       def running(self):                                      # isRunning

                if self.serial == None:
                        return False
                if self.serial.isOpen() == False:
                        return False
                return True
 
                if self.serial == None:
                        return False
                if self.serial.isOpen() == False:
                        return False
                return True
 

-       def control(self):
+       def control(self):#################
+                print("CT")
+                frame = [None]*7

                for key in list(self.config.enable.keys()):
                        state = self.config.enable[key]
                        rate  = self.config.rate[key]
                for key in list(self.config.enable.keys()):
                        state = self.config.enable[key]
                        rate  = self.config.rate[key]

-                       cmd   = 'start' if state else 'stop'
-                       self._write_ascii('%s %s' % (cmd, key))
-                       self._write_ascii('set %s int %d' % (key, rate))
+                       cmd   = Const.cmdCode['start'] if state else Const.cmdCode['stop']
+                       sns   = Const.snsCode[key]
+                       frame[0] = chr(Const.HEADER)
+                       frame[1] = chr(cmd<<(Const.SNS_BITS)|(0x1F&sns))
+                        frame[2:6] = pack('f',float(rate))
+                       frame[6] = Const.TAIL
+                       self.serial.write(frame)
+                       print('[SEND1] ',frame)
+                       frame[1] = Const.cmdCode['set']<<(Const.SNS_BITS)|(0x1F&sns)
+                       self.serial.write(frame)
+                       print('[SEND2] ',frame)
+                       self.serial.flush()
+

 
        def process(self):
                items = []
                count = 0
                limit = 1000
 
        def process(self):
                items = []
                count = 0
                limit = 1000

-               if not self.running():
-                       return items
-               while self.serial.readable():
-                       try:
-                               char = self.serial.read().decode()
-                       except Exception as ex:
-                               char = ''
-                       if len(char) == 0:
-                               break
-                       if char == '\r' or char == '\n':
-                               if len(self.inbuf) == 0:
-                                       continue
-                               line = "".join(self.inbuf)
-                               print('read: [' + line + ']')
-                               item = self._parse_ascii(line)
-                               items.append(item)
-                               self.inbuf = []
-                       else:
-                               self.inbuf.append(char)
-                       if count > limit:
-                               print("Error: exceeded read limit")
-                               break
-                       count += 1
-               return items
+                if not self.running():
+                        return items;
+                if self.serial.readable():
+                        buildingFrame = 0
+                while (self.serial.inWaiting() or count<(self.frame_len(self.inbuf)-1)):                ######
+                        char = self.serial.read()
+                        count +=1
+                        if char == chr(Const.TAIL) and buildingFrame and (len(self.inbuf))== self.frame_len(self.inbuf)-1:
+                                self.inbuf.append(char)
+                                #print("[TAIL]")
+                                line = "".join(self.inbuf)
+                                print(self.inbuf)
+                                time.sleep(0.001)
+                                #self.printHex(line)
+                                item = self._parse_ascii(line)  # analyze the received data
+                                items.append(item)
+                                buildingFrame = 0               # finished building one frame
+                                #print ("BF set to 0")
+                                self.inbuf = []
+                                count = 0
+                        elif char == chr(Const.HEADER) and buildingFrame==0:
+                                self.inbuf = []
+                                buildingFrame = 1
+                                #print ("BF set to 1")
+                                self.inbuf.append(char)
+                                #print("[HEADER]")
+                                #count +=1
+                        elif buildingFrame:
+                                self.inbuf.append(char)
+                                #print("[DT]")
+                                #count +=1
+                        else:
+                                #print("[ERROR] Byte Going Nowhere")
+                                count = 0
+                                buildingFrame = 0               # reset the construction
+                                #print ("BF set to 0!")
+                                self.inbuf = []
+                        if count > limit:
+                                count = 0
+                                print("[ERROR] Exceeded Read Limit")
+                                break
+                return items
+
+
+        # auxilary function
+        def frame_len(self, frame):
+                if len(frame) < 3:
+                        return -1
+                dataType_snsType = ord(frame[1])
+                dataNum  = ord(frame[2])
+                dataType = (0xE0&dataType_snsType)>>Const.SNS_BITS
+                snsType  = 0x1F&dataType_snsType
+                #print(dataType_snsType)
+                #print(dataType)
+                #print(snsType)
+                #print(dataNum)
+                dataSize = Const.sizeMap[Const.typeMap[dataType]]
+                return (dataSize*dataNum+4)
+
+
+        def printHex(self, frame):
+                #print("PH")
+                frameLen = self.frame_len(frame)
+                i = 0
+                while i<frameLen:
+                        print(hex(ord(frame[i])))
+                        i+=1
+

 
        # Private methods
 
        # Private methods

-       def _write_ascii(self, line):
+       def _write_ascii(self, line):                           # to be changed
+                #print("WA")

                if self.serial:
                        print('write: [' + line + ']')
                if self.serial:
                        print('write: [' + line + ']')

-                       data = bytes(line+'\n', 'UTF-8')
-                       self.serial.write(data)
+                       self.serial.write(line + '\n')

                        self.serial.flush()
                        time.sleep(0.1)
 
                        self.serial.flush()
                        time.sleep(0.1)
 

-       def _parse_ascii(self, line):
-               acc_re = compile("\[ACC\] accX=(.*) accY=(.*) accZ=(.*)")
-               mag_re = compile("\[MAG\] magX=(.*) magY=(.*) magZ=(.*)")
-               lgt_re = compile("\[LGT\] Light Intensity=(.*)")
-               tch_re = compile("\[TCH\] Force=(.*) Distance=(.*)")
-               a2d_re = compile("\[A2D\] ...")
-
-               acc_m = acc_re.match(line)
-               mag_m = mag_re.match(line)
-               lgt_m = lgt_re.match(line)
-               tch_m = tch_re.match(line)
-               a2d_m = a2d_re.match(line)
-
-               state = State()
-               if acc_m:
-                       state.acc[0]   = float(acc_m.group(1))
-                       state.acc[1]   = float(acc_m.group(2))
-                       state.acc[2]   = float(acc_m.group(3))
-               if mag_m:              
-                       state.mag[0]   = float(mag_m.group(1))
-                       state.mag[1]   = float(mag_m.group(2))
-                       state.mag[2]   = float(mag_m.group(3))
-               if lgt_m:
-                       state.light[0] = float(lgt_m.group(1))
-               if tch_m:
-                       state.touch[0] = float(tch_m.group(1))
-               if a2d_m:
-                       state.a2d[0]   = float(tch_m.group(1))
-                       state.a2d[1]   = float(tch_m.group(2))
-                       state.a2d[2]   = float(tch_m.group(3))
-                       state.a2d[3]   = float(tch_m.group(4))
-                       state.a2d[4]   = float(tch_m.group(5))
-                       state.a2d[5]   = float(tch_m.group(6))
-
-               return state
+       def _parse_ascii(self, line):##############
+                #print("PA")
+                dataType_snsType = ord(line[1]);
+                dataNum  = ord(line[2]);
+                dataType = (0xE0&dataType_snsType)>>Const.SNS_BITS
+                snsType  = 0x1F&dataType_snsType
+                state = State()
+                if snsType == Const.ACC_SNS:
+                        line = line[3:15]
+                        state.acc = unpack('3f',line)
+                elif snsType == Const.MAG_SNS:
+                        line = line[3:9]
+                        state.mag = unpack('3h',line)
+                elif snsType == Const.LGT_SNS:
+                        state.light[0] = ord(line[3])
+                elif snsType == Const.TCH_SNS:
+                        line = line[3:11]
+                        state.touch = sunpack('2f',line)
+                elif snsType == Const.ADC_SNS_1:
+                        line = line[3:15]
+                        state.a2d = unpack('6h', line)
+                else:
+                        print('[ERROR] Nothing Happened!')
+                return state

diff --git a/vis/logger.py b/vis/logger.py
index 84b6e0d5e24d3da740dd75287bddfb84a5bd3065..f7fce7baa69fd37f7b01fe533108b48957b7553b 100644 (file)
--- a/vis/logger.py
+++ b/vis/logger.py
@@ -11,7 +11,7 @@ class Logger:
        queue = []
        last  = 0
 
        queue = []
        last  = 0
 

-       # Methods
+       # Constructor

        def __init__(self, config):
                self.config = config
 
        def __init__(self, config):
                self.config = config
 

diff --git a/vis/visual.ui b/vis/visual.ui
index e614321ae812164467744816df21d6f3bcea921f..399dc9be466920a64c7de398fa774d72ffdbe1e7 100644 (file)
--- a/vis/visual.ui
+++ b/vis/visual.ui
@@ -11,7 +11,7 @@
   <object class="GtkAdjustment" id="acc_adj">
     <property name="upper">100</property>
     <property name="value">1</property>
   <object class="GtkAdjustment" id="acc_adj">
     <property name="upper">100</property>
     <property name="value">1</property>

-    <property name="step_increment">1</property>
+    <property name="step_increment">0.10000000000000001</property>

     <property name="page_increment">10</property>
   </object>
   <object class="GtkAdjustment" id="baud_adj">
     <property name="page_increment">10</property>
   </object>
   <object class="GtkAdjustment" id="baud_adj">


@@ -683,6 +683,7 @@
                     <property name="primary_icon_sensitive">True</property>
                     <property name="secondary_icon_sensitive">True</property>
                     <property name="adjustment">a2d_adj</property>
                     <property name="primary_icon_sensitive">True</property>
                     <property name="secondary_icon_sensitive">True</property>
                     <property name="adjustment">a2d_adj</property>

+                    <property name="digits">5</property>

                     <property name="numeric">True</property>
                     <signal name="value-changed" handler="on_rate" swapped="no"/>
                   </object>
                     <property name="numeric">True</property>
                     <signal name="value-changed" handler="on_rate" swapped="no"/>
                   </object>


@@ -704,6 +705,7 @@
                     <property name="primary_icon_sensitive">True</property>
                     <property name="secondary_icon_sensitive">True</property>
                     <property name="adjustment">acc_adj</property>
                     <property name="primary_icon_sensitive">True</property>
                     <property name="secondary_icon_sensitive">True</property>
                     <property name="adjustment">acc_adj</property>

+                    <property name="digits">5</property>

                     <property name="numeric">True</property>
                     <signal name="value-changed" handler="on_rate" swapped="no"/>
                   </object>
                     <property name="numeric">True</property>
                     <signal name="value-changed" handler="on_rate" swapped="no"/>
                   </object>


@@ -725,6 +727,7 @@
                     <property name="primary_icon_sensitive">True</property>
                     <property name="secondary_icon_sensitive">True</property>
                     <property name="adjustment">mag_adj</property>
                     <property name="primary_icon_sensitive">True</property>
                     <property name="secondary_icon_sensitive">True</property>
                     <property name="adjustment">mag_adj</property>

+                    <property name="digits">5</property>

                     <property name="numeric">True</property>
                     <signal name="value-changed" handler="on_rate" swapped="no"/>
                   </object>
                     <property name="numeric">True</property>
                     <signal name="value-changed" handler="on_rate" swapped="no"/>
                   </object>


@@ -746,6 +749,7 @@
                     <property name="primary_icon_sensitive">True</property>
                     <property name="secondary_icon_sensitive">True</property>
                     <property name="adjustment">touch_adj</property>
                     <property name="primary_icon_sensitive">True</property>
                     <property name="secondary_icon_sensitive">True</property>
                     <property name="adjustment">touch_adj</property>

+                    <property name="digits">5</property>

                     <property name="numeric">True</property>
                     <signal name="value-changed" handler="on_rate" swapped="no"/>
                   </object>
                     <property name="numeric">True</property>
                     <signal name="value-changed" handler="on_rate" swapped="no"/>
                   </object>


@@ -767,6 +771,7 @@
                     <property name="primary_icon_sensitive">True</property>
                     <property name="secondary_icon_sensitive">True</property>
                     <property name="adjustment">light_adj</property>
                     <property name="primary_icon_sensitive">True</property>
                     <property name="secondary_icon_sensitive">True</property>
                     <property name="adjustment">light_adj</property>

+                    <property name="digits">5</property>

                     <property name="numeric">True</property>
                     <signal name="value-changed" handler="on_rate" swapped="no"/>
                   </object>
                     <property name="numeric">True</property>
                     <signal name="value-changed" handler="on_rate" swapped="no"/>
                   </object>

diff --git a/yue/main.cpp b/yue/main.cpp
index 5b82803062bc8f9865ece13ebef0cbabd8b78540..d3addb1e679d7b004e67ff01d77631c11b91a32b 100644 (file)
--- a/yue/main.cpp
+++ b/yue/main.cpp
@@ -1,213 +1,369 @@
-#include "mbed.h"
-#include "TSISensor.h"          // Touch Sensor
-#include "MAG3110.h"            // Magnetic Sensor
-#include "MMA8451Q.h"           // AcceleroMeter
-
-
-#define MMA8451_I2C_ADDRESS (0x1d<<1)   // acc sensor address
-#define TIME_ACCURACY 0.1
-#define LIGHT_SNS_DEFAULT 1             // default collecting interval in seconds
-#define ACC_SNS_DEFAULT 1
-#define MAG_SNS_DEFAULT 1
-#define TOUCH_SNS_DEFAULT 1
-
-// Define Devices & Pins
-MMA8451Q accSensor(PTE25, PTE24, MMA8451_I2C_ADDRESS);
-TSISensor touchSensor;
-MAG3110 magSensor(PTE25, PTE24);
-Serial serial(USBTX, USBRX);
-Ticker clock1;
-AnalogIn lightSensor(PTE22);
-
-
-// Global Variables
-// Initial interval: in seconds
-float lightTmr = LIGHT_SNS_DEFAULT;
-float accTmr = ACC_SNS_DEFAULT;
-float magTmr = MAG_SNS_DEFAULT;
-float touchTmr = TOUCH_SNS_DEFAULT;
-
-bool lightEnable = true;
-bool accEnable = true;
-bool magEnable = true;
-bool touchEnable = true;
-
-void clock1_interrupt();
-void serialRx_interrupt();
-void sendLightInfo();
-void sendAccInfo();
-void sendMagInfo();
-void sendTouchInfo();
-
-
-int main() {
-    // Initialization
-    // Interruption Declarations
-    clock1.attach(&clock1_interrupt, TIME_ACCURACY);    // maximun accuracy be 0.1s
-    serial.attach(&serialRx_interrupt, Serial::RxIrq);  // receive interrupt for serialS
-    serial.baud(921600);
-    magSensor.begin();
-
-    serial.printf("\r\n============= Start of the program ============\r\n");
-    while(1){
-        wait(3);
-    }
-}
-
-
-/*---------------------------------------------------------------
-  ## Receive Interruption of the Serial ##
-  -> used to receive & process user command
-  -> and configure the board accordingly
-  ---------------------------------------------------------------*/
-void serialRx_interrupt(){
-    clock1.detach();                // close the interrupt
-    serial.printf("\r\n");
-
-    // Receive the Serial Input
-    float interval;
-    char buffer[255];
-    char temp[255];
-    char ch = serial.getc();
-    int i;
-    for (i=0 ; ch!='\n' && ch!='\r'; i++){
-        serial.putc(ch);
-        buffer[i] = ch;
-        if (ch==127){                // BackSpace
-            i--;
-            i--;
-        }
-        ch = serial.getc();
-    }
-    buffer[i] = '\0';
-    serial.printf("\r\nBUFFER: %s %d\r\n", buffer,i);
-    // TODO: buffer -> lower case
-
-    // Process the Serial Input
-    // Set-Interval Command
-    if (strstr(buffer, "set")!=NULL && strstr(buffer, "int")!= NULL){
-        sscanf(buffer, "%*[^0123456789.]%s", temp);       // find the number in buffer
-        sscanf(temp, "%f", &interval);                    // translate into float
-        if (interval<0.1 || interval>5){
-            interval  = 1;
-        }
-        if (strstr(buffer, "acc")){
-            accTmr = interval;
-        }
-        if (strstr(buffer, "mag")){
-            magTmr = interval;
-        }
-        if (strstr(buffer, "light")){
-            lightTmr = interval;
-        }
-        if (strstr(buffer, "touch")){
-            touchTmr = interval;
-        }
-    }
-
-    // Stop Command
-    else if (strstr(buffer, "stop")!= NULL){
-        serial.printf("STOP\r\n");
-        if (strstr(buffer, "acc")){
-            accEnable = false;
-            accTmr = ACC_SNS_DEFAULT;
-        }
-        if (strstr(buffer, "mag")){
-            magEnable = false;
-            magTmr = MAG_SNS_DEFAULT;
-        }
-        if (strstr(buffer, "light")){
-            lightEnable = false;
-            lightTmr = LIGHT_SNS_DEFAULT;
-        }
-        if (strstr(buffer, "touch")){
-            touchEnable = false;
-            touchTmr = TOUCH_SNS_DEFAULT;
-        }
-    }
-
-    // Start Command
-    else if (strstr(buffer, "start")!=NULL){
-        if (strstr(buffer, "acc") && !accEnable){
-            accEnable = true;
-            accTmr = ACC_SNS_DEFAULT;
-        }
-        if (strstr(buffer, "mag") && !magEnable){
-            magEnable = true;
-            magTmr = MAG_SNS_DEFAULT;
-        }
-        if (strstr(buffer, "light") && !lightEnable){
-            lightEnable = true;
-            lightTmr = LIGHT_SNS_DEFAULT;
-        }
-        if (strstr(buffer, "touch") && !touchEnable){
-            touchEnable = true;
-            touchTmr = TOUCH_SNS_DEFAULT;
-        }
-    }
-    clock1.attach(&clock1_interrupt,0.1);
-}
-
-
-void clock1_interrupt(){
-    static int accCnt;
-    static int magCnt;
-    static int lightCnt;
-    static int touchCnt;
-
-    accCnt++;
-    magCnt++;
-    lightCnt++;
-    touchCnt++;
-
-    // TODO: send data through Serial
-    if (lightEnable && (lightCnt<0 || lightCnt>=lightTmr/TIME_ACCURACY)){
-        sendLightInfo();
-        lightCnt = 0;
-    }
-    if (magEnable && (magCnt<0 || magCnt>=magTmr/TIME_ACCURACY)){
-        sendMagInfo();
-        magCnt = 0;
-    }
-    if (touchEnable && (touchCnt<0 || touchCnt>=touchTmr/TIME_ACCURACY)){
-        sendTouchInfo();
-        touchCnt = 0;
-    }
-    if (accEnable && (accCnt<0 || accCnt>=accTmr/TIME_ACCURACY)){
-        sendAccInfo();
-        accCnt = 0;
-    }
-}
-
-void sendLightInfo(){
-    serial.printf("[LGT] Light Intensity=%f\r\n", lightSensor.read());
-}
-
-
-void sendAccInfo(){
-    // get acc data
-    float accX = accSensor.getAccX();
-    float accY = accSensor.getAccY();
-    float accZ = accSensor.getAccZ();
-
-    // send acc data
-    serial.printf("[ACC] accX=%-2.4f accY=%-2.4f accZ=%-2.4f\r\n",accX,accY,accZ);
-}
-
-void sendTouchInfo(){
-    // get data
-    float touchForce = touchSensor.readPercentage();
-    float distance = touchSensor.readDistance();
-
-    // send data
-    serial.printf("[TCH] Force=%0.4f Distance=%2.2f\r\n", touchForce, distance);
-}
-
-void sendMagInfo(){
-    // get data
-    int magX, magY, magZ;
-    magSensor.getValues(&magX, &magY, &magZ);
-
-    // send data
-    serial.printf("[MAG] magX=%d magY=%d magZ=%d\r\n",magX,magY,magZ);
-}
+#include "mbed.h"\r
+#include "TSISensor.h"          // Touch Sensor\r
+#include "MAG3110.h"            // Magnetic Sensor\r
+#include "MMA8451Q.h"           // AcceleroMeter\r
+//#include "DataFrame.h"\r
+\r
+\r
+\r
+#define MMA8451_I2C_ADDRESS (0x1d<<1)   // acc sensor address\r
+#define TIME_ACCURACY     0.1\r
+#define LGT_SNS_DEFAULT   2             // default collecting interval in seconds\r
+#define ACC_SNS_DEFAULT   2\r
+#define MAG_SNS_DEFAULT   2\r
+#define TCH_SNS_DEFAULT   2\r
+\r
+// Parameters in Data Frame\r
+// TODO: need to sync with the pc program\r
+\r
+#define HEADER    0x02\r
+\r
+#define SNS_BITS  5\r
+#define LGT_SNS   0x00\r
+#define ACC_SNS   0x01\r
+#define MAG_SNS   0x02\r
+#define TCH_SNS   0x03\r
+#define ADC_SNS_1 0x04\r
+\r
+#define INT    0\r
+#define LONG   1\r
+#define FLOAT  2\r
+#define DOUBLE 3\r
+#define TAIL   0x0A             // '\n'\r
+\r
+// Command Frame\r
+#define START   0\r
+#define STOP    1\r
+#define SET_INT 2\r
+\r
+\r
+\r
+#define MAX_FRAME_LEN 255\r
+// Define Devices & Pins\r
+MMA8451Q accSensor(PTE25, PTE24, MMA8451_I2C_ADDRESS);\r
+TSISensor touchSensor;\r
+MAG3110 magSensor(PTE25, PTE24);\r
+Serial serial(USBTX, USBRX);\r
+Ticker clock1;\r
+AnalogIn lightSensor(PTE22);\r
+\r
+\r
+// Global Variables\r
+// Initial interval: in seconds\r
+float lgtTmr = LGT_SNS_DEFAULT;\r
+float accTmr = ACC_SNS_DEFAULT;\r
+float magTmr = MAG_SNS_DEFAULT;\r
+float tchTmr = TCH_SNS_DEFAULT;\r
+\r
+bool lgtEnable = false;\r
+bool accEnable = false;\r
+bool magEnable = true;\r
+bool tchEnable = false;\r
+\r
+void clock1_interrupt();\r
+void serialRx_interrupt();\r
+void sendLightInfo();\r
+void sendAccInfo();\r
+void sendMagInfo();\r
+void sendTouchInfo();\r
+int packToFrame(char*, char, char, int, void*);\r
+char* floatToByte(float);\r
+float byteToFloat(char*);\r
+int calDataSize(char);\r
+void printHex(char*);\r
+void sendFrame(char*, int);\r
+\r
+\r
+int main() {\r
+    // Initialization\r
+    // Interruption Declarations\r
+    clock1.attach(&clock1_interrupt, TIME_ACCURACY);    // maximun accuracy be 0.1s\r
+    serial.attach(&serialRx_interrupt, Serial::RxIrq);  // receive interrupt for serialS\r
+    magSensor.begin();\r
+\r
+    serial.printf("\r\n============= Start of the program ============\r\n");\r
+    while(1){\r
+        wait(3);\r
+    }\r
+}\r
+\r
+\r
+/*---------------------------------------------------------------\r
+  ## Receive Interruption of the Serial ##\r
+  -> used to receive & process user command\r
+  -> and configure the board accordingly\r
+  ---------------------------------------------------------------*/\r
+/*void serialRx_interrupt_userFriendlyVersion(){\r
+    clock1.detach();                // close the interrupt\r
+    serial.printf("\r\n");\r
+\r
+    // Receive the Serial Input\r
+    float interval;\r
+    char buffer[255];\r
+    char temp[255];\r
+    char ch = serial.getc();\r
+    int i;\r
+    for (i=0 ; ch!='\n' && ch!='\r'; i++){\r
+        serial.putc(ch);\r
+        buffer[i] = ch;\r
+        if (ch==127){                // BackSpace\r
+            i--;\r
+            i--;\r
+        }\r
+        ch = serial.getc();\r
+    }\r
+    buffer[i] = '\0';\r
+    serial.printf("\r\nBUFFER: %s %d\r\n", buffer,i);\r
+    // TODO: buffer -> lower case\r
+\r
+    // Process the Serial Input\r
+    // Set-Interval Command\r
+    if (strstr(buffer, "set")!=NULL && strstr(buffer, "int")!= NULL){\r
+        sscanf(buffer, "%*[^0123456789.]%s", temp);       // find the number in buffer\r
+        sscanf(temp, "%f", &interval);                    // translate into float\r
+        if (interval<0.1 || interval>5){\r
+            interval  = 1;\r
+        }\r
+        if (strstr(buffer, "acc")){\r
+            accTmr = interval;\r
+        }\r
+        if (strstr(buffer, "mag")){\r
+            magTmr = interval;\r
+        }\r
+        if (strstr(buffer, "light")){\r
+            lgtTmr = interval;\r
+        }\r
+        if (strstr(buffer, "touch")){\r
+            tchTmr = interval;\r
+        }\r
+    }\r
+\r
+    // Stop Command\r
+    else if (strstr(buffer, "stop")!= NULL){\r
+        serial.printf("STOP\r\n");\r
+        if (strstr(buffer, "acc")){\r
+            accEnable = false;\r
+            accTmr = ACC_SNS_DEFAULT;\r
+        }\r
+        if (strstr(buffer, "mag")){\r
+            magEnable = false;\r
+            magTmr = MAG_SNS_DEFAULT;\r
+        }\r
+        if (strstr(buffer, "light")){\r
+            lgtEnable = false;\r
+            lgtTmr = LGT_SNS_DEFAULT;\r
+        }\r
+        if (strstr(buffer, "touch")){\r
+            tchEnable = false;\r
+            tchTmr = TCH_SNS_DEFAULT;\r
+        }\r
+    }\r
+\r
+    // Start Command\r
+    else if (strstr(buffer, "start")!=NULL){\r
+        if (strstr(buffer, "acc") && !accEnable){\r
+            accEnable = true;\r
+            accTmr = ACC_SNS_DEFAULT;\r
+        }\r
+        if (strstr(buffer, "mag") && !magEnable){\r
+            magEnable = true;\r
+            magTmr = MAG_SNS_DEFAULT;\r
+        }\r
+        if (strstr(buffer, "light") && !lgtEnable){\r
+            lgtEnable = true;\r
+            lgtTmr = LGT_SNS_DEFAULT;\r
+        }\r
+        if (strstr(buffer, "touch") && !tchEnable){\r
+            tchEnable = true;\r
+            tchTmr = TCH_SNS_DEFAULT;\r
+        }\r
+    }\r
+    clock1.attach(&clock1_interrupt,TIME_ACCURACY);\r
+}\r
+*/\r
+\r
+void serialRx_interrupt(){                          // Byte version\r
+    clock1.detach();                                // close the interrupt temporarily\r
+    int i = 0;\r
+    char frame[MAX_FRAME_LEN];\r
+    char ch = serial.getc();\r
+    while (ch!=HEADER){\r
+        if (serial.readable()){\r
+            ch = serial.getc();\r
+        }\r
+        else{\r
+            serial.printf("[ERROR] broken data!\r\n");\r
+            clock1.attach(&clock1_interrupt,TIME_ACCURACY);\r
+            return;\r
+        }\r
+    }\r
+    //TODO: ticker for time out\r
+    while (serial.readable()){\r
+        frame[i] = ch;\r
+        if (ch=='\n'){\r
+            break;\r
+        }\r
+        ch = serial.getc();\r
+        i++;\r
+    }\r
+    frame[++i] = '\0';\r
+    float interval=-1;\r
+    char snsType = frame[1]|0x1F;\r
+    char oper = (frame[1]|0xE0)>>SNS_BITS;              // operation type\r
+    if (oper==SET_INT){\r
+        memcpy(&interval,&frame[2],4);\r
+        if (interval<0.0001||interval>10){\r
+            interval = 1;\r
+        }\r
+    }\r
+    switch(snsType){\r
+        case ACC_SNS:\r
+        {\r
+            switch(oper){\r
+                case START:     accEnable = true;  break;\r
+                case STOP:      accEnable = false; break;\r
+                case SET_INT:   accTmr = interval; break;\r
+            }\r
+        }\r
+        case MAG_SNS:\r
+        {\r
+            switch(oper){\r
+                case START:     magEnable = true;  break;\r
+                case STOP:      magEnable = false; break;\r
+                case SET_INT:   magTmr = interval; break;\r
+            }\r
+        }\r
+        case LGT_SNS:\r
+        {\r
+            switch(oper){\r
+                case START:     lgtEnable = true;  break;\r
+                case STOP:      lgtEnable = false; break;\r
+                case SET_INT:   lgtTmr = interval; break;\r
+            }\r
+        }\r
+        case TCH_SNS:\r
+        {\r
+            switch(oper){\r
+                case START:     tchEnable = true;  break;\r
+                case STOP:      tchEnable = false; break;\r
+                case SET_INT:   tchTmr = interval; break;\r
+            }\r
+        }\r
+    }\r
+    clock1.attach(&clock1_interrupt,TIME_ACCURACY);\r
+}\r
+\r
+void clock1_interrupt(){\r
+    static int accCnt;\r
+    static int magCnt;\r
+    static int lgtCnt;\r
+    static int tchCnt;\r
+\r
+    accCnt++;\r
+    magCnt++;\r
+    lgtCnt++;\r
+    tchCnt++;\r
+\r
+    // TODO: send data through Serial\r
+    if (lgtEnable && (lgtCnt<0 || lgtCnt>=lgtTmr/TIME_ACCURACY)){\r
+        sendLightInfo();\r
+        lgtCnt = 0;\r
+    }\r
+    if (magEnable && (magCnt<0 || magCnt>=magTmr/TIME_ACCURACY)){\r
+        sendMagInfo();\r
+        magCnt = 0;\r
+    }\r
+    if (tchEnable && (tchCnt<0 || tchCnt>=tchTmr/TIME_ACCURACY)){\r
+        sendTouchInfo();\r
+        tchCnt = 0;\r
+    }\r
+    if (accEnable && (accCnt<0 || accCnt>=accTmr/TIME_ACCURACY)){\r
+        sendAccInfo();\r
+        accCnt = 0;\r
+    }\r
+}\r
+\r
+void sendLightInfo(){\r
+    char frame[MAX_FRAME_LEN];\r
+    float lightData = lightSensor.read();\r
+    packToFrame(frame,LGT_SNS,FLOAT,1,&lightData);\r
+    serial.printf("[LGT] %s\r\n", frame);\r
+    //printHex(frame);\r
+    //serial.printf("[LGT] Light Intensity=%f\r\n", lightSensor.read());\r
+}\r
+\r
+\r
+void sendTouchInfo(){\r
+    char frame[MAX_FRAME_LEN];\r
+    float touchData[2];\r
+    touchData[0] = touchSensor.readPercentage();\r
+    touchData[1] = touchSensor.readDistance();\r
+    packToFrame(frame,TCH_SNS,FLOAT,2,touchData);\r
+    serial.printf("[TCH] %s\r\n", frame);\r
+    //printHex(frame);\r
+}\r
+\r
+void sendMagInfo(){\r
+    char frame[MAX_FRAME_LEN];\r
+    int magData[3];\r
+    magSensor.getValues(magData, magData+1, magData+2);\r
+    //serial.printf("[MAG] magX=%d magY=%d magZ=%d\r\n",magData[0],magData[1],magData[2]);\r
+    int len = packToFrame(frame,MAG_SNS,INT,3,magData);\r
+    sendFrame(frame, len);\r
+    //memcpy(magData, &frame[3], 2);\r
+    //serial.printf("[MAG] %d\r\n", magData[0]);\r
+    //printHex(frame);\r
+}\r
+\r
+void sendAccInfo(){\r
+    // get acc data\r
+    char frame[MAX_FRAME_LEN];\r
+    float accData[3];\r
+    accData[0] = accSensor.getAccX();\r
+    accData[1] = accSensor.getAccY();\r
+    accData[2] = accSensor.getAccZ();\r
+    packToFrame(frame, ACC_SNS,FLOAT,3,accData);\r
+    serial.printf("[ACC] %s\n\r", frame);\r
+    //int num = int(frame[2]);\r
+    //memcpy(&accZ, &frame[3], 4);\r
+    // send acc data\r
+    //printHex(frame);\r
+}\r
+int calDataSize(char dataType){\r
+    switch(dataType){\r
+        case INT:    return 2;\r
+        case LONG:   return 4;\r
+        case FLOAT:  return 4;\r
+        case DOUBLE: return 8;\r
+    }\r
+    return 4;\r
+}\r
+\r
+void printHex(char* frame){\r
+    int dataType = frame[1]&0xE0;\r
+    int dataSize = calDataSize(dataType);\r
+    int dataNum = frame[2];\r
+    for (int i=0; i<4+dataSize*dataNum; i++){\r
+        serial.printf("0x%x ",frame[i]);\r
+    }\r
+    serial.printf("\r\n");\r
+}\r
+\r
+int packToFrame(char* frame, char snsType,char dataType, int dataNum, void* data){\r
+    int dataSize = calDataSize(dataType);\r
+    frame[0] = HEADER;\r
+    frame[1] = (snsType|(dataType<<SNS_BITS));\r
+    frame[2] = dataNum;\r
+    memcpy(frame+3, data, dataSize*dataNum);\r
+    frame[3+dataNum*dataSize]= TAIL;\r
+    frame[4+dataNum*dataSize]= '\0';\r
+    frame[3] = 0x00;\r
+    return (4+dataNum*dataSize);\r
+}\r
+\r
+\r
+void sendFrame (char* frame, int len){\r
+    for (int i=0; i<len; i++){\r
+        serial.putc(frame[i]);\r
+    }\r
+}\r