Skip to content

Commit

Permalink
Update AS5x47.py
Browse files Browse the repository at this point in the history 
Signed-off-by: IhorNehrutsa <Ihor.Nehrutsa@gmail.com>
  • Loading branch information
IhorNehrutsa authored and IhorNehrutsa committed Dec 25, 2023
1 parent 553a7a2 commit 4fcdd34
Showing 1 changed file with 102 additions and 87 deletions.
189 changes: 102 additions & 87 deletions ports/esp32/modules/AS5x47.py
View file
Original file line number Diff line number Diff line change
@@ -1,20 +1,21 @@
from micropython import const
from uctypes import BF_POS, BF_LEN, BFUINT16, BIG_ENDIAN, struct, addressof #, bytes_at, sizeof
from utime import sleep_us
from machine import SPI, Pin

# ----------------------------------------------------------------------
__MSB_mask = 0x4000 # for lower 15 bits
__MSB_mask = const(0x4000) # for lower 15 bits
__to_angle = 360 / 0x4000

@micropython.native

Check failure on line 10 in ports/esp32/modules/AS5x47.py

View workflow job for this annotation

GitHub Actions / ruff

Ruff (F821) 

ports/esp32/modules/AS5x47.py:10:2: F821 Undefined name `micropython`
def is_even(data, msb_mask):
# Calc parity bit
count = 0
even = False
while msb_mask:
if data & msb_mask:
count ^= 1
even = not even
msb_mask >>= 1
return count
return 1 if even else 0


# ----------------------------------------------------------------------
Expand Down Expand Up @@ -168,113 +169,127 @@ class AS5x47():
#
# spi = SPI(HSPI_ID, sck=Pin(HSPI_sck), mosi=Pin(HSPI_mosi), miso=Pin(HSPI_miso), baudrate=10_000_000, polarity=0, phase=1, bits=16, firstbit=SPI.MSB)
# cs = Pin(esp32_.HSPI_cs1, Pin.OUT, value=1)
def __init__(self, spi, spi_baudrate, cs, direction=1):


NO_error = 0
EF_error = 1
PARD_error = 2
DATA_error = 3


def __init__(self, spi, spi_baudrate, cs):
assert isinstance(spi, SPI)
assert isinstance(cs, Pin)

self.spi = spi
self.tclk_2_us = 1_000_000 // (spi_baudrate * 2) + 1 # tH = tclk / 2 us # Time between last falling edge of CLK and rising edge of cs
self.tclk_2_us = 1_000_000 // (spi_baudrate * 2) # tH = tclk / 2 ns # Time between last falling edge of CLK and rising edge of cs
if self.tclk_2_us == 0:
self.tclk_2_us = 1
print('self.tclk_2_us:', self.tclk_2_us)
self.cs = cs # active is low
self.direction = direction # set -1 to reverse angles

self._write_command = bytearray(b'\xC0\x00')
self._received_data = bytearray(2)
self._received_data_prev = bytearray(b'\x00\x00')
self._received_data = bytearray(b'\x00\x00')
self.received_frame = struct(addressof(self._received_data), Read_Data_Frame_struct, BIG_ENDIAN)
self._command_buff = bytearray(2)
self._command_buff = bytearray(b'\x00\x00')
self.command_frame = struct(addressof(self._command_buff), Command_Frame_struct, BIG_ENDIAN)
self._data_buff = bytearray(2)
self._data_buff = bytearray(b'\x00\x00')
self.data_frame = struct(addressof(self._data_buff), Write_Data_Frame_struct, BIG_ENDIAN)

self.error = self.NO_error

self._angle_major = 0
self._angle14 = 0
# set _angle14 !!!
self.readAngleCom()
self.readAngleCom()
self.readAngleCom()
####### self._angle14_prev = self._angle14
####### if self._angle14_prev >= 8192:
####### self._angle14_prev = 0
self._angle14_prev = 0

self.error = False
self._angle14_prev = self._angle14
if self._angle14_prev >= 8192:
self._angle14_prev = 0

self._angle_major = 0

# ------------------
@micropython.native

Check failure on line 214 in ports/esp32/modules/AS5x47.py

View workflow job for this annotation

GitHub Actions / ruff

Ruff (F821) 

