From d2aaa7755105652459e8580345203a57df8bca8b Mon Sep 17 00:00:00 2001 From: wavicles Date: Sun, 10 Jul 2016 23:58:58 +0530 Subject: [PATCH 1/3] add sensor support MF522 --- PSL/SENSORS/MF522.py | 513 ++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 512 insertions(+), 1 deletion(-) diff --git a/PSL/SENSORS/MF522.py b/PSL/SENSORS/MF522.py index f9bf1abc..f200c312 100644 --- a/PSL/SENSORS/MF522.py +++ b/PSL/SENSORS/MF522.py @@ -1 +1,512 @@ -#TODO +# -*- coding: utf-8 -*- +# MF522 - Software stack to access the MF522 RFID reader via FOSSASIA PSLab +# + + + +from __future__ import print_function +from PSL import sciencelab +import time +def connect(I,cs): + return MF522(I,cs) + +class MF522: + #Constants from https://github.com/miguelbalboa/rfid ( Open source License : UNLICENSE) + CommandReg = 0x01 << 1 # starts and stops command execution + ComIEnReg = 0x02 << 1 # enable and disable interrupt request control bits + DivIEnReg = 0x03 << 1 # enable and disable interrupt request control bits + ComIrqReg = 0x04 << 1 # interrupt request bits + DivIrqReg = 0x05 << 1 # interrupt request bits + ErrorReg = 0x06 << 1 # error bits showing the error status of the last command executed + Status1Reg = 0x07 << 1 # communication status bits + Status2Reg = 0x08 << 1 # receiver and transmitter status bits + FIFODataReg = 0x09 << 1 # input and output of 64 byte FIFO buffer + FIFOLevelReg = 0x0A << 1 # number of bytes stored in the FIFO buffer + WaterLevelReg = 0x0B << 1 # level for FIFO underflow and overflow warning + ControlReg = 0x0C << 1 # miscellaneous control registers + BitFramingReg = 0x0D << 1 # adjustments for bit-oriented frames + CollReg = 0x0E << 1 # bit position of the first bit-collision detected on the RF sciencelab + + ModeReg = 0x11 << 1 # defines general modes for transmitting and receiving + TxModeReg = 0x12 << 1 # defines transmission data rate and framing + RxModeReg = 0x13 << 1 # defines reception data rate and framing + TxControlReg = 0x14 << 1 # controls the logical behavior of the antenna driver pins TX1 and TX2 + TxASKReg = 0x15 << 1 # controls the setting of the transmission modulation + TxSelReg = 0x16 << 1 # selects the internal sources for the antenna driver + RxSelReg = 0x17 << 1 # selects internal receiver settings + RxThresholdReg = 0x18 << 1 # selects thresholds for the bit decoder + DemodReg = 0x19 << 1 # defines demodulator settings + MfTxReg = 0x1C << 1 # controls some MIFARE communication transmit parameters + MfRxReg = 0x1D << 1 # controls some MIFARE communication receive parameters + SerialSpeedReg = 0x1F << 1 # selects the speed of the serial UART sciencelab + + CRCResultRegH = 0x21 << 1 # shows the MSB and LSB values of the CRC calculation + CRCResultRegL = 0x22 << 1 + ModWidthReg = 0x24 << 1 # controls the ModWidth setting? + RFCfgReg = 0x26 << 1 # configures the receiver gain + GsNReg = 0x27 << 1 # selects the conductance of the antenna driver pins TX1 and TX2 for modulation + CWGsPReg = 0x28 << 1 # defines the conductance of the p-driver output during periods of no modulation + ModGsPReg = 0x29 << 1 # defines the conductance of the p-driver output during periods of modulation + TModeReg = 0x2A << 1 # defines settings for the internal timer + TPrescalerReg = 0x2B << 1 # the lower 8 bits of the TPrescaler value. The 4 high bits are in TModeReg. + TReloadRegH = 0x2C << 1 # defines the 16-bit timer reload value + TReloadRegL = 0x2D << 1 + TCounterValueRegH = 0x2E << 1 # shows the 16-bit timer value + TCounterValueRegL = 0x2F << 1 + + TestSel1Reg = 0x31 << 1 # general test signal configuration + TestSel2Reg = 0x32 << 1 # general test signal configuration + TestPinEnReg = 0x33 << 1 # enables pin output driver on pins D1 to D7 + TestPinValueReg = 0x34 << 1 # defines the values for D1 to D7 when it is used as an I/O bus + TestBusReg = 0x35 << 1 # shows the status of the internal test bus + AutoTestReg = 0x36 << 1 # controls the digital self test + VersionReg = 0x37 << 1 # shows the software version + AnalogTestReg = 0x38 << 1 # controls the pins AUX1 and AUX2 + TestDAC1Reg = 0x39 << 1 # defines the test value for TestDAC1 + TestDAC2Reg = 0x3A << 1 # defines the test value for TestDAC2 + TestADCReg = 0x3B << 1 # shows the value of ADC I and Q channels + + # MFRC522 commands. Described in chapter 10 of the datasheet. + PCD_Idle = 0x00 #no action, cancels current command execution + PCD_Mem = 0x01 #stores 25 bytes into the internal buffer + PCD_GenerateRandomID = 0x02 #generates a 10-byte random ID number + PCD_CalcCRC = 0x03 #activates the CRC coprocessor or performs a self test + PCD_Transmit = 0x04 # transmits data from the FIFO buffer + PCD_NoCmdChange = 0x07 + PCD_Receive = 0x08 #activates the receiver circuits + PCD_Transceive = 0x0C #transmits data from FIFO buffer to antenna and automatically activates the receiver after transmission + PCD_MFAuthent = 0x0E #performs the MIFARE standard authentication as a reader + PCD_SoftReset = 0x0F #resets the MFRC522 + + RxGain_18dB = 0x00 << 4 # 000b - 18 dB, minimum + RxGain_23dB = 0x01 << 4 # 001b - 23 dB + RxGain_18dB_2 = 0x02 << 4 # 010b - 18 dB, it seems 010b is a duplicate for 000b + RxGain_23dB_2 = 0x03 << 4 # 011b - 23 dB, it seems 011b is a duplicate for 001b + RxGain_33dB = 0x04 << 4 # 100b - 33 dB, average, and typical default + RxGain_38dB = 0x05 << 4 # 101b - 38 dB + RxGain_43dB = 0x06 << 4 # 110b - 43 dB + RxGain_48dB = 0x07 << 4 # 111b - 48 dB, maximum + RxGain_min = 0x00 << 4 # 000b - 18 dB, minimum, convenience for RxGain_18dB + RxGain_avg = 0x04 << 4 # 100b - 33 dB, average, convenience for RxGain_33dB + RxGain_max = 0x07 << 4 # 111b - 48 dB, maximum, convenience for RxGain_48dB + + # The commands used by the PCD to manage communication with several PICCs (ISO 14443-3, Type A, section 6.4) + PICC_CMD_REQA = 0x26 # REQuest command, Type A. Invites PICCs in state IDLE to go to READY and prepare for anticollision or selection + PICC_CMD_WUPA = 0x52 # Wake-UP command, prepare for anticollision or selection. 7 bit frame. + PICC_CMD_CT = 0x88 # Cascade Tag. Not really a command, but used during anti collision. + PICC_CMD_SEL_CL1 = 0x93 # Anti collision/Select, Cascade Level 1 + PICC_CMD_SEL_CL2 = 0x95 # Anti collision/Select, Cascade Level 2 + PICC_CMD_SEL_CL3 = 0x97 # Anti collision/Select, Cascade Level 3 + PICC_CMD_HLTA = 0x50 # HaLT command, Type A. Instructs an ACTIVE PICC to go to state HALT. + # The commands used for MIFARE Classic (from http://www.mouser.com/ds/2/302/MF1S503x-89574.pdf, Section 9) + # Use PCD_MFAuthent to authenticate access to a sector, then use these commands to read/write/modify the blocks on the sector. + # The read/write commands can also be used for MIFARE Ultralight. + PICC_CMD_MF_AUTH_KEY_A = 0x60 # Perform authentication with Key A + PICC_CMD_MF_AUTH_KEY_B = 0x61 # Perform authentication with Key B + PICC_CMD_MF_READ = 0x30 # Reads one 16 byte block from the authenticated sector of the PICC. Also used for MIFARE Ultralight. + PICC_CMD_MF_WRITE = 0xA0 # Writes one 16 byte block to the authenticated sector of the PICC. Called "COMPATIBILITY WRITE" for MIFARE Ultralight. + PICC_CMD_MF_DECREMENT = 0xC0 # Decrements the contents of a block and stores the result in the internal data register. + PICC_CMD_MF_INCREMENT = 0xC1 # Increments the contents of a block and stores the result in the internal data register. + PICC_CMD_MF_RESTORE = 0xC2 # Reads the contents of a block into the internal data register. + PICC_CMD_MF_TRANSFER = 0xB0 # Writes the contents of the internal data register to a block. + + + + NRSTPD = 22 + MAX_LEN = 16 + MI_OK = 0 + MI_NOTAGERR = 1 + MI_ERR = 2 + + PCD_CALCCRC = 0x03 + + PICC_REQIDL = 0x26 + PICC_REQALL = 0x52 + PICC_ANTICOLL = 0x93 + PICC_SElECTTAG = 0x93 + PICC_AUTHENT1A = 0x60 + PICC_AUTHENT1B = 0x61 + PICC_READ = 0x30 + PICC_WRITE = 0xA0 + PICC_DECREMENT = 0xC0 + PICC_INCREMENT = 0xC1 + PICC_RESTORE = 0xC2 + PICC_TRANSFER = 0xB0 + PICC_HALT = 0x50 + + + + # The commands used for MIFARE Ultralight (from http://www.nxp.com/documents/data_sheet/MF0ICU1.pdf, Section 8.6) + # The PICC_CMD_MF_READ and PICC_CMD_MF_WRITE can also be used for MIFARE Ultralight. + PICC_CMD_UL_WRITE = 0xA2 #Writes one 4 byte page to the PICC. + + MF_ACK = 0xA # The MIFARE Classic uses a 4 bit ACK/NAK. Any other value than 0xA is NAK. + MF_KEY_SIZE = 6 # A Mifare Crypto1 key is 6 bytes. + + + def __init__(self,I,cs='CS1'): + self.cs=cs + self.I = I + self.I.SPI.set_parameters(2,1,1,0) + if not self.reset(): + self.connected=False + return None + self.write(self.TModeReg, 0x80) + self.write(self.TPrescalerReg, 0xA9) + self.write(self.TReloadRegH, 0x03) + self.write(self.TReloadRegL, 0xE8) + + self.write(self.TxASKReg, 0x40) + self.write(self.ModeReg, 0x3D) + + #Enable the antenna + self.enableAntenna() + self.connected = True + + def MFRC522_Init(self): + GPIO.output(self.NRSTPD, 1) + self.MFRC522_Reset(); + self.write(self.TModeReg, 0x8D) + self.write(self.TPrescalerReg, 0x3E) + self.write(self.TReloadRegL, 30) + self.write(self.TReloadRegH, 0) + + self.write(self.TxAutoReg, 0x40) + self.write(self.ModeReg, 0x3D) + self.AntennaOn() + + def enableAntenna(self): + val = self.read(self.TxControlReg); + if ((val & 0x03) != 0x03): + self.write(self.TxControlReg, val | 0x03); + + def reset(self): + self.write(self.CommandReg,self.PCD_SoftReset) + s=time.time() + while (self.read(self.CommandReg) & (1<<4)): + print ('wait') + time.sleep(0.1) + if time.time() - s > .5: return False + return True + + def write(self,register,val): + self.I.SPI.set_cs(self.cs,0) + ret = self.I.SPI.send16(((register&0x7F)<<8)|val) + self.I.SPI.set_cs(self.cs,1) + return ret&0xFF + + def read(self,register): + self.I.SPI.set_cs(self.cs,0) + ret = self.I.SPI.send16((register|0x80)<<8) + self.I.SPI.set_cs(self.cs,1) + return ret&0xFF + + def readMany(self,register,total): + self.I.SPI.set_cs(self.cs,0) + self.I.SPI.send8(register) + vals = [] + for a in range(total-1): + vals.append( I.SPI.send8(register) ) + vals.append( I.SPI.send8(0) ) + self.I.SPI.set_cs(self.cs,1) + return vals + + + def getStatus(self): + return self.read(self.Status1Reg) + + def getVersion(self): + ver = self.read(self.VersionReg) + if ver==0x88: print ('Cloned version: Fudan Semiconductors') + elif ver==0x90: print ('version 1.0') + elif ver==0x91: print ('version 1.0') + elif ver==0x92: print ('version 2.0') + else: print ('Unknown version ',ver) + return ver + + + def SetBitMask(self, reg, mask): + tmp = self.read(reg) + self.write(reg, tmp | mask) + + def ClearBitMask(self, reg, mask): + tmp = self.read(reg); + self.write(reg, tmp & (~mask)) + + def MFRC522_ToCard(self,command,sendData): + returnedData = [] + backLen = 0 + status = self.MI_ERR + irqEn = 0x00 + waitIRq = 0x00 + lastBits = None + n = 0 + i = 0 + + if command == self.PCD_MFAuthent: + irqEn = 0x12 + waitIRq = 0x10 + if command == self.PCD_Transceive: + irqEn = 0x77 + waitIRq = 0x30 + + self.write(self.ComIEnReg, irqEn|0x80) + self.ClearBitMask(self.ComIrqReg, 0x80) + self.SetBitMask(self.FIFOLevelReg, 0x80) + + self.write(self.CommandReg, self.PCD_Idle); + + for a in sendData: + self.write(self.FIFODataReg, a) + self.write(self.CommandReg, command) + + if command == self.PCD_Transceive: + self.SetBitMask(self.BitFramingReg, 0x80) + + i = 2000 + while True: + n = self.read(self.ComIrqReg) + i = i - 1 + if ~((i!=0) and ~(n&0x01) and ~(n&waitIRq)): + break + + self.ClearBitMask(self.BitFramingReg, 0x80) + + if i != 0: + if (self.read(self.ErrorReg) & 0x1B)==0x00: + status = self.MI_OK + + if n & irqEn & 0x01: + status = self.MI_NOTAGERR + + if command == self.PCD_Transceive: + n = self.read(self.FIFOLevelReg) + lastBits = self.read(self.ControlReg) & 0x07 + if lastBits != 0: + backLen = (n-1)*8 + lastBits + else: + backLen = n*8 + + if n == 0: + n = 1 + if n > self.MAX_LEN: + n = self.MAX_LEN + + i = 0 + while i Date: Sun, 10 Jul 2016 23:59:48 +0530 Subject: [PATCH 2/3] add sensor support --- PSL/SENSORS/MLX90614.py | 71 ++++++++++++++++++++++++++++++++++++++++- 1 file changed, 70 insertions(+), 1 deletion(-) diff --git a/PSL/SENSORS/MLX90614.py b/PSL/SENSORS/MLX90614.py index f9bf1abc..1d9bfa57 100644 --- a/PSL/SENSORS/MLX90614.py +++ b/PSL/SENSORS/MLX90614.py @@ -1 +1,70 @@ -#TODO +from __future__ import print_function +from numpy import int16 +def connect(route,**args): + return MLX90614(route,**args) + +class MLX90614(): + NUMPLOTS=1 + PLOTNAMES=['Temp'] + ADDRESS = 0x5A + name = 'PIR temperature' + def __init__(self,I2C,**args): + self.I2C=I2C + self.ADDRESS = args.get('address',self.ADDRESS) + self.OBJADDR=0x07 + self.AMBADDR=0x06 + + self.source=self.OBJADDR + + self.name = 'Passive IR temperature sensor' + self.params={'readReg':range(0x20), + 'select_source':['object temperature','ambient temperature']} + + try: + print ('switching baud to 100k') + self.I2C.configI2C(100e3) + except: + print ('FAILED TO CHANGE BAUD RATE') + + + def select_source(self,source): + if source=='object temperature': + self.source = self.OBJADDR + elif source=='ambient temperature': + self.source = self.AMBADDR + + + def readReg(self,addr): + x= self.getVals(addr,2) + print (hex(addr),hex(x[0]|(x[1]<<8))) + + + def getVals(self,addr,bytes): + vals = self.I2C.readBulk(self.ADDRESS,addr,bytes) + return vals + + def getRaw(self): + vals=self.getVals(self.source,3) + if vals: + if len(vals)==3: + return [((((vals[1]&0x007f)<<8)+vals[0])*0.02)-0.01 - 273.15] + else: + return False + else: + return False + + def getObjectTemperature(self): + self.source = self.OBJADDR + val=self.getRaw() + if val: + return val[0] + else: + return False + + def getAmbientTemperature(self): + self.source = self.AMBADDR + val=self.getRaw() + if val: + return val[0] + else: + return False From d6b2a1bb645921089727cc1ba75c3b7c1701882a Mon Sep 17 00:00:00 2001 From: wavicles Date: Mon, 11 Jul 2016 00:07:14 +0530 Subject: [PATCH 3/3] add sensor support --- PSL/SENSORS/MPU6050.py | 135 +++++++++++- PSL/SENSORS/SHT21.py | 91 ++++++++- PSL/SENSORS/SSD1306.py | 454 ++++++++++++++++++++++++++++++++++++++++- 3 files changed, 677 insertions(+), 3 deletions(-) diff --git a/PSL/SENSORS/MPU6050.py b/PSL/SENSORS/MPU6050.py index f9bf1abc..e9313173 100644 --- a/PSL/SENSORS/MPU6050.py +++ b/PSL/SENSORS/MPU6050.py @@ -1 +1,134 @@ -#TODO +from numpy import int16,std +from PSL.SENSORS.Kalman import KalmanFilter + +def connect(route,**args): + return MPU6050(route,**args) + +class MPU6050(): + ''' + Mandatory members: + GetRaw : Function called by Graphical apps. Must return values stored in a list + NUMPLOTS : length of list returned by GetRaw. Even single datapoints need to be stored in a list before returning + PLOTNAMES : a list of strings describing each element in the list returned by GetRaw. len(PLOTNAMES) = NUMPLOTS + name : the name of the sensor shown to the user + params: + A dictionary of function calls(single arguments only) paired with list of valid argument values. (Primitive. I know.) + These calls can be used for one time configuration settings + + ''' + GYRO_CONFIG = 0x1B + ACCEL_CONFIG = 0x1C + GYRO_SCALING= [131,65.5,32.8,16.4] + ACCEL_SCALING=[16384,8192,4096,2048] + AR=3 + GR=3 + NUMPLOTS=7 + PLOTNAMES = ['Ax','Ay','Az','Temp','Gx','Gy','Gz'] + ADDRESS = 0x68 + name = 'Accel/gyro' + def __init__(self,I2C,**args): + self.I2C=I2C + self.ADDRESS = args.get('address',self.ADDRESS) + self.name = 'Accel/gyro' + self.params={'powerUp':['Go'],'setGyroRange':[250,500,1000,2000],'setAccelRange':[2,4,8,16],'KalmanFilter':[.01,.1,1,10,100,1000,10000,'OFF']} + self.setGyroRange(2000) + self.setAccelRange(16) + ''' + try: + self.I2C.configI2C(400e3) + except: + pass + ''' + self.powerUp(True) + self.K=None + + + + def KalmanFilter(self,opt): + if opt=='OFF': + self.K=None + return + noise=[[]]*self.NUMPLOTS + for a in range(500): + vals=self.getRaw() + for b in range(self.NUMPLOTS):noise[b].append(vals[b]) + + self.K=[None]*7 + for a in range(self.NUMPLOTS): + sd = std(noise[a]) + self.K[a] = KalmanFilter(1./opt, sd**2) + + def getVals(self,addr,bytes): + vals = self.I2C.readBulk(self.ADDRESS,addr,bytes) + return vals + + def powerUp(self,x): + self.I2C.writeBulk(self.ADDRESS,[0x6B,0]) + + def setGyroRange(self,rs): + self.GR=self.params['setGyroRange'].index(rs) + self.I2C.writeBulk(self.ADDRESS,[self.GYRO_CONFIG,self.GR<<3]) + + def setAccelRange(self,rs): + self.AR=self.params['setAccelRange'].index(rs) + self.I2C.writeBulk(self.ADDRESS,[self.ACCEL_CONFIG,self.AR<<3]) + + def getRaw(self): + ''' + This method must be defined if you want GUIs to use this class to generate + plots on the fly. + It must return a set of different values read from the sensor. such as X,Y,Z acceleration. + The length of this list must not change, and must be defined in the variable NUMPLOTS. + + GUIs will generate as many plots, and the data returned from this method will be appended appropriately + ''' + vals=self.getVals(0x3B,14) + if vals: + if len(vals)==14: + raw=[0]*7 + for a in range(3):raw[a] = 1.*int16(vals[a*2]<<8|vals[a*2+1])/self.ACCEL_SCALING[self.AR] + for a in range(4,7):raw[a] = 1.*int16(vals[a*2]<<8|vals[a*2+1])/self.GYRO_SCALING[self.GR] + raw[3] = int16(vals[6]<<8|vals[7])/340. + 36.53 + if not self.K: + return raw + else: + for b in range(self.NUMPLOTS): + self.K[b].input_latest_noisy_measurement(raw[b]) + raw[b]=self.K[b].get_latest_estimated_measurement() + return raw + + else: + return False + else: + return False + + def getAccel(self): + vals=self.getVals(0x3B,6) + ax=int16(vals[0]<<8|vals[1]) + ay=int16(vals[2]<<8|vals[3]) + az=int16(vals[4]<<8|vals[5]) + return [ax/65535.,ay/65535.,az/65535.] + + def getTemp(self): + vals=self.getVals(0x41,6) + t=int16(vals[0]<<8|vals[1]) + return t/65535. + + def getGyro(self): + vals=self.getVals(0x43,6) + ax=int16(vals[0]<<8|vals[1]) + ay=int16(vals[2]<<8|vals[3]) + az=int16(vals[4]<<8|vals[5]) + return [ax/65535.,ay/65535.,az/65535.] + +if __name__ == "__main__": + from PSL import sciencelab + I= sciencelab.connect() + A = connect(I.I2C) + t,x,y,z = I.I2C.capture(A.ADDRESS,0x43,6,5000,1000,'int') + #print (t,x,y,z) + from pylab import * + plot(t,x) + plot(t,y) + plot(t,z) + show() diff --git a/PSL/SENSORS/SHT21.py b/PSL/SENSORS/SHT21.py index f9bf1abc..1d1c4974 100644 --- a/PSL/SENSORS/SHT21.py +++ b/PSL/SENSORS/SHT21.py @@ -1 +1,90 @@ -#TODO +from __future__ import print_function +from numpy import int16 +import time + +def connect(route,**args): + ''' + route can either be I.I2C , or a radioLink instance + ''' + return SHT21(route,**args) + +class SHT21(): + RESET = 0xFE + TEMP_ADDRESS = 0xF3 + HUMIDITY_ADDRESS = 0xF5 + selected=0xF3 + NUMPLOTS=1 + PLOTNAMES = ['Data'] + ADDRESS = 0x40 + name = 'Humidity/Temperature' + def __init__(self,I2C,**args): + self.I2C=I2C + self.ADDRESS = args.get('address',self.ADDRESS) + self.name = 'Humidity/Temperature' + ''' + try: + print ('switching baud to 400k') + self.I2C.configI2C(400e3) + except: + print ('FAILED TO CHANGE BAUD RATE') + ''' + self.params={'selectParameter':['temperature','humidity']} + self.init('') + + + def init(self,x): + self.I2C.writeBulk(self.ADDRESS,[self.RESET]) #soft reset + time.sleep(0.1) + + def rawToTemp(self,vals): + if vals: + if len(vals): + v = (vals[0]<<8)|(vals[1]&0xFC) #make integer & remove status bits + v*=175.72; v/= (1<<16); v-=46.85 + return [v] + return False + + def rawToRH(self,vals): + if vals: + if len(vals): + v = (vals[0]<<8)|(vals[1]&0xFC) #make integer & remove status bits + v*=125.; v/= (1<<16); v-=6 + return [v] + return False + + @staticmethod + def _calculate_checksum(data, number_of_bytes): + """5.7 CRC Checksum using the polynomial given in the datasheet + Credits: https://github.com/jaques/sht21_python/blob/master/sht21.py + """ + # CRC + POLYNOMIAL = 0x131 # //P(x)=x^8+x^5+x^4+1 = 100110001 + crc = 0 + # calculates 8-Bit checksum with given polynomial + for byteCtr in range(number_of_bytes): + crc ^= (data[byteCtr]) + for bit in range(8, 0, -1): + if crc & 0x80: + crc = (crc << 1) ^ POLYNOMIAL + else: + crc = (crc << 1) + return crc + + + def selectParameter(self,param): + if param=='temperature':self.selected=self.TEMP_ADDRESS + elif param=='humidity':self.selected=self.HUMIDITY_ADDRESS + + def getRaw(self): + self.I2C.writeBulk(self.ADDRESS,[self.selected]) + if self.selected==self.TEMP_ADDRESS:time.sleep(0.1) + elif self.selected==self.HUMIDITY_ADDRESS:time.sleep(0.05) + + vals = self.I2C.simpleRead(self.ADDRESS,3) + if vals: + if self._calculate_checksum(vals,2)!=vals[2]: + return False + print (vals) + if self.selected==self.TEMP_ADDRESS:return self.rawToTemp(vals) + elif self.selected==self.HUMIDITY_ADDRESS:return self.rawToRH(vals) + diff --git a/PSL/SENSORS/SSD1306.py b/PSL/SENSORS/SSD1306.py index f9bf1abc..f85c3d9f 100644 --- a/PSL/SENSORS/SSD1306.py +++ b/PSL/SENSORS/SSD1306.py @@ -1 +1,453 @@ -#TODO +''' +Adapted into Python from Adafruit's oled.cpp +Original license text: + +Software License Agreement (BSD License) + +Copyright (c) 2012, Adafruit Industries +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright +notice, this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright +notice, this list of conditions and the following disclaimer in the +documentation and/or other materials provided with the distribution. +3. Neither the name of the copyright holders nor the +names of its contributors may be used to endorse or promote products +derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +''' +from __future__ import print_function +from numpy import int16 +import time + +def connect(route,**args): + return SSD1306(route,**args) + +class SSD1306(): + ADDRESS = 0x3C + + #--------------Parameters-------------------- + #This must be defined in order to let GUIs automatically create menus + #for changing various options of this sensor + #It's a dictionary of the string representations of functions matched with an array + #of options that each one can accept + params={'load':['logo'], + 'scroll':['left','right','topright','topleft','bottomleft','bottomright','stop'] + } + + NUMPLOTS=0 + PLOTNAMES = [''] + name = 'OLED Display' + _width=128;WIDTH = 128; + _height=64;HEIGHT = 64; + + rotation = 0 + cursor_y = 0 + cursor_x = 0 + textsize = 1 + textcolor =1 + textbgcolor = 0 + wrap = True + + + + SSD1306_128_64 =1 + SSD1306_128_32 =2 + SSD1306_96_16 =3 + + # -----------------------------------------------------------------------*/ + DISPLAY_TYPE = SSD1306_96_16 + ## self.SSD1306_128_32 + #/*=========================================================================*/ + + SSD1306_LCDWIDTH = 128 + SSD1306_LCDHEIGHT = 64 + + + SSD1306_SETCONTRAST =0x81 + SSD1306_DISPLAYALLON_RESUME =0xA4 + SSD1306_DISPLAYALLON =0xA5 + SSD1306_NORMALDISPLAY =0xA6 + SSD1306_INVERTDISPLAY =0xA7 + SSD1306_DISPLAYOFF= 0xAE + SSD1306_DISPLAYON= 0xAF + + SSD1306_SETDISPLAYOFFSET= 0xD3 + SSD1306_SETCOMPINS= 0xDA + + SSD1306_SETVCOMDETECT= 0xDB + + SSD1306_SETDISPLAYCLOCKDIV =0xD5 + SSD1306_SETPRECHARGE= 0xD9 + + SSD1306_SETMULTIPLEX= 0xA8 + + SSD1306_SETLOWCOLUMN =0x00 + SSD1306_SETHIGHCOLUMN =0x10 + + SSD1306_SETSTARTLINE= 0x40 + + SSD1306_MEMORYMODE= 0x20 + + SSD1306_COMSCANINC =0xC0 + SSD1306_COMSCANDEC= 0xC8 + + SSD1306_SEGREMAP =0xA0 + + SSD1306_CHARGEPUMP= 0x8D + + SSD1306_EXTERNALVCC= 0x1 + SSD1306_SWITCHCAPVCC =0x2 + + + logobuff = [255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 127, 63, 63, 159, 159, 223, 223, 207, 207, 207, 239, 239, 47, 47, 39, 39, 7, 7, 67, 67, 83, 131, 135, 7, 7, 15, 15, 31, 191, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 63, 31, 15, 199, 99, 17, 25, 12, 4, 2, 3, 7, 63, 255, 255, 255, 255, 255, 255, 255, 255, 254, 252, 240, 224, 224, 224, 192, 192, 128, 128, 128, 128, 129, 128, 0, 0, 0, 0, 0, 3, 3, 7, 31, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 127, 15, 3, 192, 120, 134, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 254, 254, 254, 252, 252, 249, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 143, 0, 0, 124, 199, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 128, 240, 252, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 240, 128, 0, 7, 56, 96, 128, 0, 0, 0, 0, 0, 0, 0, 12, 63, 255, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 31, 7, 227, 243, 249, 249, 249, 249, 249, 249, 243, 255, 255, 199, 131, 49, 57, 57, 57, 121, 115, 255, 255, 255, 255, 15, 15, 159, 207, 207, 207, 143, 31, 63, 255, 255, 159, 207, 207, 207, 143, 31, 63, 255, 255, 255, 15, 15, 159, 207, 207, 207, 255, 255, 0, 0, 255, 127, 63, 159, 207, 239, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 254, 248, 240, 224, 129, 2, 4, 8, 16, 32, 96, 64, 128, 128, 135, 30, 115, 207, 159, 255, 255, 255, 255, 127, 63, 31, 31, 31, 31, 31, 31, 31, 7, 7, 7, 127, 127, 127, 127, 127, 127, 255, 255, 255, 255, 252, 240, 227, 231, 207, 207, 207, 207, 207, 207, 231, 255, 255, 231, 207, 207, 207, 207, 207, 198, 224, 240, 255, 255, 255, 0, 0, 231, 207, 207, 207, 199, 224, 240, 255, 225, 193, 204, 204, 204, 228, 192, 192, 255, 255, 255, 192, 192, 255, 255, 255, 255, 255, 255, 192, 192, 252, 248, 243, 231, 207, 223, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 254, 252, 248, 248, 240, 240, 224, 225, 225, 193, 193, 195, 195, 195, 195, 195, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 62, 62, 62, 62, 62, 62, 255, 243, 3, 3, 51, 51, 51, 19, 135, 239, 255, 255, 63, 63, 159, 159, 159, 159, 63, 127, 255, 255, 255, 63, 31, 159, 159, 159, 31, 252, 252, 255, 63, 63, 159, 159, 159, 159, 63, 127, 255, 255, 255, 223, 159, 159, 159, 31, 127, 255, 255, 255, 255, 223, 31, 31, 191, 159, 159, 159, 255, 255, 127, 63, 159, 159, 159, 159, 31, 31, 255, 255, 247, 3, 7, 159, 159, 159, 31, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 254, 252, 252, 252, 252, 252, 252, 252, 252, 224, 224, 224, 255, 255, 255, 255, 255, 255, 255, 243, 240, 240, 247, 255, 254, 252, 248, 243, 255, 255, 248, 248, 242, 242, 242, 242, 242, 250, 255, 255, 255, 241, 242, 242, 242, 242, 248, 253, 255, 255, 248, 248, 242, 242, 242, 242, 242, 250, 255, 255, 249, 240, 242, 242, 242, 240, 240, 255, 255, 255, 255, 243, 240, 240, 243, 243, 255, 255, 255, 255, 252, 248, 243, 243, 243, 243, 243, 255, 255, 255, 247, 240, 240, 247, 255, 247, 240, 240, 247, 255] + font = [0x00, 0x00, 0x00, 0x00, 0x00, 0x3E, 0x5B, 0x4F, 0x5B, 0x3E, 0x3E, 0x6B, 0x4F, 0x6B, 0x3E, + 0x1C, 0x3E, 0x7C, 0x3E, 0x1C, 0x18, 0x3C, 0x7E, 0x3C, 0x18, 0x1C, 0x57, 0x7D, 0x57, 0x1C, + 0x1C, 0x5E, 0x7F, 0x5E, 0x1C, 0x00, 0x18, 0x3C, 0x18, 0x00, 0xFF, 0xE7, 0xC3, 0xE7, 0xFF, + 0x00, 0x18, 0x24, 0x18, 0x00, 0xFF, 0xE7, 0xDB, 0xE7, 0xFF, 0x30, 0x48, 0x3A, 0x06, 0x0E, + 0x26, 0x29, 0x79, 0x29, 0x26, 0x40, 0x7F, 0x05, 0x05, 0x07, 0x40, 0x7F, 0x05, 0x25, 0x3F, + 0x5A, 0x3C, 0xE7, 0x3C, 0x5A, 0x7F, 0x3E, 0x1C, 0x1C, 0x08, 0x08, 0x1C, 0x1C, 0x3E, 0x7F, + 0x14, 0x22, 0x7F, 0x22, 0x14, 0x5F, 0x5F, 0x00, 0x5F, 0x5F, 0x06, 0x09, 0x7F, 0x01, 0x7F, + 0x00, 0x66, 0x89, 0x95, 0x6A, 0x60, 0x60, 0x60, 0x60, 0x60, 0x94, 0xA2, 0xFF, 0xA2, 0x94, + 0x08, 0x04, 0x7E, 0x04, 0x08, 0x10, 0x20, 0x7E, 0x20, 0x10, 0x08, 0x08, 0x2A, 0x1C, 0x08, + 0x08, 0x1C, 0x2A, 0x08, 0x08, 0x1E, 0x10, 0x10, 0x10, 0x10, 0x0C, 0x1E, 0x0C, 0x1E, 0x0C, + 0x30, 0x38, 0x3E, 0x38, 0x30, 0x06, 0x0E, 0x3E, 0x0E, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x5F, 0x00, 0x00, 0x00, 0x07, 0x00, 0x07, 0x00, 0x14, 0x7F, 0x14, 0x7F, 0x14, + 0x24, 0x2A, 0x7F, 0x2A, 0x12, 0x23, 0x13, 0x08, 0x64, 0x62, 0x36, 0x49, 0x56, 0x20, 0x50, + 0x00, 0x08, 0x07, 0x03, 0x00, 0x00, 0x1C, 0x22, 0x41, 0x00, 0x00, 0x41, 0x22, 0x1C, 0x00, + 0x2A, 0x1C, 0x7F, 0x1C, 0x2A, 0x08, 0x08, 0x3E, 0x08, 0x08, 0x00, 0x80, 0x70, 0x30, 0x00, + 0x08, 0x08, 0x08, 0x08, 0x08, 0x00, 0x00, 0x60, 0x60, 0x00, 0x20, 0x10, 0x08, 0x04, 0x02, + 0x3E, 0x51, 0x49, 0x45, 0x3E, 0x00, 0x42, 0x7F, 0x40, 0x00, 0x72, 0x49, 0x49, 0x49, 0x46, + 0x21, 0x41, 0x49, 0x4D, 0x33, 0x18, 0x14, 0x12, 0x7F, 0x10, 0x27, 0x45, 0x45, 0x45, 0x39, + 0x3C, 0x4A, 0x49, 0x49, 0x31, 0x41, 0x21, 0x11, 0x09, 0x07, 0x36, 0x49, 0x49, 0x49, 0x36, + 0x46, 0x49, 0x49, 0x29, 0x1E, 0x00, 0x00, 0x14, 0x00, 0x00, 0x00, 0x40, 0x34, 0x00, 0x00, + 0x00, 0x08, 0x14, 0x22, 0x41, 0x14, 0x14, 0x14, 0x14, 0x14, 0x00, 0x41, 0x22, 0x14, 0x08, + 0x02, 0x01, 0x59, 0x09, 0x06, 0x3E, 0x41, 0x5D, 0x59, 0x4E, 0x7C, 0x12, 0x11, 0x12, 0x7C, + 0x7F, 0x49, 0x49, 0x49, 0x36, 0x3E, 0x41, 0x41, 0x41, 0x22, 0x7F, 0x41, 0x41, 0x41, 0x3E, + 0x7F, 0x49, 0x49, 0x49, 0x41, 0x7F, 0x09, 0x09, 0x09, 0x01, 0x3E, 0x41, 0x41, 0x51, 0x73, + 0x7F, 0x08, 0x08, 0x08, 0x7F, 0x00, 0x41, 0x7F, 0x41, 0x00, 0x20, 0x40, 0x41, 0x3F, 0x01, + 0x7F, 0x08, 0x14, 0x22, 0x41, 0x7F, 0x40, 0x40, 0x40, 0x40, 0x7F, 0x02, 0x1C, 0x02, 0x7F, + 0x7F, 0x04, 0x08, 0x10, 0x7F, 0x3E, 0x41, 0x41, 0x41, 0x3E, 0x7F, 0x09, 0x09, 0x09, 0x06, + 0x3E, 0x41, 0x51, 0x21, 0x5E, 0x7F, 0x09, 0x19, 0x29, 0x46, 0x26, 0x49, 0x49, 0x49, 0x32, + 0x03, 0x01, 0x7F, 0x01, 0x03, 0x3F, 0x40, 0x40, 0x40, 0x3F, 0x1F, 0x20, 0x40, 0x20, 0x1F, + 0x3F, 0x40, 0x38, 0x40, 0x3F, 0x63, 0x14, 0x08, 0x14, 0x63, 0x03, 0x04, 0x78, 0x04, 0x03, + 0x61, 0x59, 0x49, 0x4D, 0x43, 0x00, 0x7F, 0x41, 0x41, 0x41, 0x02, 0x04, 0x08, 0x10, 0x20, + 0x00, 0x41, 0x41, 0x41, 0x7F, 0x04, 0x02, 0x01, 0x02, 0x04, 0x40, 0x40, 0x40, 0x40, 0x40, + 0x00, 0x03, 0x07, 0x08, 0x00, 0x20, 0x54, 0x54, 0x78, 0x40, 0x7F, 0x28, 0x44, 0x44, 0x38, + 0x38, 0x44, 0x44, 0x44, 0x28, 0x38, 0x44, 0x44, 0x28, 0x7F, 0x38, 0x54, 0x54, 0x54, 0x18, + 0x00, 0x08, 0x7E, 0x09, 0x02, 0x18, 0xA4, 0xA4, 0x9C, 0x78, 0x7F, 0x08, 0x04, 0x04, 0x78, + 0x00, 0x44, 0x7D, 0x40, 0x00, 0x20, 0x40, 0x40, 0x3D, 0x00, 0x7F, 0x10, 0x28, 0x44, 0x00, + 0x00, 0x41, 0x7F, 0x40, 0x00, 0x7C, 0x04, 0x78, 0x04, 0x78, 0x7C, 0x08, 0x04, 0x04, 0x78, + 0x38, 0x44, 0x44, 0x44, 0x38, 0xFC, 0x18, 0x24, 0x24, 0x18, 0x18, 0x24, 0x24, 0x18, 0xFC, + 0x7C, 0x08, 0x04, 0x04, 0x08, 0x48, 0x54, 0x54, 0x54, 0x24, 0x04, 0x04, 0x3F, 0x44, 0x24, + 0x3C, 0x40, 0x40, 0x20, 0x7C, 0x1C, 0x20, 0x40, 0x20, 0x1C, 0x3C, 0x40, 0x30, 0x40, 0x3C, + 0x44, 0x28, 0x10, 0x28, 0x44, 0x4C, 0x90, 0x90, 0x90, 0x7C, 0x44, 0x64, 0x54, 0x4C, 0x44, + 0x00, 0x08, 0x36, 0x41, 0x00, 0x00, 0x00, 0x77, 0x00, 0x00, 0x00, 0x41, 0x36, 0x08, 0x00, + 0x02, 0x01, 0x02, 0x04, 0x02, 0x3C, 0x26, 0x23, 0x26, 0x3C, 0x1E, 0xA1, 0xA1, 0x61, 0x12, + 0x3A, 0x40, 0x40, 0x20, 0x7A, 0x38, 0x54, 0x54, 0x55, 0x59, 0x21, 0x55, 0x55, 0x79, 0x41, + 0x21, 0x54, 0x54, 0x78, 0x41, 0x21, 0x55, 0x54, 0x78, 0x40, 0x20, 0x54, 0x55, 0x79, 0x40, + 0x0C, 0x1E, 0x52, 0x72, 0x12, 0x39, 0x55, 0x55, 0x55, 0x59, 0x39, 0x54, 0x54, 0x54, 0x59, + 0x39, 0x55, 0x54, 0x54, 0x58, 0x00, 0x00, 0x45, 0x7C, 0x41, 0x00, 0x02, 0x45, 0x7D, 0x42, + 0x00, 0x01, 0x45, 0x7C, 0x40, 0xF0, 0x29, 0x24, 0x29, 0xF0, 0xF0, 0x28, 0x25, 0x28, 0xF0, + 0x7C, 0x54, 0x55, 0x45, 0x00, 0x20, 0x54, 0x54, 0x7C, 0x54, 0x7C, 0x0A, 0x09, 0x7F, 0x49, + 0x32, 0x49, 0x49, 0x49, 0x32, 0x32, 0x48, 0x48, 0x48, 0x32, 0x32, 0x4A, 0x48, 0x48, 0x30, + 0x3A, 0x41, 0x41, 0x21, 0x7A, 0x3A, 0x42, 0x40, 0x20, 0x78, 0x00, 0x9D, 0xA0, 0xA0, 0x7D, + 0x39, 0x44, 0x44, 0x44, 0x39, 0x3D, 0x40, 0x40, 0x40, 0x3D, 0x3C, 0x24, 0xFF, 0x24, 0x24, + 0x48, 0x7E, 0x49, 0x43, 0x66, 0x2B, 0x2F, 0xFC, 0x2F, 0x2B, 0xFF, 0x09, 0x29, 0xF6, 0x20, + 0xC0, 0x88, 0x7E, 0x09, 0x03, 0x20, 0x54, 0x54, 0x79, 0x41, 0x00, 0x00, 0x44, 0x7D, 0x41, + 0x30, 0x48, 0x48, 0x4A, 0x32, 0x38, 0x40, 0x40, 0x22, 0x7A, 0x00, 0x7A, 0x0A, 0x0A, 0x72, + 0x7D, 0x0D, 0x19, 0x31, 0x7D, 0x26, 0x29, 0x29, 0x2F, 0x28, 0x26, 0x29, 0x29, 0x29, 0x26, + 0x30, 0x48, 0x4D, 0x40, 0x20, 0x38, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x38, + 0x2F, 0x10, 0xC8, 0xAC, 0xBA, 0x2F, 0x10, 0x28, 0x34, 0xFA, 0x00, 0x00, 0x7B, 0x00, 0x00, + 0x08, 0x14, 0x2A, 0x14, 0x22, 0x22, 0x14, 0x2A, 0x14, 0x08, 0xAA, 0x00, 0x55, 0x00, 0xAA, + 0xAA, 0x55, 0xAA, 0x55, 0xAA, 0x00, 0x00, 0x00, 0xFF, 0x00, 0x10, 0x10, 0x10, 0xFF, 0x00, + 0x14, 0x14, 0x14, 0xFF, 0x00, 0x10, 0x10, 0xFF, 0x00, 0xFF, 0x10, 0x10, 0xF0, 0x10, 0xF0, + 0x14, 0x14, 0x14, 0xFC, 0x00, 0x14, 0x14, 0xF7, 0x00, 0xFF, 0x00, 0x00, 0xFF, 0x00, 0xFF, + 0x14, 0x14, 0xF4, 0x04, 0xFC, 0x14, 0x14, 0x17, 0x10, 0x1F, 0x10, 0x10, 0x1F, 0x10, 0x1F, + 0x14, 0x14, 0x14, 0x1F, 0x00, 0x10, 0x10, 0x10, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x1F, 0x10, + 0x10, 0x10, 0x10, 0x1F, 0x10, 0x10, 0x10, 0x10, 0xF0, 0x10, 0x00, 0x00, 0x00, 0xFF, 0x10, + 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0xFF, 0x10, 0x00, 0x00, 0x00, 0xFF, 0x14, + 0x00, 0x00, 0xFF, 0x00, 0xFF, 0x00, 0x00, 0x1F, 0x10, 0x17, 0x00, 0x00, 0xFC, 0x04, 0xF4, + 0x14, 0x14, 0x17, 0x10, 0x17, 0x14, 0x14, 0xF4, 0x04, 0xF4, 0x00, 0x00, 0xFF, 0x00, 0xF7, + 0x14, 0x14, 0x14, 0x14, 0x14, 0x14, 0x14, 0xF7, 0x00, 0xF7, 0x14, 0x14, 0x14, 0x17, 0x14, + 0x10, 0x10, 0x1F, 0x10, 0x1F, 0x14, 0x14, 0x14, 0xF4, 0x14, 0x10, 0x10, 0xF0, 0x10, 0xF0, + 0x00, 0x00, 0x1F, 0x10, 0x1F, 0x00, 0x00, 0x00, 0x1F, 0x14, 0x00, 0x00, 0x00, 0xFC, 0x14, + 0x00, 0x00, 0xF0, 0x10, 0xF0, 0x10, 0x10, 0xFF, 0x10, 0xFF, 0x14, 0x14, 0x14, 0xFF, 0x14, + 0x10, 0x10, 0x10, 0x1F, 0x00, 0x00, 0x00, 0x00, 0xF0, 0x10, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, + 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x38, 0x44, 0x44, 0x38, 0x44, 0x7C, 0x2A, 0x2A, 0x3E, 0x14, + 0x7E, 0x02, 0x02, 0x06, 0x06, 0x02, 0x7E, 0x02, 0x7E, 0x02, 0x63, 0x55, 0x49, 0x41, 0x63, + 0x38, 0x44, 0x44, 0x3C, 0x04, 0x40, 0x7E, 0x20, 0x1E, 0x20, 0x06, 0x02, 0x7E, 0x02, 0x02, + 0x99, 0xA5, 0xE7, 0xA5, 0x99, 0x1C, 0x2A, 0x49, 0x2A, 0x1C, 0x4C, 0x72, 0x01, 0x72, 0x4C, + 0x30, 0x4A, 0x4D, 0x4D, 0x30, 0x30, 0x48, 0x78, 0x48, 0x30, 0xBC, 0x62, 0x5A, 0x46, 0x3D, + 0x3E, 0x49, 0x49, 0x49, 0x00, 0x7E, 0x01, 0x01, 0x01, 0x7E, 0x2A, 0x2A, 0x2A, 0x2A, 0x2A, + 0x44, 0x44, 0x5F, 0x44, 0x44, 0x40, 0x51, 0x4A, 0x44, 0x40, 0x40, 0x44, 0x4A, 0x51, 0x40, + 0x00, 0x00, 0xFF, 0x01, 0x03, 0xE0, 0x80, 0xFF, 0x00, 0x00, 0x08, 0x08, 0x6B, 0x6B, 0x08, + 0x36, 0x12, 0x36, 0x24, 0x36, 0x06, 0x0F, 0x09, 0x0F, 0x06, 0x00, 0x00, 0x18, 0x18, 0x00, + 0x00, 0x00, 0x10, 0x10, 0x00, 0x30, 0x40, 0xFF, 0x01, 0x01, 0x00, 0x1F, 0x01, 0x01, 0x1E, + 0x00, 0x19, 0x1D, 0x17, 0x12, 0x00, 0x3C, 0x3C, 0x3C, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00] #ascii fonts + def __init__(self,I2C,**args): + self.buff =[0 for a in range(1024)] + self.ADDRESS = args.get('address',self.ADDRESS) + self.I2C = I2C + self.SSD1306_command(self.SSD1306_DISPLAYOFF); #0xAE + self.SSD1306_command(self.SSD1306_SETDISPLAYCLOCKDIV); # 0xD5 + self.SSD1306_command(0x80); # the suggested ratio 0x80 + self.SSD1306_command(self.SSD1306_SETMULTIPLEX); # 0xA8 + self.SSD1306_command(0x3F); + self.SSD1306_command(self.SSD1306_SETDISPLAYOFFSET); # 0xD3 + self.SSD1306_command(0x0); # no offset + self.SSD1306_command(self.SSD1306_SETSTARTLINE | 0x0); # line #0 + self.SSD1306_command(self.SSD1306_CHARGEPUMP); # 0x8D + self.SSD1306_command(0x14); #vccstate = self.SSD1306_SWITCHCAPVCC; + self.SSD1306_command(self.SSD1306_MEMORYMODE); # 0x20 + self.SSD1306_command(0x00); # 0x0 act like ks0108 + self.SSD1306_command(self.SSD1306_SEGREMAP | 0x1); + self.SSD1306_command(self.SSD1306_COMSCANDEC); + self.SSD1306_command(self.SSD1306_SETCOMPINS); # 0xDA + self.SSD1306_command(0x12); + self.SSD1306_command(self.SSD1306_SETCONTRAST); # 0x81 + self.SSD1306_command(0xFF); # vccstate = self.SSD1306_SWITCHCAPVCC; + self.SSD1306_command(self.SSD1306_SETPRECHARGE); # 0xd9 + self.SSD1306_command(0xF1); # vccstate = self.SSD1306_SWITCHCAPVCC; + self.SSD1306_command(self.SSD1306_SETVCOMDETECT); # 0xDB + self.SSD1306_command(0x40); + self.SSD1306_command(self.SSD1306_DISPLAYALLON_RESUME); # 0xA4 + self.SSD1306_command(self.SSD1306_NORMALDISPLAY); # 0xA6 + self.SSD1306_command(self.SSD1306_DISPLAYON);#--turn on oled panel + + def load(self,arg): + self.scroll('stop') + if arg=='logo': + self.clearDisplay() + for a in range(1024):self.buff[a]=self.logobuff[a] + self.displayOLED() + + def SSD1306_command(self,cmd): + self.I2C.writeBulk(self.ADDRESS,[0x00,cmd]) + + def SSD1306_data(self,data): + self.I2C.writeBulk(self.ADDRESS,[0x40,data]) + + def clearDisplay(self): + self.setCursor(0,0) + for a in range(self.SSD1306_LCDWIDTH*self.SSD1306_LCDHEIGHT/8):self.buff[a]=0; + + + def displayOLED(self): + self.SSD1306_command(self.SSD1306_SETLOWCOLUMN | 0x0); # low col = 0 + self.SSD1306_command(self.SSD1306_SETHIGHCOLUMN | 0x0); # hi col = 0 + self.SSD1306_command(self.SSD1306_SETSTARTLINE | 0x0); # line #0 + a=0 + while (a < self.SSD1306_LCDWIDTH*self.SSD1306_LCDHEIGHT/8): + self.I2C.writeBulk(self.ADDRESS,[0x40]+self.buff[a:a+16]) + a+=16 + + def setContrast(self,i): + self.SSD1306_command(self.SSD1306_SETCONTRAST) + self.SSD1306_command(i) + + + + def drawPixel(self,x,y,color): + if (color == 1): + self.buff[x+ (y/8)*self.SSD1306_LCDWIDTH] |= (1<<(y%8)) + else: + self.buff[x+ (y/8)*self.SSD1306_LCDWIDTH] &= ~(1<<(y%8)) + + def drawCircle(self,x0,y0, r,color): + f = 1 - r + ddF_x = 1 + ddF_y = -2 * r + x = 0 + y = r + self.drawPixel(x0, y0+r, color) + self.drawPixel(x0, y0-r, color) + self.drawPixel(x0+r, y0, color) + self.drawPixel(x0-r, y0, color) + while (x= 0): + y-=1 + ddF_y += 2 + f += ddF_y + x+=1 + ddF_x += 2 + f += ddF_x + self.drawPixel(x0 + x, y0 + y, color) + self.drawPixel(x0 - x, y0 + y, color) + self.drawPixel(x0 + x, y0 - y, color) + self.drawPixel(x0 - x, y0 - y, color) + self.drawPixel(x0 + y, y0 + x, color) + self.drawPixel(x0 - y, y0 + x, color) + self.drawPixel(x0 + y, y0 - x, color) + self.drawPixel(x0 - y, y0 - x, color) + + + def drawLine(self,x0, y0, x1, y1, color): + steep = abs(y1 - y0) > abs(x1 - x0) + if (steep): + tmp = y0 + y0=x0 + x0=tmp + tmp = y1 + y1=x1 + x1=tmp + if (x0 > x1): + tmp = x1 + x1=x0 + x0=tmp + tmp = y1 + y1=y0 + y0=tmp + + dx = x1 - x0 + dy = abs(y1 - y0) + err = dx / 2 + + if (y0 < y1): + ystep = 1 + else: + ystep = -1 + + while(x0<=x1): + if (steep): self.drawPixel(y0, x0, color) + else: self.drawPixel(x0, y0, color) + err -= dy + if (err < 0): + y0 += ystep + err += dx + x0+=1 + + def drawRect(self,x, y, w,h,color): + self.drawFastHLine(x, y, w, color) + self.drawFastHLine(x, y+h-1, w, color) + self.drawFastVLine(x, y, h, color) + self.drawFastVLine(x+w-1, y, h, color) + + + def drawFastVLine(self,x, y, h, color): + self.drawLine(x, y, x, y+h-1, color) + + def drawFastHLine(self,x, y, w, color): + self.drawLine(x, y, x+w-1, y, color) + + + def fillRect(self, x, y, w, h, color): + for i in range(x,x+w): + self.drawFastVLine(i, y, h, color) + + + def writeString(self,s): + for a in s: self.writeChar(ord(a)) + + def writeChar(self,c): + if (c == '\n'): + cursor_y += textsize*8; + cursor_x = 0; + elif(c == '\r'): + pass + else: + self.drawChar(self.cursor_x, self.cursor_y, c, self.textcolor, self.textbgcolor, self.textsize) + self.cursor_x += self.textsize*6 + if (self.wrap and (self.cursor_x > (self._width - self.textsize*6))): + self.cursor_y += self.textsize*8 + self.cursor_x = 0 + + def drawChar(self, x, y, c,color, bg, size): + if((x >= self._width) or (y >= self._height) or ((x + 5 * size - 1) < 0) or ((y + 8 * size - 1) < 0)): + return; + for i in range(6): + if (i == 5): line = 0x0; + else: line = self.font[c*5+i]; + for j in range(8): + if (line & 0x1): + if (size == 1): self.drawPixel(x+i, y+j, color); + else: self.fillRect(x+(i*size), y+(j*size), size, size, color); + elif (bg != color): + if (size == 1): self.drawPixel(x+i, y+j, bg); + else: self.fillRect(x+i*size, y+j*size, size, size, bg); + line >>= 1 + + def setCursor(self, x, y): + self.cursor_x = x + self.cursor_y = y + + def setTextSize(self,s): + self.textsize = s if (s > 0) else 1 + + def setTextColor(self,c, b): + self.textcolor = c + self.textbgcolor = b + + + def setTextWrap(self,w): + self.wrap = w + + def scroll(self,arg): + if arg=='left': + self.SSD1306_command(0x27) #up-0x29 ,2A left-0x27 right0x26 + if arg=='right': + self.SSD1306_command(0x26) #up-0x29 ,2A left-0x27 right0x26 + if arg in ['topright','bottomright']: + self.SSD1306_command(0x29) #up-0x29 ,2A left-0x27 right0x26 + if arg in ['topleft','bottomleft']: + self.SSD1306_command(0x2A) #up-0x29 ,2A left-0x27 right0x26 + + if arg in ['left','right','topright','topleft','bottomleft','bottomright']: + self.SSD1306_command(0x00) #dummy + self.SSD1306_command(0x0) #start page + self.SSD1306_command(0x7) #time interval 0b100 - 3 frames + self.SSD1306_command(0xf) #end page + if arg in ['topleft','topright']: + self.SSD1306_command(0x02) #dummy 00 . xx for horizontal scroll (speed) + elif arg in ['bottomleft','bottomright']: + self.SSD1306_command(0xfe) #dummy 00 . xx for horizontal scroll (speed) + + if arg in ['left','right']: + self.SSD1306_command(0x02) #dummy 00 . xx for horizontal scroll (speed) + self.SSD1306_command(0xff) + + self.SSD1306_command(0x2F) + + if arg=='stop': + self.SSD1306_command(0x2E) + + + def pulseIt(self): + for a in range(2): + self.SSD1306_command(0xD6) + self.SSD1306_command(0x01) + time.sleep(0.1) + self.SSD1306_command(0xD6) + self.SSD1306_command(0x00) + time.sleep(0.1) + + + +if __name__ == "__main__": + from PSL import sciencelab + I= sciencelab.connect() + O=connect(I.I2C) + textbgcolor=0 + textcolor=1 + O.load('logo') + O.scroll('topright') + import time + time.sleep(2.8) + O.scroll('stop') + +