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

27
compLib/Motor.py
View file

@ -8,7 +8,7 @@ from compLib.Spi import Spi, Register
MOTOR_COUNT = 4
MAX_MOTOR_SPEED = 65535
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_MULT = 1.0
@ -33,7 +33,7 @@ class Motor(object):
:param mode: Motor mode. See enum MotorMode for more info
:raises: IndexError
"""
if SPEED_LOCK:
return
@ -92,11 +92,9 @@ class Motor(object):
:param percent: Percentage of max speed. between -100 and 100
:raises: IndexError
"""
raw_power = 0
if percent > 0:
raw_power = Motor.__linearizePower(MOTOR_CURVE, percent)
elif percent < 0:
raw_power = -Motor.__linearizePower(MOTOR_CURVE, -percent)
raw_power = raw_power = Motor.__linearizePower(MOTOR_CURVE, abs(percent))
if percent < 0:
raw_power *= -1
MetricsLogging.put("Motor", float(percent), port)
Motor.power_raw(port, raw_power, False)
@ -135,7 +133,8 @@ class Motor(object):
"""
if (len(curve) != 21):
raise ValueError('The motor curve is invalid, check documentation for set_motor_curve()!')
global MOTOR_CURVE
MOTOR_CURVE = curve
@staticmethod
@ -152,6 +151,12 @@ class Motor(object):
"""
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
@staticmethod
@ -160,10 +165,10 @@ class Motor(object):
Interpolate a speed in the specified motor curve and return
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')
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]:
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:
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)