ports/esp32/modules/AS5x47.py:214:6: F821 Undefined name `micropython`
def _readAngleInfinity(self, readAngleFunc):
angle_now = readAngleFunc() # 8192 == 2 ** 14 / 2
delta = self._angle14_prev - self._angle14
if delta >= 8192:
self._angle_major += self.direction * 360
elif delta <= -8192:
self._angle_major -= self.direction * 360
self._angle14_prev = self._angle14
return self._angle_major + angle_now
readAngleFunc() # 8192 == 2 ** 14 // 2
if not self.error:
delta = self._angle14 - self._angle14_prev
if delta >= 8192:
self._angle_major -= 360
#print(' self._angle14, self._angle14_prev, delta', self._angle14, self._angle14_prev, delta)
elif delta <= -8192:
self._angle_major += 360
#print(' self._angle14, self._angle14_prev, delta', self._angle14, self._angle14_prev, delta)
self._angle14_prev = self._angle14
return self._angle14 * __to_angle + self._angle_major
else:
return None

def __repr__(self):
return 'AS5x47(spi={}, cs={}, direction={})'.format(self.spi, self.cs, self.direction)
return 'AS5x47(spi={}, cs={})'.format(self.spi, self.cs)

@micropython.native
@micropython.viper

Check failure on line 233 in ports/esp32/modules/AS5x47.py

View workflow job for this annotation

GitHub Actions / ruff

Ruff (F821) 

ports/esp32/modules/AS5x47.py:233:6: F821 Undefined name `micropython`
def writeData(self, command, value):
# Send command
self.cs.off()
self.cs(0)
self.spi.write(command)
sleep_us(self.tclk_2_us)
self.cs.on()
self.cs(1)

# Send data
self.cs.off()
self.cs(0)
self.spi.write(value)
sleep_us(self.tclk_2_us)
self.cs.on()
self.cs(1)

# after every spi.write_readinto()
@micropython.native

Check failure on line 248 in ports/esp32/modules/AS5x47.py

View workflow job for this annotation

GitHub Actions / ruff

Ruff (F821) 

ports/esp32/modules/AS5x47.py:248:6: F821 Undefined name `micropython`
def checkReceivedFrame(self):
# if self.received_frame.EF:
# #raise RuntimeError('received_frame.EF on 0x%X' % self.command_frame.ADDR)
# print('received_frame.EF on 0x%X' % self.command_frame.ADDR)
# self._received_data[:] = self._received_data_prev[:]
# elif self.received_frame.PARD != is_even(self.received_frame.DATA, __MSB_mask):
# #raise ValueError('received_frame.PARD != is_even on 0x%X' % self.command_frame.ADDR)
# print('received_frame.PARD != is_even on 0x%X' % self.command_frame.ADDR)
# self._received_data[:] = self._received_data_prev[:]
# else:
# self._received_data_prev[:] = self._received_data[:]
if (self._received_data[0] == 0xff and self._received_data[1] == 0xff) \
or (self._received_data[0] == 0x00 and self._received_data[1] == 0x00):
self._received_data[:] = self._received_data_prev[:]
self.error = True
if self.received_frame.EF:
#raise RuntimeError('received_frame.EF on 0x%X' % self.command_frame.ADDR)
print('received_frame.EF on 0x%X' % self.command_frame.ADDR)
self.error = self.EF_error
elif self.received_frame.PARD != is_even(self.received_frame.DATA, __MSB_mask):
#raise ValueError('received_frame.PARD != is_even on 0x%X' % self.command_frame.ADDR)
print('received_frame.PARD != is_even on 0x%X' % self.command_frame.ADDR)
self.error = self.PARD_error
elif self._received_data == b'\xff\xff':
print("_received_data == b'\xff\xff'")
self.error = self.DATA_error
else:
self._received_data_prev[:] = self._received_data[:]
self.error = False
self.error = self.NO_error

@micropython.native
@micropython.viper

Check failure on line 264 in ports/esp32/modules/AS5x47.py

View workflow job for this annotation

GitHub Actions / ruff

Ruff (F821) 

ports/esp32/modules/AS5x47.py:264:6: F821 Undefined name `micropython`
def readData(self, command):
# Send Read Command
self._write_command = command # bytes_at(addressof(command), sizeof(command))
self.cs.off()
self.cs(0)
self.spi.write(self._write_command)
sleep_us(self.tclk_2_us)
self.cs.on()
self.cs(1)

# Send Read Command while receiving data
self.cs.off()
self.cs(0)
self.spi.write_readinto(self._write_command, self._received_data)
sleep_us(self.tclk_2_us)
self.cs.on()
# sleep_us(self.tclk_2_us)
# self.cs(1)

self.checkReceivedFrame()
self.cs(1)

@micropython.native
@micropython.viper

Check failure on line 282 in ports/esp32/modules/AS5x47.py

View workflow job for this annotation

GitHub Actions / ruff

Ruff (F821) 

