Refactor all and add some tests for the calculations
This commit is contained in:
@@ -3,9 +3,13 @@ from adafruit_servokit import ServoKit
|
||||
class Board:
|
||||
MIN = 500
|
||||
MAX = 2500
|
||||
COVER = 180
|
||||
|
||||
count = 0
|
||||
|
||||
def __init__(self, channels=16, frequency=50):
|
||||
self.channels = channels
|
||||
self.address = "" # For the future
|
||||
self.frequency = frequency
|
||||
self.kit = ServoKit(channels=channels, frequency=frequency)
|
||||
|
||||
|
||||
@@ -1,76 +0,0 @@
|
||||
from objects.generic import Target, Source
|
||||
import numpy as np
|
||||
|
||||
# Einheitsvektoren
|
||||
unit_x = np.array([1, 0, 0])
|
||||
unit_y = np.array([0, 1, 0])
|
||||
unit_z = np.array([0, 0, 1])
|
||||
|
||||
def get_axis(axis):
|
||||
"Axis are numbered from 1 to 3 from x to z."
|
||||
match axis:
|
||||
case 1:
|
||||
ax = unit_x
|
||||
case 2:
|
||||
ax = unit_y
|
||||
case 3:
|
||||
ax = unit_z
|
||||
case _:
|
||||
ax = unit_x
|
||||
return ax
|
||||
|
||||
def proj(vec, axis: int =1):
|
||||
"""Simple vector projection onto an axis."""
|
||||
ax = get_axis(axis)
|
||||
return np.dot(vec, ax) * ax
|
||||
|
||||
def rotate(v, angle=90, axis=1):
|
||||
"Rotate a vector with an angle around a axis with the right hand rule."
|
||||
angle = angle/180 * np.pi
|
||||
k = get_axis(axis)
|
||||
|
||||
return (
|
||||
v * np.cos(angle)
|
||||
+ np.cross(k, v) * np.sin(angle)
|
||||
+ k * np.dot(k, v) * (1 - np.cos(angle))
|
||||
)
|
||||
|
||||
def agl(a, b):
|
||||
"Get the angle between two vectors. This is always between 0 and 180 degree."
|
||||
return np.round(np.acos(np.dot(a, b)/(np.linalg.norm(a) * np.linalg.norm(b)))/(2 * np.pi) * 360)
|
||||
|
||||
def get_angles(source: Source, target: Target):
|
||||
"""Main function to get the phi and theta angles for a source and a target vector. Both vectors must lie on the front half sphere.
|
||||
Phi is from 0 to 180 where 0 means left when you look at the mirrors. The hardware is bounded between 45 and 135 degree. Thus the here provided angle needs to be subtracted by 45 and then doubled.
|
||||
Theta is from 0 to 90 where 0 means up."""
|
||||
source_planar = source - proj(source, 3)
|
||||
target_planar = target - proj(target, 3)
|
||||
|
||||
source_phi = agl(source_planar, unit_x)
|
||||
target_phi = agl(target_planar, unit_x)
|
||||
|
||||
source_theta = agl(rotate(source, 90 - source_phi, 3), unit_z)
|
||||
target_theta = agl(rotate(target, 90 - target_phi, 3), unit_z)
|
||||
print(target_theta)
|
||||
|
||||
phi = None
|
||||
theta = None
|
||||
|
||||
theta_diff = None
|
||||
phi_diff = agl(source_planar, target_planar)
|
||||
if source_phi < target_phi:
|
||||
rota = rotate(source_planar, phi_diff, 3)
|
||||
theta_diff = agl(rota, target)
|
||||
phi = source_phi + phi_diff/2
|
||||
else:
|
||||
rota = rotate(target_planar, phi_diff, 3)
|
||||
theta_diff = agl(rota, source)
|
||||
phi = target_phi + phi_diff/2
|
||||
|
||||
if source_theta < target_theta:
|
||||
theta = target_theta + theta_diff/2
|
||||
else:
|
||||
theta = source_theta + theta_diff/2
|
||||
|
||||
print(phi, theta)
|
||||
return (phi, theta)
|
||||
42
objects/mirror.py
Normal file
42
objects/mirror.py
Normal file
@@ -0,0 +1,42 @@
|
||||
import numpy as np
|
||||
|
||||
from objects.generic import Source, Target
|
||||
from objects.motor import Motor
|
||||
|
||||
from calculator import get_angles
|
||||
|
||||
class Mirror:
|
||||
def __init__(self, world, cluster_x=0, cluster_y=0):
|
||||
self.world = world
|
||||
self.cluster_x = cluster_x
|
||||
self.cluster_y = cluster_y
|
||||
|
||||
# Store the motors
|
||||
# Need to get first the theta because
|
||||
# of the ordeing of the cables on the board
|
||||
self.motor_theta: Motor = Motor(self.world.board)
|
||||
self.motor_phi: Motor = Motor(self.world.board)
|
||||
|
||||
# Position in un-tilted coordinate system
|
||||
self.pos = np.array(
|
||||
[cluster_x * self.world.grid_size, cluster_y * self.world.grid_size, 0.0]
|
||||
)
|
||||
|
||||
def get_pos_rotated(self):
|
||||
return self.world.rotate_point_y(self.pos)
|
||||
|
||||
def set_angle_from_source_target(self, source: Source, target: Target):
|
||||
"Set the angles of a mirror from global source and target vectors."
|
||||
|
||||
rot_pos = self.get_pos_rotated()
|
||||
rel_source = source.pos - rot_pos
|
||||
rel_target = target.pos - rot_pos
|
||||
|
||||
phi, theta = get_angles(rel_source, rel_target) # ty:ignore[unresolved-reference]
|
||||
|
||||
# Update the angles based on the normals in rotated positions
|
||||
self.motor_phi.set_angle(phi)
|
||||
self.motor_theta.set_angle(theta)
|
||||
|
||||
def get_angles(self):
|
||||
return self.motor_phi.angle, self.motor_theta.angle
|
||||
@@ -5,24 +5,17 @@ from objects.board import Board
|
||||
class Motor:
|
||||
"""Model a type of servo motor."""
|
||||
|
||||
# Default vaules for every motor
|
||||
MAX_PULSE = 2500
|
||||
MIN_PULSE = 500
|
||||
COVERAGE = 180 # Total degree of freedom in degrees
|
||||
OFFSET = 0 # In degrees a constant to be added
|
||||
SCALE = 1 # Scaling
|
||||
|
||||
# Used for ids
|
||||
count = 0
|
||||
|
||||
def __init__(self, board: Board, angle=0):
|
||||
self.board: Board = board
|
||||
self.id: int = Motor.count
|
||||
Motor.count += 1
|
||||
self.id: int = Board.count
|
||||
Board.count += 1
|
||||
|
||||
self.angle = angle
|
||||
self.offset = Motor.OFFSET # Fine grained controls over every motor
|
||||
self.coverage = Motor.COVERAGE
|
||||
self.coverage = Board.COVER
|
||||
self.scale = Motor.SCALE
|
||||
|
||||
# Initialization
|
||||
@@ -41,5 +34,5 @@ class Motor:
|
||||
|
||||
def inc(self, inc):
|
||||
self.angle += inc
|
||||
self.angle = min(max(self.angle, 0), Motor.COVERAGE) # Clip
|
||||
self.angle = min(max(self.angle, 0), Board.COVER) # Clip
|
||||
self.set()
|
||||
|
||||
@@ -1,70 +0,0 @@
|
||||
"""Alle gemessenen Koordinaten der Quelle und der Sonne haben den Ursprung in der linken unteren Ecke des Clusters in einem rechtshaendigen flachen System."""
|
||||
|
||||
from objects.generic import Source, Target
|
||||
|
||||
import math
|
||||
import objects.motor as motor
|
||||
import numpy as np
|
||||
from objects.calculator import get_angles
|
||||
|
||||
|
||||
class Mirror:
|
||||
def __init__(self, world, cluster_x=0, cluster_y=0):
|
||||
self.world: World = world
|
||||
self.cluster_x = cluster_x
|
||||
self.cluster_y = cluster_y
|
||||
|
||||
# Store the motors
|
||||
self.theta = motor.Motor(self.world.board)
|
||||
self.phi = motor.Motor(self.world.board)
|
||||
|
||||
# Position in un-tilted coordinate system
|
||||
self.pos = np.array(
|
||||
[cluster_x * self.world.grid_size, cluster_y * self.world.grid_size, 0.0]
|
||||
)
|
||||
|
||||
def get_pos_rotated(self):
|
||||
return self.world.rotate_point_y(self.pos)
|
||||
|
||||
def set_angle_from_source_target(self, source: Source, target: Target):
|
||||
"Set the angles of a mirror from global source and target vectors."
|
||||
|
||||
rot_pos = self.get_pos_rotated()
|
||||
rel_source = source.pos - rot_pos
|
||||
rel_target = target.pos - rot_pos
|
||||
|
||||
phi, theta = get_angles(rel_source, rel_target)
|
||||
|
||||
# Update the angles based on the normals in rotated positions
|
||||
self.phi.set_angle(phi)
|
||||
self.theta.set_angle(theta)
|
||||
|
||||
def get_angles(self):
|
||||
return self.phi.angle, self.theta.angle
|
||||
|
||||
|
||||
class World:
|
||||
def __init__(self, board, tilt_deg=0.0):
|
||||
self.board = board
|
||||
|
||||
self.grid_size = 10 # In cm
|
||||
self.tilt_deg = tilt_deg # Tilt of the grid system around y-axis
|
||||
self.mirrors: list[Mirror] = []
|
||||
|
||||
def add_mirror(self, mirror):
|
||||
self.mirrors.append(mirror)
|
||||
|
||||
def update_mirrors_from_source_target(self, source: Source, target: Target):
|
||||
for mirror in self.mirrors:
|
||||
mirror.set_angle_from_source_target(source, target)
|
||||
|
||||
def rotate_point_y(self, point):
|
||||
"""Rotate a point around the y-axis by the world's tilt angle."""
|
||||
x, y, z = point
|
||||
theta = math.radians(self.tilt_deg)
|
||||
cos_t = math.cos(theta)
|
||||
sin_t = math.sin(theta)
|
||||
x_rot = x * cos_t + z * sin_t
|
||||
y_rot = y
|
||||
z_rot = -x * sin_t + z * cos_t
|
||||
return np.array([x_rot, y_rot, z_rot])
|
||||
55
objects/world.py
Normal file
55
objects/world.py
Normal file
@@ -0,0 +1,55 @@
|
||||
"""
|
||||
Alle gemessenen Koordinaten der Quelle und der Sonne haben den Ursprung in der rechten
|
||||
oberen Ecke des Clusters in einem rechtshaendigen flachen System.
|
||||
|
||||
Achsen in der Welt mit der z-Achse nach oben.
|
||||
Alles in cm gemessen.
|
||||
Der phi Winkel wird zur x-Achse gemessen.
|
||||
Der thetha Winkel wird zur z-Achse gemessen.
|
||||
|
||||
So sind y und z Koordinaten immer positiv.
|
||||
|
||||
|
||||
|
||||
x (2,0) (1,0) (0,0)
|
||||
<-----S----S----S O:z
|
||||
|
|
||||
S S S (0,1)
|
||||
|
|
||||
v y
|
||||
|
||||
"""
|
||||
|
||||
from objects.generic import Source, Target
|
||||
from objects.mirror import Mirror
|
||||
|
||||
import numpy as np
|
||||
|
||||
import math
|
||||
|
||||
|
||||
class World:
|
||||
def __init__(self, board, tilt_deg=0.0):
|
||||
self.board = board
|
||||
|
||||
self.grid_size = 10 # In cm
|
||||
self.tilt_deg = tilt_deg # Tilt of the grid system around y-axis
|
||||
self.mirrors: list[Mirror] = []
|
||||
|
||||
def add_mirror(self, mirror):
|
||||
self.mirrors.append(mirror)
|
||||
|
||||
def update_mirrors_from_source_target(self, source: Source, target: Target):
|
||||
for mirror in self.mirrors:
|
||||
mirror.set_angle_from_source_target(source, target)
|
||||
|
||||
def rotate_point_y(self, point):
|
||||
"""Rotate a point around the y-axis by the world's tilt angle."""
|
||||
x, y, z = point
|
||||
theta = math.radians(self.tilt_deg)
|
||||
cos_t = np.cos(theta)
|
||||
sin_t = np.sin(theta)
|
||||
x_rot = x * cos_t + z * sin_t
|
||||
y_rot = y
|
||||
z_rot = -x * sin_t + z * cos_t
|
||||
return np.array([x_rot, y_rot, z_rot])
|
||||
Reference in New Issue
Block a user