87 lines
No EOL
2.6 KiB
Python
87 lines
No EOL
2.6 KiB
Python
import atexit
|
|
from enum import Enum
|
|
|
|
from compLib.LogstashLogging import Logging
|
|
from compLib.MetricsLogging import MetricsLogging
|
|
from compLib.Spi import Spi, Register
|
|
|
|
MOTOR_COUNT = 4
|
|
|
|
|
|
encoder_start_values = [0] * (MOTOR_COUNT + 1)
|
|
encoder_last_raw_value = [0] * (MOTOR_COUNT + 1)
|
|
|
|
class Encoder(object):
|
|
"""Class used to read the encoders
|
|
"""
|
|
|
|
@staticmethod
|
|
def read_raw(port: int) -> int:
|
|
"""Read raw encoder from a specified port. Will not be reset to 0 at start.
|
|
|
|
:param port: Port, which the motor is connected to. Between 1 and 4
|
|
:raises: IndexError
|
|
:return: Current encoder position
|
|
"""
|
|
if port <= 0 or port > MOTOR_COUNT:
|
|
raise IndexError("Invalid encoder port specified!")
|
|
|
|
global encoder_last_raw_value
|
|
raw_value = 0
|
|
|
|
if port == 1:
|
|
raw_value = Spi.read(Register.MOTOR_1_POS_B3, 4)
|
|
elif port == 2:
|
|
raw_value = Spi.read(Register.MOTOR_2_POS_B3, 4)
|
|
elif port == 3:
|
|
raw_value = Spi.read(Register.MOTOR_3_POS_B3, 4)
|
|
elif port == 4:
|
|
raw_value = Spi.read(Register.MOTOR_4_POS_B3, 4)
|
|
|
|
if abs(raw_value - encoder_last_raw_value[port]) > 1000:
|
|
encoder_last_raw_value[port] = raw_value
|
|
return Encoder.read_raw(port)
|
|
|
|
encoder_last_raw_value[port] = raw_value
|
|
return raw_value
|
|
|
|
@staticmethod
|
|
def read(port: int) -> int:
|
|
"""Read encoder from a specified port
|
|
|
|
:param port: Port, which the motor is connected to. Between 1 and 4
|
|
:raises: IndexError
|
|
:return: Current encoder position
|
|
"""
|
|
if port <= 0 or port > MOTOR_COUNT:
|
|
raise IndexError("Invalid encoder port specified!")
|
|
|
|
diff = Encoder.read_raw(port) - encoder_start_values[port]
|
|
if diff > 2 ** 31:
|
|
diff -= 2 ** 32
|
|
elif diff < -2 ** 31:
|
|
diff += 2 ** 32
|
|
|
|
MetricsLogging.put("Encoder", diff, port)
|
|
return diff
|
|
|
|
@staticmethod
|
|
def clear(port: int):
|
|
"""Reset encoder position to 0
|
|
|
|
:param port: Port, which the motor is connected to. Between 1 and 4
|
|
:raises: IndexError
|
|
"""
|
|
if port <= 0 or port > MOTOR_COUNT:
|
|
raise IndexError("Invalid encoder port specified!")
|
|
|
|
encoder_start_values[port] = Encoder.read_raw(port)
|
|
|
|
@staticmethod
|
|
def clear_all():
|
|
"""Reset all encoder positions to 0
|
|
"""
|
|
|
|
for i in range(1, MOTOR_COUNT + 1):
|
|
encoder_start_values[i] = Encoder.read_raw(i)
|
|
|