Add Odometry, Add Robot class, Change SPI Speed, Change Motor to old speeds
This commit is contained in:
root
parent
c70ac8fd03
commit
c800b30e31
6 changed files with 163 additions and 37 deletions
|
|
@ -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.
|
||||
|
|
@ -26,7 +45,6 @@ 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:
|
||||
|
|
@ -38,13 +56,43 @@ class Encoder(object):
|
|||
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)
|
||||
|
||||
|
|
@ -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,15 +73,14 @@ 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():
|
||||
|
|
@ -88,7 +88,7 @@ class MetricsLogging():
|
|||
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:
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
69
compLib/Odom.py
Normal file
69
compLib/Odom.py
Normal file
|
|
@ -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
|
||||
12
compLib/Robot.py
Normal file
12
compLib/Robot.py
Normal file
|
|
@ -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
|
||||
|
|
@ -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):
|
||||
|
|
|
|||
Reference in a new issue