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

New curve, encoder filter

Browse source
This commit is contained in:
root 2021-11-19 20:26:57 +00:00
parent d3a26a9539
commit 9e89123e92
2 changed files with 33 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

21
compLib/Encoder.py
View file

@ -9,6 +9,7 @@ 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
@ -24,15 +25,25 @@ class Encoder(object):
"""
if port <= 0 or port > MOTOR_COUNT:
raise IndexError("Invalid encoder port specified!")
global encoder_last_raw_value
raw_value = 0
if port == 1:
return Spi.read(Register.MOTOR_1_POS_B3, 4)
raw_value = Spi.read(Register.MOTOR_1_POS_B3, 4)
elif port == 2:
return Spi.read(Register.MOTOR_2_POS_B3, 4)
raw_value = Spi.read(Register.MOTOR_2_POS_B3, 4)
elif port == 3:
return Spi.read(Register.MOTOR_3_POS_B3, 4)
raw_value = Spi.read(Register.MOTOR_3_POS_B3, 4)
elif port == 4:
return Spi.read(Register.MOTOR_4_POS_B3, 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:
@ -48,6 +59,8 @@ class Encoder(object):
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