diff --git a/compLib/Encoder.py b/compLib/Encoder.py index a2490d2..8d9d4d3 100644 --- a/compLib/Encoder.py +++ b/compLib/Encoder.py @@ -2,19 +2,38 @@ 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 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. @@ -25,8 +44,7 @@ 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: @@ -37,14 +55,44 @@ class Encoder(object): 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_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 @@ -56,14 +104,7 @@ class Encoder(object): 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 + return Encoder.handle_wrap(Encoder.read_raw(port), port) @staticmethod def clear(port: int): @@ -84,4 +125,3 @@ class Encoder(object): for i in range(1, MOTOR_COUNT + 1): encoder_start_values[i] = Encoder.read_raw(i) - \ No newline at end of file diff --git a/compLib/MetricsLogging.py b/compLib/MetricsLogging.py index 5fc4ee2..b783edb 100644 --- a/compLib/MetricsLogging.py +++ b/compLib/MetricsLogging.py @@ -2,12 +2,13 @@ from influxdb_client import InfluxDBClient, Point, WritePrecision import socket import uuid import os -import queue +import multiprocessing +import faster_fifo import datetime -import threading import requests +import time -CONCURRENCY = 2 +CONCURRENCY = 1 HOSTNAME = socket.gethostname() RUN_TRACE = str(uuid.uuid4()) @@ -28,7 +29,7 @@ else: influx_client = InfluxDBClient(url=INFLUX_HOST, token=TOKEN) write_api = influx_client.write_api() -point_queue = queue.Queue() +point_queue = faster_fifo.Queue() workers = [] class MetricsLogging(): @@ -72,23 +73,22 @@ class MetricsLogging(): @staticmethod def worker(): while True: - point = point_queue.get() - if point is None: # Kill signal - return - try: - write_api.write(BUCKET, ORG, point) - + points = point_queue.get_many() + write_api.write(BUCKET, ORG, points) + time.sleep(0.1) + except Exception as e: + pass finally: - point_queue.task_done() - + pass + @staticmethod def start_workers(): global EXTENSIVE_LOGGING if EXTENSIVE_LOGGING: if MetricsLogging.is_influx_reachable(): for i in range(CONCURRENCY): - worker = threading.Thread(target=MetricsLogging.worker, daemon=True) + worker = multiprocessing.Process(target=MetricsLogging.worker, daemon=True) worker.start() workers.append(worker) else: diff --git a/compLib/Motor.py b/compLib/Motor.py index 8d46f58..74c92ec 100644 --- a/compLib/Motor.py +++ b/compLib/Motor.py @@ -8,7 +8,8 @@ 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, 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] +NEW_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] +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] SPEED_LOCK = False SPEED_MULT = 1.0 diff --git a/compLib/Odom.py b/compLib/Odom.py new file mode 100644 index 0000000..7fe7c58 --- /dev/null +++ b/compLib/Odom.py @@ -0,0 +1,69 @@ +import time +import math +from compLib.Encoder import Encoder +from compLib.Robot import Robot + +last_run = time.time() +last_enc_left = 0 +last_enc_right = 0 + +pos_x = 0 +pos_y = 0 +orientation = 0 + + +class Odometry(): + def __init__(self, x, y, orientation): + self.x = x + self.y = y + self.orientation = orientation + + def __str__(self): + return f"X: {self.x} Y: {self.y} O: {self.orientation}" + +class Odom(object): + + @staticmethod + def get_odom(): + return Odometry(pos_x, pos_y, orientation) + + @staticmethod + def clear(): + global last_run, last_enc_left, last_enc_right, pos_x, pos_y, orientation + + last_run = 0 + last_enc_left = 0 + last_enc_right = 0 + pos_x = 0 + pos_y = 0 + orientation = 0 + + @staticmethod + def update(): + global last_run, last_enc_left, last_enc_right, pos_x, pos_y, orientation + + now = time.time() + if last_run + 0.01 < now: + last_run = now + + encoder_positions = Encoder.read_all() + current_left = encoder_positions[Robot.LEFT_PORT - 1] + current_right = -encoder_positions[Robot.RIGHT_PORT - 1] + distance_left = (current_left - last_enc_left) / Robot.TICKS_PER_METER + distance_right = (current_right - last_enc_right) / Robot.TICKS_PER_METER + last_enc_left = current_left + last_enc_right = current_right + + distance = (distance_left + distance_right) / 2.0 + theta = (distance_left - distance_right) / Robot.ARBOR_LENGTH_M + + if distance != 0: + distance_x = math.cos(theta) * distance + distance_y = -math.sin(theta) * distance + + pos_x = pos_x + (math.cos(orientation) * distance_x - + math.sin(orientation) * distance_y) + pos_y = pos_y + (math.sin(orientation) * distance_x + + math.cos(orientation) * distance_y) + if theta != 0: + orientation += theta \ No newline at end of file diff --git a/compLib/Robot.py b/compLib/Robot.py new file mode 100644 index 0000000..4a17691 --- /dev/null +++ b/compLib/Robot.py @@ -0,0 +1,12 @@ +import math + + +class Robot(object): + + WHEEL_CIRCUMFERENCE_MM = 71.0 * math.pi + TICKS_PER_TURN = 27.7 * 100.0 + ARBOR_LENGTH_MM = 139.0 + ARBOR_LENGTH_M = ARBOR_LENGTH_MM / 1000.0 + TICKS_PER_METER = 1000.0 / WHEEL_CIRCUMFERENCE_MM * TICKS_PER_TURN + LEFT_PORT = 4 + RIGHT_PORT = 1 diff --git a/compLib/Spi.py b/compLib/Spi.py index 3310c07..88cc1eb 100644 --- a/compLib/Spi.py +++ b/compLib/Spi.py @@ -8,7 +8,7 @@ from compLib.LogstashLogging import Logging SPI_BUS = 1 SPI_DEVICE = 2 -SPI_SPEED = 1000000 +SPI_SPEED = 4000000 SPI_BUFFER_SIZE = 32 SPI_HEALTH = True @@ -117,7 +117,7 @@ class Spi(object): write_reg = tx_buffer[1] spi.xfer(tx_buffer) - rx_buffer = spi.xfer([0] * 32) + rx_buffer = spi.xfer([0] * SPI_BUFFER_SIZE) if rx_buffer[1] != write_reg: Logging.get_logger().error(f"SPI error during read/write of register {write_reg}!") @@ -126,6 +126,10 @@ class Spi(object): @staticmethod def read(reg: int, length: int): + return int.from_bytes(Spi.read_array(reg, length), byteorder='big', signed=False) + + @staticmethod + def read_array(reg: int, length: int): if not type(reg) is int: reg = int(reg) @@ -136,7 +140,7 @@ class Spi(object): tx_buf[2] = length rx_buf = Spi.transfer(tx_buf) - return int.from_bytes(rx_buf[2:2 + length], byteorder='big', signed=False) + return rx_buf[2:2 + length] @staticmethod def write(reg: int, length: int, value: int):