compAIR/compLIB
compAIR/compLIB
Archived
4
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Protobuf prototype

Browse source
This commit is contained in:
Konstantin Lampalzer 2022-03-18 18:11:16 +01:00
parent e277a83c5f
commit 9b567b8c6c
119 changed files with 8599 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

127
compLib/Encoder.py
View file

@ -1,127 +0,0 @@
import atexit
from enum import Enum
from compLib.LogstashLogging import Logging
from compLib.Spi import Spi, Register
MOTOR_COUNT = 4
encoder_start_values = [0] * (MOTOR_COUNT + 1)
class Encoder(object):
"""Class used to read the encoders
"""
@staticmethod
def handle_wrap(raw_value, port):
"""Handle overflow and underflow of int for encoders.
:param raw_value: Raw value which was read on 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 = raw_value - encoder_start_values[port]
if diff > 2 ** 31:
diff -= 2 ** 32
elif diff < -2 ** 31:
diff += 2 ** 32
return diff
@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!")
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)
return raw_value
@staticmethod
def read_all_raw():
"""Read all encoders at once.
This is faster than read_raw as it only uses one SPI call for all encoders!
:return: Tuple of all current raw encoder positions
"""
encoders = Spi.read_array(Register.MOTOR_1_POS_B3, 4 * 4)
encoder_1 = int.from_bytes(
encoders[0:4], byteorder='big', signed=False)
encoder_2 = int.from_bytes(
encoders[4:8], byteorder='big', signed=False)
encoder_3 = int.from_bytes(
encoders[8:12], byteorder='big', signed=False)
encoder_4 = int.from_bytes(
encoders[12:16], byteorder='big', signed=False)
return (encoder_1, encoder_2, encoder_3, encoder_4)
@staticmethod
def read_all():
"""Read all encoders at once.
This is faster than read as it only uses one SPI call for all encoders!
:return: Tuple of all current encoder positions
"""
encoders = Encoder.read_all_raw()
return (Encoder.handle_wrap(encoders[0], 1),
Encoder.handle_wrap(encoders[1], 2),
Encoder.handle_wrap(encoders[2], 3),
Encoder.handle_wrap(encoders[3], 4))
@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!")
return Encoder.handle_wrap(Encoder.read_raw(port), port)
@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)