101 lines
No EOL
3.1 KiB
Python
101 lines
No EOL
3.1 KiB
Python
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():
|
|
"""DTO used for holding all odometry information. \n
|
|
Coordinate system: \n
|
|
X: + Forward; - Backwards \n
|
|
Y: + Right; - Left \n
|
|
Orientation: + Right; - Left
|
|
"""
|
|
|
|
def __init__(self, x, y, orientation):
|
|
self.x = x
|
|
self.y = y
|
|
self.orientation = orientation
|
|
|
|
def get_x(self) -> float:
|
|
"""Returns distance driven on x-axis in meters"""
|
|
return self.x
|
|
|
|
def get_y(self) -> float:
|
|
"""Returns distance driven on y-axis in meters"""
|
|
return self.y
|
|
|
|
def get_orientation(self) -> float:
|
|
"""Returns degrees turned in radians"""
|
|
return self.orientation
|
|
|
|
def __str__(self):
|
|
return f"X: {self.x} Y: {self.y} O: {self.orientation}"
|
|
|
|
|
|
class Odom(object):
|
|
"""Class used to track the movement of the robot in X, Y, Theta (Orientation)
|
|
"""
|
|
|
|
@staticmethod
|
|
def get_odom() -> Odometry:
|
|
"""
|
|
:return: Current orientation of the robot
|
|
"""
|
|
return Odometry(pos_x, pos_y, orientation)
|
|
|
|
@staticmethod
|
|
def clear() -> None:
|
|
"""
|
|
Clears the current odometry information and start from X, Y, Orientation set to 0
|
|
"""
|
|
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() -> None:
|
|
"""
|
|
Updates the current odometry information of the robot, Should be called in a loop with at least 100HZ.
|
|
Do not clear encoder positions between updates! Must be cleared, when clearing encoders!
|
|
"""
|
|
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 |