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_start_values = [0] * (MOTOR_COUNT + 1)
encoder_last_raw_value = [0] * (MOTOR_COUNT + 1)
class Encoder(object): class Encoder(object):
"""Class used to read the encoders """Class used to read the encoders
@ -24,15 +25,25 @@ class Encoder(object):
""" """
if port <= 0 or port > MOTOR_COUNT: if port <= 0 or port > MOTOR_COUNT:
raise IndexError("Invalid encoder port specified!") raise IndexError("Invalid encoder port specified!")
global encoder_last_raw_value
raw_value = 0
if port == 1: 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: 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: 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: 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 @staticmethod
def read(port: int) -> int: def read(port: int) -> int:
@ -48,6 +59,8 @@ class Encoder(object):
diff = Encoder.read_raw(port) - encoder_start_values[port] diff = Encoder.read_raw(port) - encoder_start_values[port]
if diff > 2 ** 31: if diff > 2 ** 31:
diff -= 2 ** 32 diff -= 2 ** 32
elif diff < -2 ** 31:
diff += 2 ** 32
MetricsLogging.put("Encoder", diff, port) MetricsLogging.put("Encoder", diff, port)
return diff return diff

27
compLib/Motor.py
View file

@ -8,7 +8,7 @@ from compLib.Spi import Spi, Register
MOTOR_COUNT = 4 MOTOR_COUNT = 4
MAX_MOTOR_SPEED = 65535 MAX_MOTOR_SPEED = 65535
MOTOR_PERCENTAGE_MULT = MAX_MOTOR_SPEED / 100.0 MOTOR_PERCENTAGE_MULT = MAX_MOTOR_SPEED / 100.0
MOTOR_CURVE = [0.0, 0.0, 426.5, 692.0, 842.5, 953.5, 1032.5, 1090.5, 1135.5, 1171.0, 1203.5, 1230.0, 1249.5, 1268.0, 1283.0, 1298.5, 1308.0, 1320.0, 1332.0, 1339.5, 1352.5] MOTOR_CURVE = [0.0, 0.0, 0.5, 60.0, 199.83333333333334, 377.66666666666663, 990.3333333333333, 1860.6666666666665, 2587.0, 3091.6666666666665, 3489.0, 3860.5, 4197.333333333333, 4432.166666666667, 4647.166666666666, 4873.166666666666, 5054.333333333334, 5208.666666666667, 5353.0, 5466.5, 5604.0]
SPEED_LOCK = False SPEED_LOCK = False
SPEED_MULT = 1.0 SPEED_MULT = 1.0
@ -33,7 +33,7 @@ class Motor(object):
:param mode: Motor mode. See enum MotorMode for more info :param mode: Motor mode. See enum MotorMode for more info
:raises: IndexError :raises: IndexError
""" """
if SPEED_LOCK: if SPEED_LOCK:
return return
@ -92,11 +92,9 @@ class Motor(object):
:param percent: Percentage of max speed. between -100 and 100 :param percent: Percentage of max speed. between -100 and 100
:raises: IndexError :raises: IndexError
""" """
raw_power = 0 raw_power = raw_power = Motor.__linearizePower(MOTOR_CURVE, abs(percent))
if percent > 0: if percent < 0:
raw_power = Motor.__linearizePower(MOTOR_CURVE, percent) raw_power *= -1
elif percent < 0:
raw_power = -Motor.__linearizePower(MOTOR_CURVE, -percent)
MetricsLogging.put("Motor", float(percent), port) MetricsLogging.put("Motor", float(percent), port)
Motor.power_raw(port, raw_power, False) Motor.power_raw(port, raw_power, False)
@ -135,7 +133,8 @@ class Motor(object):
""" """
if (len(curve) != 21): if (len(curve) != 21):
raise ValueError('The motor curve is invalid, check documentation for set_motor_curve()!') raise ValueError('The motor curve is invalid, check documentation for set_motor_curve()!')
global MOTOR_CURVE
MOTOR_CURVE = curve MOTOR_CURVE = curve
@staticmethod @staticmethod
@ -152,6 +151,12 @@ class Motor(object):
""" """
Linear interpolation. Check https://www.arduino.cc/reference/en/language/functions/math/map/ Linear interpolation. Check https://www.arduino.cc/reference/en/language/functions/math/map/
""" """
x = float(x)
in_min = float(in_min)
in_max = float(in_max)
out_min = float(out_min)
out_max = float(out_max)
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
@staticmethod @staticmethod
@ -160,10 +165,10 @@ class Motor(object):
Interpolate a speed in the specified motor curve and return Interpolate a speed in the specified motor curve and return
the 'power percentage that is needed to reach that speed' the 'power percentage that is needed to reach that speed'
""" """
if speed < curve[0] or speed > curve[20]: if speed < min(curve) or speed > max(curve):
raise ValueError(f'Speed out of range: {str(speed)} ticks/s') raise ValueError(f'Speed out of range: {str(speed)} ticks/s')
for index in range(20): # There are 20 speed ranges for index in range(len(curve) - 2):
if speed >= curve[index] and speed <= curve[index + 1] and curve[index] != curve[index + 1]: if speed >= curve[index] and speed <= curve[index + 1] and curve[index] != curve[index + 1]:
return Motor.__map(speed, curve[index], curve[index + 1], index * 5, index * 5 + 5) return Motor.__map(speed, curve[index], curve[index + 1], index * 5, index * 5 + 5)
@ -177,7 +182,7 @@ class Motor(object):
if power < 0 or power > 100: if power < 0 or power > 100:
raise ValueError(f'Power out of range: {str(power)}%') raise ValueError(f'Power out of range: {str(power)}%')
requiredSpeed = Motor.__map(power, 0, 100, curve[0], curve[20]) requiredSpeed = Motor.__map(power, 0, 100, min(curve), max(curve))
return Motor.__interpolateSpeed(curve, requiredSpeed) return Motor.__interpolateSpeed(curve, requiredSpeed)