X-Git-Url: http://pileus.org/git/?a=blobdiff_plain;f=vis%2Fdevice.py;h=34a8be483dff2e9ac1a8a83de8a5744561192093;hb=12ef45a56c075d0587534a79451a999e9be51f8e;hp=24860bac33c430cd2aef444208d9bdd2dad08700;hpb=925a7238a8f844fed05b8c4074fed829e7b2a892;p=~andy%2Fcsm213a-hw

diff --git a/vis/device.py b/vis/device.py
index 24860ba..34a8be4 100644
--- a/vis/device.py
+++ b/vis/device.py
@@ -1,8 +1,45 @@
+import time
+
 from re       import compile
 from serial   import Serial
 from datetime import datetime
+from struct   import *
+
+
+
+#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:
+
+class State:                                    #information stored
 	acc   = [None]*3
 	mag   = [None]*3
 	touch = [None]*2
@@ -14,103 +51,164 @@ class State:
 		self.time = datetime.utcnow()
 
 class Device:
-	# Attributes
-	port = "/dev/ttyACM0"
-
 	# Constructors
 	def __init__(self, config):
+                print("IN")
 		self.config = config
 		self.serial = None
 
 	# Methods
 	def connect(self):
+                print("C")
+                buildingFrame = 0
 		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, \
 				stopbits = self.config.stopbits, \
 				timeout  = 0)
+			self.control()
 			self.serial.flushInput()
 		except Exception as ex:
 			return str(ex)
 
 	def disconnect(self):
+                print("DC")
 		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
 
-	def control(self):
-		pass
+	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]
+			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
-		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)
-				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
-	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 _write_ascii(self, line):                           # to be changed
+                #print("WA")
+		if self.serial:
+			print('write: [' + line + ']')
+			self.serial.write(line + '\n')
+			self.serial.flush()
+			time.sleep(0.1)
+
+	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