diff --git a/README.md b/README.md index 36e03b1..45786f7 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,15 @@ # Solarmotor Mobilizing mirror cluster. + +## Information + +Use adafruit servokit to manage the servos through the external board. +Enable the I2C module for the rasberry pi. +Install the package and initialize with + +```python +from adafruit_servokit import ServoKit +kit = ServoKit(channels=16) +kit.servo[0].angle = 180 +``` diff --git a/clock.py b/clock.py new file mode 100644 index 0000000..875fccb --- /dev/null +++ b/clock.py @@ -0,0 +1,61 @@ +import pigpio +import RPi.GPIO as GPIO +import time +import os + +from motor import Motor # Models the motor + +# Constants +SERVO1_PIN = 18 +SERVO2_PIN = 19 + +BUTTON1_FWD = 5 +BUTTON1_BWD = 6 +BUTTON2_FWD = 17 +BUTTON2_BWD = 27 +SHUTDOWN_BTN = 26 + +STEP = 2 +LOOP_DELAY = 0.3 # In seconds + +# Local pi +pi = pigpio.pi() +if not pi: + os.exit() + +# Setup the controls +GPIO.setmode(GPIO.BCM) +for btn in [BUTTON1_FWD, BUTTON1_BWD, BUTTON2_FWD, BUTTON2_BWD, SHUTDOWN_BTN]: + GPIO.setup(btn, GPIO.IN, pull_up_down=GPIO.PUD_DOWN) + +# Setup motors +m1 = Motor(pi, SERVO1_PIN) +m2 = Motor(pi, SERVO2_PIN) + +# Main +try: + while True: + # Inputs shutdown + if GPIO.input(SHUTDOWN_BTN) == GPIO.HIGH: + os.system("sudo shutdown now") + + # Motors + if GPIO.input(BUTTON1_FWD): + m1.inc(STEP) + elif GPIO.input(BUTTON1_BWD): + m1.inc(-STEP) + if GPIO.input(BUTTON2_FWD): + m2.inc(STEP) + elif GPIO.input(BUTTON2_BWD): + m2.inc(-STEP) + + time.sleep(LOOP_DELAY) + +except KeyboardInterrupt: + pass + +finally: + del m1 + del m2 + pi.stop() + GPIO.cleanup() diff --git a/motor.py b/motor.py index abcbb88..e1301e9 100644 --- a/motor.py +++ b/motor.py @@ -1,100 +1,49 @@ -import pigpio -import RPi.GPIO as GPIO -import time -import os +"""Helpers for building moving mirrors.""" -# Solar module for simulation of world -import solar +class Motor: + """Model a type of servo motor.""" -# Constants -SERVO1_PIN = 18 -SERVO2_PIN = 19 + # Default vaules for every motor + MAX_PULSE = 2500 + MIN_PULSE = 500 + COVERAGE = 180 # Total degree of freedom in degrees + AREA = MAX_PULSE - MIN_PULSE + OFFSET = 2 # In degrees a constant to be added + SCALE = 1 # Scaling -BUTTON1_FWD = 5 -BUTTON1_BWD = 6 -BUTTON2_FWD = 17 -BUTTON2_BWD = 27 -SHUTDOWN_BTN = 26 + # Used for ids + count = 0 -MIN_PULSE = 500 # In ms -MAX_PULSE = 2500 -INIT_PULSE = 1000 -STEP = 10 -LOOP_DELAY = 0.01 # In seconds + def __init__(self, pi, pin, angle=0): + self.id = Motor.count; Motor.count += 1 + self.pi = pi # Local pi instance -# Setup -GPIO.setmode(GPIO.BCM) -for btn in [BUTTON1_FWD, BUTTON1_BWD, BUTTON2_FWD, BUTTON2_BWD, SHUTDOWN_BTN]: - GPIO.setup(btn, GPIO.IN, pull_up_down=GPIO.PUD_DOWN) + self.pin = pin + self.angle = angle + self.offset = Motor.OFFSET # Fine grained controls over every motor + self.scale = Motor.SCALE -pi = pigpio.pi() -if not pi.connected: - print("Cannot connect to pigpio daemon!") - exit() + # Initialization + self.set() -pulse1 = INIT_PULSE -pulse2 = INIT_PULSE -pi.set_servo_pulsewidth(SERVO1_PIN, pulse1) -pi.set_servo_pulsewidth(SERVO2_PIN, pulse2) + def angle_to_pulse(self, angle): + return min(max(Motor.MIN_PULSE, (Motor.MIN_PULSE + Motor.AREA * angle/Motor.COVERAGE + self.offset) * self.scale), Motor.MAX_PULSE) -# Helpers -def move_servo(current, target, step=STEP): - if current < target: - current = min(current + step, target) - elif current > target: - current = max(current - step, target) - return current + # Update the motor position on hardware + def set(self): + self.pi.set_servo_pulsewidth(self.pin, self.angle_to_pulse(self.angle)) -# Testing embedding the mirrors in the world -world = solar.World(tilt_deg=15) # The world is tilted 15 degrees around y-axis + def set_angle(self, angle): + self.angle = angle + self.set() -source = solar.Source(world, pos=(100, 100, 100)) -target = solar.Target(world, pos=(50, 50, 0)) + def __str__(self): + return f"Motor {self.id} is set at {self.angle} degrees." -# 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) + def __del__(self): + self.pi.set_servo_pulsewidth(self.pin, 0) -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}°") - -# Main -try: - while True: - # Shutdown - if GPIO.input(SHUTDOWN_BTN) == GPIO.HIGH: - os.system("sudo shutdown now") - - # 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() + def inc(self, inc): + self.angle += inc + self.angle = min(max(self.angle, 0), Motor.COVERAGE) # Clip + self.set() diff --git a/print.py b/print.py deleted file mode 100644 index b22f8df..0000000 --- a/print.py +++ /dev/null @@ -1,23 +0,0 @@ -import RPi.GPIO as GPIO -import time - -# Setup -GPIO.setmode(GPIO.BCM) -PIN = 26 -GPIO.setup(PIN, GPIO.IN, pull_up_down=GPIO.PUD_DOWN) - -print(f"Monitoring GPIO{PIN}. Press Ctrl+C to exit.") - -try: - while True: - if GPIO.input(PIN): - print(f"GPIO{PIN} is HIGH") - else: - print(f"GPIO{PIN} is LOW") - time.sleep(0.5) - -except KeyboardInterrupt: - print("Exiting...") - -finally: - GPIO.cleanup() diff --git a/simulation.py b/simulation.py new file mode 100644 index 0000000..d0e213f --- /dev/null +++ b/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() diff --git a/solar.py b/solar.py index d77166c..62f4864 100644 --- a/solar.py +++ b/solar.py @@ -25,17 +25,17 @@ class Source(MovingEntity): self.pos = pos class Mirror: - def __init__(self, world, cluster_x=0, cluster_y=0): + def __init__(self, world, pitch_pin, yaw_pin, cluster_x=0, cluster_y=0): self.world = world self.cluster_x = cluster_x self.cluster_y = cluster_y - self.angle_x = 0.0 - self.angle_y = 0.0 + + # Store the motors + self.yaw = motor.Motor(self.world.pi, yaw_pin) + self.pitch = motor.Motor(self.world.pi, pitch_pin) # Position in un-tilted coordinate system - self.pos = (cluster_x * self.world.grid_size, - cluster_y * self.world.grid_size, - 0.0) + self.pos = (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) @@ -74,14 +74,16 @@ class Mirror: mirror_normal = normalize(mirror_normal) # Update the angles based on the normals in rotated positions - self.angle_y = math.degrees(math.atan2(mirror_normal[0], mirror_normal[2])) - self.angle_x = math.degrees(math.atan2(mirror_normal[1], mirror_normal[2])) + self.yaw.set_angle(math.degrees(math.atan2(mirror_normal[0], mirror_normal[2]))) + self.pitch.set_angle(math.degrees(math.atan2(mirror_normal[1], mirror_normal[2]))) def get_angles(self): - return self.angle_x, self.angle_y + return self.yaw.angle, self.pitch.angle class World: - def __init__(self, tilt_deg=0.0): + def __init__(self, pi, tilt_deg=0.0): + self.pi = pi + self.grid_size = 10 # In cm self.tilt_deg = tilt_deg # Tilt of the grid system around y-axis self.mirrors = []