from machine import Pin, SoftSPI
import time

class TLE5012B() :
    def __init__(self,sck_pin=2,mosi_pin=13,miso_pin=15,cs_pin=4):
        self.spi = SoftSPI(baudrate=8000000, sck=Pin(sck_pin), mosi=Pin(mosi_pin), miso=Pin(miso_pin))          #defining the SPI object
        #self.spi.init()
        self.cs = Pin(cs_pin,mode=Pin.OUT, value=1) #defining chip select pin
        self.rotation_counter = 0
        self.previous_angle = 0
        self.start_angle = 0
        self.zero()
        
    def zero(self):
        self.start_angle = self.get_angle_displacement()
        

    def get_angle(self):
        rxdata = bytearray(2) #defining a 4 byte array
        command = '1000000000100001'  #defining the command that reads the chip
        txdata = int(command,2).to_bytes(2,'big')  #converting to bytes
        self.cs(0) #activating the chip through the chip select pin
        self.spi.write(txdata) # Write the command which takes up two bytes
        self.spi.readinto(rxdata)# Reading the data that was recieve from the chip and placing it the rx register
        self.cs(1) #deactivating the chip once transaction is finished
        byte1 = '{0:08b}'.format(rxdata[0]) #converting first bit to a string of ones and zeros
        byte2 = '{0:08b}'.format(rxdata[1]) #converting second bit to a string of ones and zeros
        data = byte1[1:]+byte2 #adding all of the strings together and extracting the angle data (total is a 15 bit number)
        steps = int(data,2) #getting the 15 bit number as an integer
        angle = (360*steps)/32768 #converting to angle
        if ((self.previous_angle > 240) and (self.previous_angle <= 360)) and ((angle >= 0) and (angle < 120)):
            self.rotation_counter += 1
        elif ((angle > 240) and (angle <= 360)) and ((self.previous_angle >= 0) and (self.previous_angle < 120)):
            self.rotation_counter -= 1
        self.previous_angle = angle
        return(angle)
    
    def get_revs_slave(self):
        rxdata = bytearray(4) #defining a 4 byte array
        command = '1000000000110010'  #defining the command that reads the chip
        txdata = int(command,2).to_bytes(2,'big')  #converting to bytes
        self.cs(0) #activating the chip through the chip select pin
        self.spi.write(txdata) # Write the command which takes up two bytes
        self.spi.readinto(rxdata)# Reading the data that was recieve from the chip and placing it the rx register
        self.cs(1) #deactivating the chip once transaction is finished
        byte1 = '{0:08b}'.format(rxdata[2]) #converting third bit to a string of ones and zeros
        byte2 = '{0:08b}'.format(rxdata[3]) #converting fourth bit to a string of ones and zeros
        data_rev = byte1[-1]+byte2 #adding all of the strings together and extracting the rev data (total is a 9 bit number)
        revs = int(data_rev,2) #getting the 9 bit number as an integer
        return revs
    
    def print_all_data(self,command):
        #command = '1000000000100001' read angle command
        #command = '1000000001000001' read revolutions command (TLE5012B Ragester Settings datasheet has more information https://www.infineon.com/dgdl/TLE5012B_Register_Setting_AN_Rev1.1.pdf?folderId=db3a30431ce5fb52011d29810c9e1b6a&fileId=db3a304330f68606013141cf6ae75690) 
        txdata2 = int(command,2).to_bytes(2,'big')  #converting to bytes
        rxdata2 = bytearray(10)
        self.cs(0) #activating the chip through the chip select pin
        self.spi.write(txdata2) # Write the command which takes up two bytes
        self.spi.readinto(rxdata2)# Reading the data that was recieve from the chip and placing it the rx register
        self.cs(1) #deactivating the chip once transaction is finished
        for x in rxdata2:
            print('{0:08b}'.format(x))
        print("######################")
        
    def get_revs_master(self):
        return self.rotation_counter
    
    def get_angle_displacement(self):
        return self.get_angle() + (self.get_revs_master()*360) - self.start_angle
        
############################################################################
    #Example     
         
         
encoder = TLE5012B(sck_pin=2,mosi_pin=13,miso_pin=15,cs_pin=4) #defining sensor object
while True:#creating a loop to get live updates
    #print(encoder.get_angle()) #printing sensor's position
    #encoder.print_all_data('1000000000100001') #printing the binary that is returned from a certain command as a debugging tool
    #print(encoder.get_revs_slave())
    #print(encoder.get_revs_master())
    print(encoder.get_angle_displacement())
    time.sleep(.05) #control the speed of the loop by sleeping for .4 second