Hi everyone,
I’m using the Grove - 12-bit Magnetic Rotary Position Sensor / Encoder (AS5600) to acquire some information on a wheel. I’m using python and a Raspberry Pi to acquire the data.
I’m very satisfied of the precision of the sensor, but I post on the forum because I have small errors when I move slowly my wheel like you can see on the attachments
.
I have some difficulties to debug my program because I cannot visualize properly due to the speed of the process (the error is on the convert_deltaAngleDeg_rotations function).
Do you have any suggestion to fix this issue or improve my program ?
from smbus import SMBus
import logging
from math import pi
import time
from dataclasses import dataclass
from numpy import deg2rad
import threading
logging.basicConfig(level=logging.INFO)
RADIUS_WHEEL = 0.04 # in meter
wait = threading.Event().wait
@dataclass
class Angle:
rawValue: int
degree: float
radian: float
class single_encoder:
def __init__(self, busI2C):
self.bus = SMBus(busI2C) # Create an instance of the SMBus class
self.AS5600_ADDRESS = 0x36 # I2C address of the AS5600 sensor
self.AS5600_REG_ANGLE = 0x0E # Register to read the angular position
self.rotations = 0
self.totalDistance = 0
self.rotationPerMinute = 0
self.speed_dps = 0
self.speed_rad_sec = 0
self.linear_speed = 0
def getRawAngle(self):
"""Reading data from the angle register
Returns:
rawAngle: Numeric value between 0 and 4096 representing the angle
"""
data = self.bus.read_i2c_block_data(self.AS5600_ADDRESS, self.AS5600_REG_ANGLE, 2)
rawAngle = ((data[0] & 0xFF) << 8 | (data[1] & 0xFF))
return rawAngle
def getDegree(self):
"""Convert raw data from the encoder to degree
Returns:
float: value in degree
"""
rawAngle = self.getRawAngle()
angleDegree = rawAngle / 4096 * 360.0
return angleDegree
def getRadian(self):
"""Convert raw data from the encoder to radian
Returns:
float: value in radian
"""
rawAngle = self.getRawAngle()
angleRadian = rawAngle / 4096 * 2 * pi
return angleRadian
def convert_deltaAngleDeg_rotations(delta_angle_deg):
"""Convertis la différence d'angle en rotation de roue
Args:
delta_angle_deg : différence d'angle en degrée
"""
# ToDo : Measurement error (rotation) on low translation / move of the wheel
if delta_angle_deg != 0: # Detection of a move
if abs(delta_angle_deg) >= 180: # we have crossed the 0°. The count of degree we moved is calculated by adding or subtracting a full turn (360°) depending on current sense of rotation
if delta_angle_deg > 0: # the difference sign is positive we have moved clockwise (increment angle)
return (delta_angle_deg + 360) / 360
else: # the difference sign is negative we have moved counter clockwise (decrement angle)
return (delta_angle_deg - 360) / 360
else:
return delta_angle_deg / 360
else:
return 0
def calculate_rotationPerMinute(time_interval, rotations):
return rotations / time_interval * 60
def getSpeedDps(time_interval, delta_angle_deg):
return delta_angle_deg / time_interval
def getSpeedRadS(speed_dps):
return deg2rad(speed_dps)
def getLinearSpeed(speed_rad_sec):
return RADIUS_WHEEL * speed_rad_sec
def getDistance(totalAngleRad):
return RADIUS_WHEEL * totalAngleRad
def CalculateSpeed(self):
"""Process & update the speed variables of the object
"""
last_time = time.time()
last_angle = Angle(self.getRawAngle(), self.getDegree(), self.getRadian())
while True:
current_angle = Angle(self.getRawAngle(), self.getDegree(), self.getRadian())
delta_angle_deg = round(current_angle.degree - last_angle.degree, 2)
self.rotations += single_encoder.convert_deltaAngleDeg_rotations(delta_angle_deg)
current_time = time.time()
delta_time = current_time - last_time
logging.debug(f"delta_time = {delta_time}")
if delta_time >= 1: # update speed info every second
self.rotationPerMinute = single_encoder.calculate_rotationPerMinute(delta_time, self.rotations)
totalAngleDeg = self.rotations * 360
totalAngleRad = deg2rad(totalAngleDeg)
self.speed_dps = single_encoder.getSpeedDps(delta_time, totalAngleDeg)
self.speed_rad_sec = single_encoder.getSpeedRadS(self.speed_dps)
self.linear_speed = single_encoder.getLinearSpeed(self.speed_rad_sec)
self.totalDistance += single_encoder.getDistance(totalAngleRad)
last_time = current_time
self.rotations = 0
last_angle = current_angle
def Capture(self):
Thread = threading.Thread(target=self.CalculateSpeed,daemon=True)
Thread.start()
LeftEncoder = single_encoder(5)
LeftEncoder.Capture()
while True:
print(f"number of rotations: {LeftEncoder.rotations}")
wait(0.1)
Thanks in advance for your answer,
Best regards
PJ