From cc3371b73bb6e914292beb4222f59035b9333b8d Mon Sep 17 00:00:00 2001 From: Jonas Date: Thu, 15 Jan 2026 15:25:06 +0100 Subject: [PATCH] Some refactor and tried to figure out the angles right --- simple-angle.py => objects/calculator.py | 41 +------ objects/generic.py | 25 +++++ objects/motor.py | 6 -- objects/solar.py | 130 +++-------------------- shell.nix | 13 --- simulation.py | 24 +++-- 6 files changed, 59 insertions(+), 180 deletions(-) rename simple-angle.py => objects/calculator.py (68%) create mode 100644 objects/generic.py delete mode 100644 shell.nix diff --git a/simple-angle.py b/objects/calculator.py similarity index 68% rename from simple-angle.py rename to objects/calculator.py index c46a11f..d53af76 100644 --- a/simple-angle.py +++ b/objects/calculator.py @@ -1,3 +1,4 @@ +from objects.generic import Target, Source import numpy as np # Einheitsvektoren @@ -23,12 +24,6 @@ def proj(vec, axis: int =1): ax = get_axis(axis) return np.dot(vec, ax) * ax -def abs_custom(vec): - l = 0 - for i in range(3): - l += vec[i] ** 2 - return np.sqrt(l) - 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 @@ -42,13 +37,9 @@ def rotate(v, angle=90, axis=1): 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)/(abs_custom(a) * abs_custom(b)))/(2 * np.pi) * 360) + return np.round(np.acos(np.dot(a, b)/(np.linalg.norm(a) * np.linalg.norm(b)))/(2 * np.pi) * 360) -def normalize(vec): - l = abs_custom(vec) - return vec/l - -def get_angles(source, target): +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.""" @@ -60,6 +51,7 @@ def get_angles(source, target): 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 @@ -80,28 +72,5 @@ def get_angles(source, target): else: theta = source_theta + theta_diff/2 + print(phi, theta) return (phi, theta) - -GRID_SIZE = 10 - -# Aufbau der Koordinaten -# Das Zentrum des Spiegels hinten rechts bildet den Ursprung -# Dann geht die x-Achse nach links und die y-Achse nach vorne - -# X, Y, Z -source_orig = np.array([0, 20, 0]) -target_orig = np.array([0, 20, 0]) - -# Strategie des Programms -# 1. Iteration ueber jeden Spiegel -# 2. Berechnung des Quellvektors und des Targetvektors fuer die Position des Spiegels -# 3. Berechne - -for x in range(4): - for y in range(2): - x_size = x * GRID_SIZE - y_size = y * GRID_SIZE - - phi, theta = get_angles(source_orig - unit_x * x_size - unit_y * y_size, target_orig - unit_x * x_size - unit_y * y_size) - - print(f"For grid ({x}, {y}), phi = {phi} and theta = {theta}.") diff --git a/objects/generic.py b/objects/generic.py new file mode 100644 index 0000000..5991c39 --- /dev/null +++ b/objects/generic.py @@ -0,0 +1,25 @@ +import numpy as np + +class MovingEntity: + """Embedded entity in the world with a position.""" + + def __init__(self, world): + self.world = world + self.pos = np.array((0.0, 0.0, 0.0)) # (x, y, z) in local untilted coordinates + + def get_pos_rotated(self): + """Return position rotated by world's tilt around y-axis.""" + return self.world.rotate_point_y(self.pos) + + def move(self, dx=0, dy=0, dz=0): + self.pos = (self.pos[0] + dx, self.pos[1] + dy, self.pos[2] + dz) + +class Target(MovingEntity): + def __init__(self, world, pos=(0.0, 0.0, 0.0)): + super().__init__(world) + self.pos = np.array(pos) # Store everything in numpy + +class Source(MovingEntity): + def __init__(self, world, pos=(10.0, 10.0, 10.0)): + super().__init__(world) + self.pos = np.array(pos) diff --git a/objects/motor.py b/objects/motor.py index 00db284..faf1fdf 100644 --- a/objects/motor.py +++ b/objects/motor.py @@ -1,7 +1,6 @@ """Helpers for building moving mirrors.""" from objects.board import Board -import time class Motor: """Model a type of servo motor.""" @@ -33,11 +32,6 @@ class Motor: def set(self): self.board.kit.servo[self.id].angle = self.angle * self.scale + self.offset - def safe_set_angle(angle=0, sleep=0.01, offset=1): - self.board.kit.servo[NUM].angle = angle + offset - time.sleep(sleep) - kit.servo[NUM].angle = angle - def set_angle(self, angle): self.angle = min(self.coverage, max(0, angle)) # Double check bad self.set() diff --git a/objects/solar.py b/objects/solar.py index 671b406..b79a240 100644 --- a/objects/solar.py +++ b/objects/solar.py @@ -1,128 +1,27 @@ -"""Alle gemessenen Koordinaten der Quelle und der Sonne haben den Ursprung in der linken unteren Ecke des Clusters in einem rechtshaendigen flachen System. -""" +"""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 -# 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 abs_custom(vec): - l = 0 - for i in range(3): - l += vec[i] ** 2 - return np.sqrt(l) - -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)/(abs_custom(a) * abs_custom(b)))/(2 * np.pi) * 360) - -def normalize(vec): - l = abs_custom(vec) - return vec/l - -def get_angles(source, 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) - - 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 - - return (phi, theta) - -class MovingEntity: - """Embedded entity in the world with a position.""" - - def __init__(self, world): - self.world = world - self.pos = (0.0, 0.0, 0.0) # (x, y, z) in local untilted coordinates - - def get_pos_rotated(self): - """Return position rotated by world's tilt around y-axis.""" - return self.world.rotate_point_y(self.pos) - - def move(self, dx=0, dy=0, dz=0): - self.pos = (self.pos[0] + dx, self.pos[1] + dy, self.pos[2] + dz) - -class Target(MovingEntity): - def __init__(self, world, pos=(0.0, 0.0, 0.0)): - super().__init__(world) - self.pos = pos - -class Source(MovingEntity): - def __init__(self, world, pos=(10.0, 10.0, 10.0)): - super().__init__(world) - self.pos = pos class Mirror: def __init__(self, world, cluster_x=0, cluster_y=0): - self.world = world + self.world: World = world self.cluster_x = cluster_x self.cluster_y = cluster_y # Store the motors - self.phi = motor.Motor(self.world.board) self.theta = motor.Motor(self.world.board) + self.phi = motor.Motor(self.world.board) # Position in un-tilted coordinate system - self.pos = (cluster_x * self.world.grid_size, cluster_y * self.world.grid_size, 0.0) + 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) @@ -130,9 +29,9 @@ class Mirror: 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 = np.array([self.get_pos_rotated()]) - rel_source = np.array([source.pos]) - rot_pos - rel_target = np.array([target.pos]) - rot_pos + 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) @@ -143,13 +42,14 @@ class Mirror: 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 = [] + self.mirrors: list[Mirror] = [] def add_mirror(self, mirror): self.mirrors.append(mirror) @@ -167,4 +67,4 @@ class World: x_rot = x * cos_t + z * sin_t y_rot = y z_rot = -x * sin_t + z * cos_t - return (x_rot, y_rot, z_rot) + return np.array([x_rot, y_rot, z_rot]) diff --git a/shell.nix b/shell.nix deleted file mode 100644 index 835e429..0000000 --- a/shell.nix +++ /dev/null @@ -1,13 +0,0 @@ -{ pkgs ? import {} }: - -# Simple python shell for all packages - -pkgs.mkShell { - buildInputs = with pkgs; [ - (pkgs.python313.withPackages (ps: with ps; [ - matplotlib - numpy - ty - ])) - ]; -} diff --git a/simulation.py b/simulation.py index 6e2ba3b..4c4b0f5 100644 --- a/simulation.py +++ b/simulation.py @@ -1,10 +1,8 @@ import time -import math # Solar module for simulation of world import objects.solar as solar # Modeling of the world -from objects.motor import Motor # Small helper functions and constants from objects.board import Board STEP = 10 @@ -20,8 +18,8 @@ source = solar.Source(world, pos=(0, 50, 0)) target = solar.Target(world, pos=(0, 50, 0)) # Create mirrors in a 3x2 grid -for x in range(4): - for y in range(2): +for x in range(2): + for y in range(1): mirror = solar.Mirror(world, cluster_x=x, cluster_y=y) world.add_mirror(mirror) @@ -29,21 +27,27 @@ world.update_mirrors_from_source_target(source, target) def print_status(): for i, mirror in enumerate(world.mirrors): - pitch, yaw = mirror.get_angles() - print(f"Mirror {i} ({mirror.cluster_x}, {mirror.cluster_y}) angles -> pitch: {pitch:.2f}°, yaw: {yaw:.2f}°") + phi, theta = mirror.get_angles() + print(f"Mirror {i} ({mirror.cluster_x}, {mirror.cluster_y}) angles -> phi: {phi:.2f}°, theta: {theta:.2f}°") -print_status() a = 1 t = time.time() +world.mirrors[0].phi.set_angle(180) +world.mirrors[0].theta.set_angle(180) +world.mirrors[1].phi.set_angle(0) +world.mirrors[1].theta.set_angle(0) + +print_status() + # Main try: while True: - source.move(0, 0, 0.1) + #source.move(0, 0, 0.5) #source.move(10 * math.sin(a * t), 10 * math.cos(a * t)) - print(source.pos) - print(target.pos) + #print(source.pos) + #print(target.pos) world.update_mirrors_from_source_target(source, target) print_status()