From 12ef45a56c075d0587534a79451a999e9be51f8e Mon Sep 17 00:00:00 2001 From: Andy Spencer Date: Sun, 9 Feb 2014 05:54:00 +0000 Subject: [PATCH] Pull changes from Yue --- vis/device.py | 225 ++++++++++++------- vis/logger.py | 2 +- vis/visual.ui | 7 +- yue/main.cpp | 582 ++++++++++++++++++++++++++++++++------------------ 4 files changed, 529 insertions(+), 287 deletions(-) diff --git a/vis/device.py b/vis/device.py index a896375..34a8be4 100644 --- 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 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 @@ -16,19 +51,19 @@ 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, \ @@ -40,94 +75,140 @@ class Device: 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): + 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 = '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 - 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>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 84b6e0d..f7fce7b 100644 --- a/vis/logger.py +++ b/vis/logger.py @@ -11,7 +11,7 @@ class Logger: queue = [] last = 0 - # Methods + # Constructor def __init__(self, config): self.config = config diff --git a/vis/visual.ui b/vis/visual.ui index e614321..399dc9b 100644 --- a/vis/visual.ui +++ b/vis/visual.ui @@ -11,7 +11,7 @@ 100 1 - 1 + 0.10000000000000001 10 @@ -683,6 +683,7 @@ True True a2d_adj + 5 True @@ -704,6 +705,7 @@ True True acc_adj + 5 True @@ -725,6 +727,7 @@ True True mag_adj + 5 True @@ -746,6 +749,7 @@ True True touch_adj + 5 True @@ -767,6 +771,7 @@ True True light_adj + 5 True diff --git a/yue/main.cpp b/yue/main.cpp index 5b82803..d3addb1 100644 --- 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" +#include "TSISensor.h" // Touch Sensor +#include "MAG3110.h" // Magnetic Sensor +#include "MMA8451Q.h" // AcceleroMeter +//#include "DataFrame.h" + + + +#define MMA8451_I2C_ADDRESS (0x1d<<1) // acc sensor address +#define TIME_ACCURACY 0.1 +#define LGT_SNS_DEFAULT 2 // default collecting interval in seconds +#define ACC_SNS_DEFAULT 2 +#define MAG_SNS_DEFAULT 2 +#define TCH_SNS_DEFAULT 2 + +// Parameters in Data Frame +// TODO: need to sync with the pc program + +#define HEADER 0x02 + +#define SNS_BITS 5 +#define LGT_SNS 0x00 +#define ACC_SNS 0x01 +#define MAG_SNS 0x02 +#define TCH_SNS 0x03 +#define ADC_SNS_1 0x04 + +#define INT 0 +#define LONG 1 +#define FLOAT 2 +#define DOUBLE 3 +#define TAIL 0x0A // '\n' + +// Command Frame +#define START 0 +#define STOP 1 +#define SET_INT 2 + + + +#define MAX_FRAME_LEN 255 +// 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 lgtTmr = LGT_SNS_DEFAULT; +float accTmr = ACC_SNS_DEFAULT; +float magTmr = MAG_SNS_DEFAULT; +float tchTmr = TCH_SNS_DEFAULT; + +bool lgtEnable = false; +bool accEnable = false; +bool magEnable = true; +bool tchEnable = false; + +void clock1_interrupt(); +void serialRx_interrupt(); +void sendLightInfo(); +void sendAccInfo(); +void sendMagInfo(); +void sendTouchInfo(); +int packToFrame(char*, char, char, int, void*); +char* floatToByte(float); +float byteToFloat(char*); +int calDataSize(char); +void printHex(char*); +void sendFrame(char*, int); + + +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 + 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_userFriendlyVersion(){ + 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")){ + lgtTmr = interval; + } + if (strstr(buffer, "touch")){ + tchTmr = 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")){ + lgtEnable = false; + lgtTmr = LGT_SNS_DEFAULT; + } + if (strstr(buffer, "touch")){ + tchEnable = false; + tchTmr = TCH_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") && !lgtEnable){ + lgtEnable = true; + lgtTmr = LGT_SNS_DEFAULT; + } + if (strstr(buffer, "touch") && !tchEnable){ + tchEnable = true; + tchTmr = TCH_SNS_DEFAULT; + } + } + clock1.attach(&clock1_interrupt,TIME_ACCURACY); +} +*/ + +void serialRx_interrupt(){ // Byte version + clock1.detach(); // close the interrupt temporarily + int i = 0; + char frame[MAX_FRAME_LEN]; + char ch = serial.getc(); + while (ch!=HEADER){ + if (serial.readable()){ + ch = serial.getc(); + } + else{ + serial.printf("[ERROR] broken data!\r\n"); + clock1.attach(&clock1_interrupt,TIME_ACCURACY); + return; + } + } + //TODO: ticker for time out + while (serial.readable()){ + frame[i] = ch; + if (ch=='\n'){ + break; + } + ch = serial.getc(); + i++; + } + frame[++i] = '\0'; + float interval=-1; + char snsType = frame[1]|0x1F; + char oper = (frame[1]|0xE0)>>SNS_BITS; // operation type + if (oper==SET_INT){ + memcpy(&interval,&frame[2],4); + if (interval<0.0001||interval>10){ + interval = 1; + } + } + switch(snsType){ + case ACC_SNS: + { + switch(oper){ + case START: accEnable = true; break; + case STOP: accEnable = false; break; + case SET_INT: accTmr = interval; break; + } + } + case MAG_SNS: + { + switch(oper){ + case START: magEnable = true; break; + case STOP: magEnable = false; break; + case SET_INT: magTmr = interval; break; + } + } + case LGT_SNS: + { + switch(oper){ + case START: lgtEnable = true; break; + case STOP: lgtEnable = false; break; + case SET_INT: lgtTmr = interval; break; + } + } + case TCH_SNS: + { + switch(oper){ + case START: tchEnable = true; break; + case STOP: tchEnable = false; break; + case SET_INT: tchTmr = interval; break; + } + } + } + clock1.attach(&clock1_interrupt,TIME_ACCURACY); +} + +void clock1_interrupt(){ + static int accCnt; + static int magCnt; + static int lgtCnt; + static int tchCnt; + + accCnt++; + magCnt++; + lgtCnt++; + tchCnt++; + + // TODO: send data through Serial + if (lgtEnable && (lgtCnt<0 || lgtCnt>=lgtTmr/TIME_ACCURACY)){ + sendLightInfo(); + lgtCnt = 0; + } + if (magEnable && (magCnt<0 || magCnt>=magTmr/TIME_ACCURACY)){ + sendMagInfo(); + magCnt = 0; + } + if (tchEnable && (tchCnt<0 || tchCnt>=tchTmr/TIME_ACCURACY)){ + sendTouchInfo(); + tchCnt = 0; + } + if (accEnable && (accCnt<0 || accCnt>=accTmr/TIME_ACCURACY)){ + sendAccInfo(); + accCnt = 0; + } +} + +void sendLightInfo(){ + char frame[MAX_FRAME_LEN]; + float lightData = lightSensor.read(); + packToFrame(frame,LGT_SNS,FLOAT,1,&lightData); + serial.printf("[LGT] %s\r\n", frame); + //printHex(frame); + //serial.printf("[LGT] Light Intensity=%f\r\n", lightSensor.read()); +} + + +void sendTouchInfo(){ + char frame[MAX_FRAME_LEN]; + float touchData[2]; + touchData[0] = touchSensor.readPercentage(); + touchData[1] = touchSensor.readDistance(); + packToFrame(frame,TCH_SNS,FLOAT,2,touchData); + serial.printf("[TCH] %s\r\n", frame); + //printHex(frame); +} + +void sendMagInfo(){ + char frame[MAX_FRAME_LEN]; + int magData[3]; + magSensor.getValues(magData, magData+1, magData+2); + //serial.printf("[MAG] magX=%d magY=%d magZ=%d\r\n",magData[0],magData[1],magData[2]); + int len = packToFrame(frame,MAG_SNS,INT,3,magData); + sendFrame(frame, len); + //memcpy(magData, &frame[3], 2); + //serial.printf("[MAG] %d\r\n", magData[0]); + //printHex(frame); +} + +void sendAccInfo(){ + // get acc data + char frame[MAX_FRAME_LEN]; + float accData[3]; + accData[0] = accSensor.getAccX(); + accData[1] = accSensor.getAccY(); + accData[2] = accSensor.getAccZ(); + packToFrame(frame, ACC_SNS,FLOAT,3,accData); + serial.printf("[ACC] %s\n\r", frame); + //int num = int(frame[2]); + //memcpy(&accZ, &frame[3], 4); + // send acc data + //printHex(frame); +} +int calDataSize(char dataType){ + switch(dataType){ + case INT: return 2; + case LONG: return 4; + case FLOAT: return 4; + case DOUBLE: return 8; + } + return 4; +} + +void printHex(char* frame){ + int dataType = frame[1]&0xE0; + int dataSize = calDataSize(dataType); + int dataNum = frame[2]; + for (int i=0; i<4+dataSize*dataNum; i++){ + serial.printf("0x%x ",frame[i]); + } + serial.printf("\r\n"); +} + +int packToFrame(char* frame, char snsType,char dataType, int dataNum, void* data){ + int dataSize = calDataSize(dataType); + frame[0] = HEADER; + frame[1] = (snsType|(dataType<