Add limit switch code

limits.py

@@ -0,0 +1,37 @@
+import time
+import sys
+import RPi.GPIO as GPIO
+
+
+PATH_OPEN = 0
+PATH_BLOCKED = 1
+LIMIT_LOWER_RIGHT = 19
+LIMIT_LOWER_LEFT = 20
+LIMIT_UPPER_RIGHT = 21
+
+
+GPIO.setmode(GPIO.BCM)
+
+GPIO.setup(LIMIT_LOWER_RIGHT, GPIO.IN, pull_up_down=GPIO.PUD_UP)
+GPIO.setup(LIMIT_LOWER_LEFT, GPIO.IN, pull_up_down=GPIO.PUD_UP)
+GPIO.setup(LIMIT_UPPER_RIGHT, GPIO.IN, pull_up_down=GPIO.PUD_UP)
+
+
+# True = optical path blocked, False = open
+
+def test():
+    while True:
+        try:
+            print(
+                'Lower Right (GPIO19):', GPIO.input(LIMIT_LOWER_RIGHT) == PATH_BLOCKED,
+                '| Lower Left (GPIO20):', GPIO.input(LIMIT_LOWER_LEFT) == PATH_BLOCKED,
+                '| Upper Right (GPIO21):', GPIO.input(LIMIT_UPPER_RIGHT) == PATH_BLOCKED,
+            )
+
+            time.sleep(1)
+        except KeyboardInterrupt as e:
+            sys.exit()
+
+
+if __name__ == '__main__':
+    test()

main.py

@@ -12,9 +12,11 @@ import aiomqtt
 import serial
 import glob
 import time
+import RPi.GPIO as GPIO
 
 import dyn4
 import relays
+import limits
 
 ENCODER_PPR = 65536
 
@@ -24,6 +26,13 @@ dmm1 = None
 dmm2 = None
 
 
+# 2025-10-24 facts:
+#    Motor1 is left
+#    Motor2 is right
+#    Positive RPM moves the crosshead down
+#    Negative RPM moves the crosshead up
+
+
 MOTOR_EN1 = 1
 MOTOR_EN2 = 2
 DISABLED = None
@@ -128,26 +137,48 @@ async def init_motor(path, name):
 
 
 async def read_motor(dmm, name):
+    global RPM_TARGET
+
     try:
         return dmm.read_AbsPos32()
     except BaseException as e:
         # any problems, kill both motors
+        RPM_TARGET = 0
         set_motors(0)
         disable_motors()
 
-        logging.error('Problem reading %s: %s - %s', name, e.__class__.__name__, e)
+        logging.error('Problem reading position of %s: %s - %s', name, e.__class__.__name__, e)
+        await send_mqtt('server/motor_status', name + ' disconnected')
+        dmm = False
+        return False
+
+async def check_status(dmm, name):
+    global RPM_TARGET
+
+    try:
+        status = dmm.read_Status()
+        current = dmm.read_TrqCurrent()
+        logging.info('%s status, alarm: %s, current: %s', name, status['alarm'] or 'None', current)
+    except BaseException as e:
+        # any problems, kill both motors
+        RPM_TARGET = 0
+        set_motors(0)
+        disable_motors()
+
+        logging.error('Problem reading status of %s: %s - %s', name, e.__class__.__name__, e)
         await send_mqtt('server/motor_status', name + ' disconnected')
         dmm = False
         return False
 
 
 async def check_sync(rev1, rev2):
-    global dmm1, dmm2
+    global dmm1, dmm2, RPM_TARGET
 
     difference = rev1 - rev2
 
     if abs(difference) > 2.0:
         # out of sync, kill both motors
+        RPM_TARGET = 0
         set_motors(0)
         disable_motors()
 
@@ -176,6 +207,12 @@ async def monitor_dyn4():
             dmm2 = await init_motor('/dev/ttyUSB1', 'Motor2')
             continue
 
+        if dmm1:
+            await check_status(dmm1, 'Motor1')
+
+        if dmm2:
+            await check_status(dmm2, 'Motor2')
+
         pos1 = await read_motor(dmm1, 'Motor1')
         if pos1 is False:
             dmm1 = False
@@ -202,25 +239,25 @@ async def monitor_dyn4():
             kp = 0.01
             difference = rev1 - rev2
 
