from comm import * import sys import time from struct import * class AppTest: def __init__(self, adr_module, adr_sensor): self.serial_port = SerialComm('/dev/ttyUSB0') self.module = adr_module self.sensor = adr_sensor def finish(self): self.serial_port.close() def cmd_version(self): resp = self.serial_port.request(self.module, 0, CMD_VERSION,[]) version = chr(resp[3])+chr(resp[4])+chr(resp[5]) print("Version: "+version) def cmd_echo(self, msg): resp = self.serial_port.request(self.module, 0, CMD_ECHO,msg) echo = "" if len(resp) == 0: print("No ECHO (0-size resp)") return 0 print(resp) for r in range(len(msg)): echo = echo + chr(resp[3+r]) print("Echo:"+echo) return len(resp) def cmd_set_param(self, sensor, param, value): print("SET param:", PARAM_NAME[param], "[", param, "]=>", value) int_to_four_bytes = struct.Struct(' 2: val = resp[4] + resp[5]*256 + resp[6]*256*256 + resp[7] * 256*256*256 return val def cmd_get_params(self, sensor): print("GET params:") resp = self.serial_port.request(self.module, sensor, (CMD_PARAM),[]) def cmd_sensor_cnt(self): print("SENSOR CNT") resp = self.serial_port.request(self.module, 0, CMD_SENSOR_CNT,[]) return resp[3] def cmd_sensor_type(self, index): resp = self.serial_port.request(self.module, index, CMD_SENSOR_TYPE,[]) return resp[3] def cmd_sensors_type(self, pocet): resp = self.serial_port.request(self.module, 0, CMD_SENSOR_TYPE,[]) typy = []; for i in range(pocet): typy.append([resp[3+2*i],resp[4+2*i]]) return typy def cmd_sensor_get_params(self, index): resp = self.serial_port.request(self.module, index, CMD_PARAM,[]) return resp def cmd_sensor_get_data(self, index): resp = self.serial_port.request(self.module, index, CMD_DATA,[]) if len(resp)>1: h = resp[5] * 256 + resp[4] # print(hex(h)) return h/4096*3.3 return 0 def cmd_sensor_get_data_IMU(self, sensor_index): ''' sensor_index: 1 - ACC 2 - GYRO ''' resp = self.serial_port.request(self.module, sensor_index, CMD_DATA,[]) #print(resp) x = 0 y = 0 z = 0 if len(resp)>6: x = resp[5]*256 + resp[4] y = resp[7]*256 + resp[6] z = resp[9]*256 + resp[8] if x > 32768: x = x - 65535 if y > 32768: y = y - 65535 if z > 32768: z = z - 65535 return [x,y,z] def cmd_sensor_get_data_FSR(self, cnt): resp = self.serial_port.request(self.module, 0, CMD_DATA,[]) print(resp) if len(resp)>1: for i in range(cnt): sen = resp[3+3*i] h = resp[5+3*i] * 256 + resp[4+3*i] print("FSR",sen,":\t", h/4096.0*3.3) def cmd_module_info(self, param): resp = self.serial_port.request(self.module, 0, CMD_INFO,[param]) if param == 0xE3: data=0 for i in range(4): data=data+resp[i+3]*pow(256,i) return hex(data) if param == 0xE6: data=0 for i in range(8): data=data+resp[i+3]*pow(256,7-i) return hex(data) l = len(resp) data="" for i in range(l-4): data=data+chr(resp[i+3]) return data def cmd_module_start(self): print("MODULE START") self.serial_port.requestBroadcast(CMD_START,[]) def cmd_module_stop(self): print("MODULE STOP") self.serial_port.requestBroadcast(CMD_STOP,[]) def cmd_reset(self): print("MODULE RESET") resp = self.serial_port.request(self.module, 0, (SET+CMD_RESET),[], long_answer=0.3) def cmd_store(self, sensor): print("MODULE STORE PARAM") resp = self.serial_port.request(self.module, sensor, (SET+CMD_STORE),[], long_answer=0.1) if __name__ == "__main__": app = AppTest(0x05, 0x0) #app.cmd_version() #app.cmd_version() # app.cmd_module_stop() if app.cmd_echo([97,98,99,100]) == 0: sys.exit() #app.cmd_reset() store_param = False if store_param: app.cmd_set_param(1, PARAM_SAMPLERATE, 10) app.cmd_set_param(1, PARAM_RANGE, 1) app.cmd_set_param(1, PARAM_FILTER, 2) app.cmd_store(1) sys.exit() for s in range(1): sr=app.cmd_get_param(s+1,PARAM_SAMPLERATE) r=app.cmd_get_param(s+1,PARAM_RANGE) lpf=app.cmd_get_param(s+1,PARAM_FILTER) ee=app.cmd_get_param(s+1,PARAM_GAIN) print(sr,r,lpf,ee) # sys.exit() #time.sleep(0.5) #app.cmd_module_stop() app.cmd_module_start() time.sleep(0.5) #app.cmd_module_stop() #sys.exit() pocet = app.cmd_sensor_cnt() #sys.exit() print("pocet senzorov=", pocet) #app.cmd_sensor_get_data_FSR(pocet) for i in range(50): acc = app.cmd_sensor_get_data_IMU(1) gyr = app.cmd_sensor_get_data_IMU(2) print(acc) print(gyr) app.cmd_module_stop() sys.exit() for i in range(pocet): d=app.cmd_sensor_get_data(i+1) print("Data", i+1, ":\t", d) time.sleep(1) app.cmd_module_stop() print("Sensor type") for i in range(pocet): print(app.cmd_sensor_type(i+1)) print("Sensors type") print(app.cmd_sensors_type(pocet)) par = app.cmd_sensor_get_params(1) print(par) r=app.cmd_module_info(0xE1) print('NAME:' , r) r=app.cmd_module_info(0xE2) print('TYPE', r) r=app.cmd_module_info(0xE4) print('FW', r) r=app.cmd_module_info(0xE5) print('HW', r) r=app.cmd_module_info(0xE3) print("UUID", r) r=app.cmd_module_info(0xE6) print("MEM ID", r) app.finish()