From 7448c9aeb7e8c6882fc6d0b08e19477e4aaee026 Mon Sep 17 00:00:00 2001
From: Jonas Hahn <jonashahn1@gmx.net>
Date: Tue, 23 Sep 2025 22:58:21 +0200
Subject: [PATCH] Fix jittering and cleanup

---
 links.txt |  1 +
 motor.py  | 90 +++++++++++++++++++++++++++++--------------------------
 print.py  | 13 ++++----
 3 files changed, 54 insertions(+), 50 deletions(-)

diff --git a/links.txt b/links.txt
index a73b29e..a5e671e 100644
--- a/links.txt
+++ b/links.txt
@@ -1,2 +1,3 @@
 https://pinout.xyz/ Full pinout for the rpi3
 https://components101.com/sites/default/files/component_datasheet/SG90%20Servo%20Motor%20Datasheet.pdf Datasheet for the servomotor used
+https://ben.akrin.com/raspberry-pi-servo-jitter/ Blog post how to fix jittering
diff --git a/motor.py b/motor.py
index 28131e1..9978c20 100644
--- a/motor.py
+++ b/motor.py
@@ -1,73 +1,79 @@
+import pigpio
 import RPi.GPIO as GPIO
 import time
 import os
 
-# Use the fat numberings
-GPIO.setmode(GPIO.BCM)
-
 # Constants
 SERVO1_PIN = 18
 SERVO2_PIN = 19
-BUTTON1_FWD = 5   # Motor1
+
+BUTTON1_FWD = 5
 BUTTON1_BWD = 6
-BUTTON2_FWD = 17  # Motor2
+BUTTON2_FWD = 17
 BUTTON2_BWD = 27
-SHUTDOWN_BTN = 26 # Shutdown rpi
+SHUTDOWN_BTN = 26
 
-# Setup pins
-for button in [BUTTON1_FWD, BUTTON1_BWD, BUTTON2_FWD, BUTTON2_BWD, SHUTDOWN_BTN]:
-    GPIO.setup(button, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
-GPIO.setup(SERVO1_PIN, GPIO.OUT)
-GPIO.setup(SERVO2_PIN, GPIO.OUT)
-
-servo1 = GPIO.PWM(SERVO1_PIN, 50)
-servo2 = GPIO.PWM(SERVO2_PIN, 50)
-servo1.start(0)
-servo2.start(0)
-
-# Adjust this for maximum range
-def pulse_to_duty(pulse_us):
-    return (pulse_us / 20000.0) * 100.0
-
-# Pulse limits
-MIN_PULSE = 1000
+MIN_PULSE = 1000 # In ms
 MAX_PULSE = 2000
 INIT_PULSE = 1500
-STEP = 10 # Adjust smoothness
-SLEEP = 0.005
+STEP = 10
+LOOP_DELAY = 0.01 # In seconds
+
+# 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)
+
+pi = pigpio.pi()
+if not pi.connected:
+    print("Cannot connect to pigpio daemon!")
+    exit()
 
-# Initialize servos
 pulse1 = INIT_PULSE
 pulse2 = INIT_PULSE
-servo1.ChangeDutyCycle(pulse_to_duty(pulse1))
-servo2.ChangeDutyCycle(pulse_to_duty(pulse2))
+pi.set_servo_pulsewidth(SERVO1_PIN, pulse1)
+pi.set_servo_pulsewidth(SERVO2_PIN, pulse2)
 
-# Main loop
+# 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
+
+# Main
 try:
     while True:
+        # Shutdown
         if GPIO.input(SHUTDOWN_BTN) == GPIO.HIGH:
             os.system("sudo shutdown now")
 
         # Motor 1
-        if GPIO.input(BUTTON1_FWD) == GPIO.HIGH:
-            pulse1 = min(MAX_PULSE, pulse1 + STEP)
-        elif GPIO.input(BUTTON1_BWD) == GPIO.HIGH:
-            pulse1 = max(MIN_PULSE, pulse1 - STEP)
-        servo1.ChangeDutyCycle(pulse_to_duty(pulse1))
+        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
-        if GPIO.input(BUTTON2_FWD) == GPIO.HIGH:
-            pulse2 = min(MAX_PULSE, pulse2 + STEP)
-        elif GPIO.input(BUTTON2_BWD) == GPIO.HIGH:
-            pulse2 = max(MIN_PULSE, pulse2 - STEP)
-        servo2.ChangeDutyCycle(pulse_to_duty(pulse2))
+        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(SLEEP)
+        time.sleep(LOOP_DELAY)
 
 except KeyboardInterrupt:
     pass
 
 finally:
-    servo1.stop()
-    servo2.stop()
+    pi.set_servo_pulsewidth(SERVO1_PIN, 0)
+    pi.set_servo_pulsewidth(SERVO2_PIN, 0)
+    pi.stop()
     GPIO.cleanup()
diff --git a/print.py b/print.py
index 0c1a38c..82f3924 100644
--- a/print.py
+++ b/print.py
@@ -1,11 +1,9 @@
 import RPi.GPIO as GPIO
 import time
 
-# ----------------------------
 # Setup
-# ----------------------------
-GPIO.setmode(GPIO.BCM)      # Use BCM numbering
-PIN = 26                    # The pin you want to monitor (BCM26)
+GPIO.setmode(GPIO.BCM)
+PIN = 26
 
 # Setup pin as input with pull-up
 GPIO.setup(PIN, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
@@ -14,15 +12,14 @@ print(f"Monitoring GPIO{PIN}. Press Ctrl+C to exit.")
 
 try:
     while True:
-        if GPIO.input(PIN):   # Pin is HIGH (1)
+        if GPIO.input(PIN):
             print(f"GPIO{PIN} is HIGH")
-        else:                 # Pin is LOW (0)
+        else:
             print(f"GPIO{PIN} is LOW")
-        time.sleep(0.1)       # Check every 100 ms
+        time.sleep(0.5)
 
 except KeyboardInterrupt:
     print("Exiting...")
 
 finally:
     GPIO.cleanup()
-