-            if abs(difference) > 0.25:
-                if RPM_TARGET > 0:
-                    if rev1 > rev2:
-                        motor1_scaler = 1.0 - kp * abs(difference)
-                        motor1_scaler = min(motor1_scaler, 1.0)  # clamp to range 0.5 - 1.0
-                        motor1_scaler = max(motor1_scaler, 0.5)
-                    elif rev2 > rev1:
-                        motor2_scaler = 1.0 - kp * abs(difference)
-                        motor2_scaler = min(motor2_scaler, 1.0)  # clamp to range 0.5 - 1.0
-                        motor2_scaler = max(motor2_scaler, 0.5)
-                elif RPM_TARGET < 0:
-                    if rev1 < rev2:
-                        motor1_scaler = 1.0 - kp * abs(difference)
-                        motor1_scaler = min(motor1_scaler, 1.0)  # clamp to range 0.5 - 1.0
-                        motor1_scaler = max(motor1_scaler, 0.5)
-                    elif rev2 < rev1:
-                        motor2_scaler = 1.0 - kp * abs(difference)
-                        motor2_scaler = min(motor2_scaler, 1.0)  # clamp to range 0.5 - 1.0
-                        motor2_scaler = max(motor2_scaler, 0.5)
+            #if abs(difference) > 0.25:
+            #    if RPM_TARGET > 0:
+            #        if rev1 > rev2:
+            #            motor1_scaler = 1.0 - kp * abs(difference)
+            #            motor1_scaler = min(motor1_scaler, 1.0)  # clamp to range 0.5 - 1.0
+            #            motor1_scaler = max(motor1_scaler, 0.5)
+            #        elif rev2 > rev1:
+            #            motor2_scaler = 1.0 - kp * abs(difference)
+            #            motor2_scaler = min(motor2_scaler, 1.0)  # clamp to range 0.5 - 1.0
+            #            motor2_scaler = max(motor2_scaler, 0.5)
+            #    elif RPM_TARGET < 0:
+            #        if rev1 < rev2:
+            #            motor1_scaler = 1.0 - kp * abs(difference)
+            #            motor1_scaler = min(motor1_scaler, 1.0)  # clamp to range 0.5 - 1.0
+            #            motor1_scaler = max(motor1_scaler, 0.5)
+            #        elif rev2 < rev1:
+            #            motor2_scaler = 1.0 - kp * abs(difference)
+            #            motor2_scaler = min(motor2_scaler, 1.0)  # clamp to range 0.5 - 1.0
+            #            motor2_scaler = max(motor2_scaler, 0.5)
 
             motor1_rpm = int(RPM_TARGET * motor1_scaler)
             motor2_rpm = int(RPM_TARGET * motor2_scaler)
@@ -229,6 +266,27 @@ async def monitor_dyn4():
                 logging.info('Pos difference: %s, rev1: %s, rev2: %s, scaler1: %s, scaler2: %s, target: %s, rpm1: %s, rpm2: %s',
                     difference, rev1, rev2, motor1_scaler, motor2_scaler, RPM_TARGET, motor1_rpm, motor2_rpm)
 
+
+        # check limit switches
+        if ONE_MOTOR:
+            pass
+        else:
+            GOING_DOWNWARD = motor1_rpm > 0 or motor2_rpm > 0
+            GOING_UPWARD = motor1_rpm < 0 or motor2_rpm < 0
+
+            if GOING_DOWNWARD:
+                if GPIO.input(limits.LIMIT_LOWER_RIGHT) == limits.PATH_BLOCKED or GPIO.input(limits.LIMIT_LOWER_LEFT) == limits.PATH_BLOCKED:
+                    motor1_rpm = 0
+                    motor2_rpm = 0
+                    logging.info('Lower limit switch hit, preventing downward travel.')
+
+            if GOING_UPWARD:
+                if GPIO.input(limits.LIMIT_UPPER_RIGHT) == limits.PATH_BLOCKED:
+                    motor1_rpm = 0
+                    motor2_rpm = 0
+                    logging.info('Upper limit switch hit, preventing upward travel.')
+
+
         if ONE_MOTOR:
             logging.debug('Setting motor1: %s', motor1_rpm)
             dmm1.set_speed(motor1_rpm)