49 lines
1.5 KiB
Python
49 lines
1.5 KiB
Python
"""
|
|
When theta motor is at 0 then mirror is up and when at 180 then mirror is front.
|
|
Phi 0 equals right and phi 180 equals left by 45 degrees.
|
|
"""
|
|
|
|
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 # TODO: Fix this cyclic reference
|
|
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)
|
|
|
|
# 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
|