ports/esp32/modules/AS5x47.py:282:6: F821 Undefined name `micropython`
def readDataAgain(self):
self.cs.off()
self.cs(0)
self.spi.write_readinto(self._write_command, self._received_data)
sleep_us(self.tclk_2_us)
self.cs.on()
# sleep_us(self.tclk_2_us)
# self.cs(1)

self.checkReceivedFrame()
self.cs(1)

@micropython.native
@micropython.viper

Check failure on line 292 in ports/esp32/modules/AS5x47.py

View workflow job for this annotation

GitHub Actions / ruff

Ruff (F821) 

ports/esp32/modules/AS5x47.py:292:6: F821 Undefined name `micropython`
def receivedFrameStruct(self, received_frame):
return struct(addressof(received_frame), Read_Data_Frame_struct, BIG_ENDIAN)

Expand All @@ -286,9 +301,9 @@ def readRegister(self, registerAddress):

self.readData(self.command_frame)

@micropython.native
def readRegisterAgain(self):
self.readDataAgain()
# @micropython.viper
# def readRegisterAgain(self):
# self.readDataAgain()

@micropython.native

Check failure on line 308 in ports/esp32/modules/AS5x47.py

View workflow job for this annotation

GitHub Actions / ruff

Ruff (F821) 

ports/esp32/modules/AS5x47.py:308:6: F821 Undefined name `micropython`
def writeRegister(self, registerAddress, registerValue):
Expand All @@ -303,49 +318,49 @@ def writeRegister(self, registerAddress, registerValue):
self.writeData(self.command_frame, self.data_frame)

# ------------------
# @micropython.native
# def readAngle(self):
# self.readRegister(ANGLEUNC)
# self._angle14 = struct(addressof(self._received_data), ANGLEUNC_struct, BIG_ENDIAN).CORDICANG
# return self.direction * self._angle14 * __to_angle
#
# @micropython.native
# def readAngleAgain(self):
# self.readRegisterAgain()
# self._angle14 = struct(addressof(self._received_data), ANGLEUNC_struct, BIG_ENDIAN).CORDICANG
# return self.direction * self._angle14 * __to_angle
#
# @micropython.native
# def readAngleInfinity(self):
# return self._readAngleInfinity(self.readAngleAgain)
@micropython.viper

Check failure on line 321 in ports/esp32/modules/AS5x47.py

View workflow job for this annotation

GitHub Actions / ruff

Ruff (F821) 

ports/esp32/modules/AS5x47.py:321:6: F821 Undefined name `micropython`
def readAngle(self):
self.readRegister(ANGLEUNC)
if not self.error:
self._angle14 = struct(addressof(self._received_data), ANGLEUNC_struct, BIG_ENDIAN).CORDICANG

@micropython.viper
def readAngleAgain(self):
self.readDataAgain()
if not self.error:
self._angle14 = struct(addressof(self._received_data), ANGLEUNC_struct, BIG_ENDIAN).CORDICANG

@micropython.viper
def readAngleInfinity(self):
return self._readAngleInfinity(self.readAngleAgain)

# ------------------
@micropython.native
@micropython.viper
def readAngleCom(self):
self.readRegister(ANGLECOM)
self._angle14 = struct(addressof(self._received_data), ANGLECOM_struct, BIG_ENDIAN).DAECANG
return self.direction * self._angle14 * __to_angle
if not self.error:
self._angle14 = struct(addressof(self._received_data), ANGLECOM_struct, BIG_ENDIAN).DAECANG

@micropython.native
@micropython.viper
def readAngleComAgain(self):
self.readRegisterAgain()
self._angle14 = struct(addressof(self._received_data), ANGLECOM_struct, BIG_ENDIAN).DAECANG
return self.direction * self._angle14 * __to_angle
self.readDataAgain()
if not self.error:
self._angle14 = struct(addressof(self._received_data), ANGLECOM_struct, BIG_ENDIAN).DAECANG

@micropython.native
@micropython.viper
def readAngleComInfinity(self):
return self._readAngleInfinity(self.readAngleComAgain)

# ------------------
# @micropython.native
# @micropython.viper
# def writeSettings1(self, value):
# self.writeRegister(SETTINGS1, value)
#
# @micropython.native
# @micropython.viper
# def writeSettings2(self, value):
# self.writeRegister(SETTINGS2, value)
#
# @micropython.native
# @micropython.viper
# def writeZeroPosition(self, zposm, zposl):
# self.writeRegister(ZPOSM, zposm)
# self.writeRegister(ZPOSL, zposl)

0 comments on commit 4fcdd34

Please sign in to comment.