Testing with continous movement

motor.py

@@ -15,32 +15,34 @@ BUTTON2_FWD = 17  # Motor2 forward
 BUTTON2_BWD = 27  # Motor2 backward
 SHUTDOWN_BTN = 26 # Shutdown button
 
-# Setup buttons
-buttons = [BUTTON1_FWD, BUTTON1_BWD, BUTTON2_FWD, BUTTON2_BWD, SHUTDOWN_BTN]
-for button in buttons:
+# Setup buttons (pulled down, pressed = HIGH)
+for button in [BUTTON1_FWD, BUTTON1_BWD, BUTTON2_FWD, BUTTON2_BWD, SHUTDOWN_BTN]:
     GPIO.setup(button, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
 
 # Setup servo pins
 GPIO.setup(SERVO1_PIN, GPIO.OUT)
 GPIO.setup(SERVO2_PIN, GPIO.OUT)
 
-# PWM setup for servo motors (50Hz typical for servos)
-servo1 = GPIO.PWM(SERVO1_PIN, 50)
+servo1 = GPIO.PWM(SERVO1_PIN, 50)  # 50Hz
 servo2 = GPIO.PWM(SERVO2_PIN, 50)
 servo1.start(0)
 servo2.start(0)
 
 # ----------------------------
-# Speed variable (0-100%)
+# Helpers
 # ----------------------------
-speed = 50  # Default speed percentage
+def pulse_to_duty(pulse_us):
+    return (pulse_us / 20000.0) * 100.0  # 20ms period → duty %
 
-# Function to convert speed to duty cycle for servo (1-2 ms pulse)
-def speed_to_duty(speed_percent):
-    # 1 ms = 5% duty, 2 ms = 10% duty at 50Hz
-    # Map speed 0-100% to 5-10% duty cycle
-    duty = 5 + (speed_percent / 100) * 5
-    return duty
+# Pulse limits
+STOP_PULSE = 1500
+MIN_PULSE  = 1000
+MAX_PULSE  = 1500
+
+# Initialize at 1200 µs
+INIT_PULSE = 1200
+servo1.ChangeDutyCycle(pulse_to_duty(INIT_PULSE))
+servo2.ChangeDutyCycle(pulse_to_duty(INIT_PULSE))
 
 # ----------------------------
 # Main loop
@@ -48,32 +50,23 @@ def speed_to_duty(speed_percent):
 try:
     while True:
         if GPIO.input(SHUTDOWN_BTN) == GPIO.HIGH:
-            print("Shutting down...")
             os.system("sudo shutdown now")
-        
+
         # Motor 1
         if GPIO.input(BUTTON1_FWD) == GPIO.HIGH:
-            servo1.ChangeDutyCycle(speed_to_duty(speed))
-            print("Pressed.")
+            servo1.ChangeDutyCycle(pulse_to_duty(MAX_PULSE))
         elif GPIO.input(BUTTON1_BWD) == GPIO.HIGH:
-            servo1.ChangeDutyCycle(speed_to_duty(100 - speed)) # backward
-            print("Pressed.")
+            servo1.ChangeDutyCycle(pulse_to_duty(MIN_PULSE))
         else:
-            servo1.ChangeDutyCycle(0)  # stop
-        
+            servo1.ChangeDutyCycle(pulse_to_duty(STOP_PULSE))
+
         # Motor 2
         if GPIO.input(BUTTON2_FWD) == GPIO.HIGH:
-            servo2.ChangeDutyCycle(speed_to_duty(speed))
-            print("Pressed.")
-
-
+            servo2.ChangeDutyCycle(pulse_to_duty(MAX_PULSE))
         elif GPIO.input(BUTTON2_BWD) == GPIO.HIGH:
-            servo2.ChangeDutyCycle(speed_to_duty(100 - speed)) # backward
-            print("Pressed.")
-
-
+            servo2.ChangeDutyCycle(pulse_to_duty(MIN_PULSE))
         else:
-            servo2.ChangeDutyCycle(0)  # stop
+            servo2.ChangeDutyCycle(pulse_to_duty(STOP_PULSE))
 
         time.sleep(0.05)
 
@@ -84,4 +77,3 @@ finally:
     servo1.stop()
     servo2.stop()
     GPIO.cleanup()
-