Some work

simulation.py

@@ -0,0 +1,107 @@
+import pigpio
+import RPi.GPIO as GPIO
+import time
+
+# Solar module for simulation of world
+import solar # Modeling of the world
+from motor import Motor # Small helper functions and constants
+
+# Constants
+SERVO1_PIN = 18
+SERVO2_PIN = 19
+
+INIT_PULSE = 0
+STEP = 10
+LOOP_DELAY = 0.01 # In seconds
+
+pi = pigpio.pi()
+if not pi.connected:
+    print("Cannot connect to pigpio daemon!")
+    exit()
+
+angle1 = init_motor(SERVO1_PIN)
+angle2 = init_motor(SERVO2_PIN)
+
+# Testing embedding the mirrors in the world
+world = solar.World(tilt_deg=15)  # The world is tilted 15 degrees around y-axis
+
+HEIGHT = 30
+
+source = solar.Source(world, pos=(0, 0, 30))
+target = solar.Target(world, pos=(20, 0, 30))
+
+# Create mirrors in a 9x9 grid
+for x in range(3):
+    for y in range(3):
+        mirror = solar.Mirror(world, cluster_x=x, cluster_y=y)
+        world.add_mirror(mirror)
+
+world.update_mirrors_from_source_target(source, target)
+
+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}°")
+
+def sm1(a): # Set motor 1
+    pi.set_servo_pulsewidth(SERVO1_PIN, angle_to_pulse(a))
+    time.sleep(2)
+
+
+angle = 150
+time.sleep(10)
+
+sm1(150)
+sm1(90)
+sm1(0)
+sm1(180)
+sm1(30)
+sm1(120)
+sm1(60)
+sm1(30)
+sm1(180)
+sm1(120)
+sm1(30)
+sm1(150)
+sm1(3)
+
+
+
+# Main
+try:
+    while True:
+        # Shutdown
+        if GPIO.input(SHUTDOWN_BTN) == GPIO.HIGH:
+            os.system("sudo shutdown now")
+
+        pulse1 = angle_to_pulse(angle)
+        time.sleep(3)
+        angle += 10
+
+        # Motor 1
+        target1 = pulse1
+        if GPIO.input(BUTTON1_FWD):
+            target1 = min(MAX_PULSE, pulse1 + STEP)
+        elif GPIO.input(BUTTON1_BWD):
+            target1 = max(MIN_PULSE, pulse1 - STEP)
+        pulse1 = move_servo(pulse1, target1)
+        pi.set_servo_pulsewidth(SERVO1_PIN, pulse1)
+
+        # Motor 2
+        target2 = pulse2
+        if GPIO.input(BUTTON2_FWD):
+            target2 = min(MAX_PULSE, pulse2 + STEP)
+        elif GPIO.input(BUTTON2_BWD):
+            target2 = max(MIN_PULSE, pulse2 - STEP)
+        pulse2 = move_servo(pulse2, target2)
+        pi.set_servo_pulsewidth(SERVO2_PIN, pulse2)
+
+        time.sleep(LOOP_DELAY)
+
+except KeyboardInterrupt:
+    pass
+
+finally:
+    pi.set_servo_pulsewidth(SERVO1_PIN, 0)
+    pi.set_servo_pulsewidth(SERVO2_PIN, 0)
+    pi.stop()
+    GPIO.cleanup()