rotate servo with mqtt

camera1/rotate-servo.py

@@ -1,63 +1,89 @@
 #!/usr/bin/env python3
-from sys import exit, argv
+import paho.mqtt.client as mqtt
+import threading
 from time import sleep
 from os import path
 
-args = argv
-if len(args) != 3:
-	print("Args: <servo(x/y)> <offset (10,+10,-10)>")
-	exit(1)
 
-servo_arg = args[1]
-offset = int(args[2])
+# The callback for when the client receives a CONNACK response from the server.
+def on_connect(client, userdata, flags, rc):
+    print("Connected with result code "+str(rc))
 
-servo = -1 
+    # Subscribing in on_connect() means that if we lose the connection and
+    # reconnect then subscriptions will be renewed.
+    client.subscribe("video/servo1")
 
-if servo_arg == "x" or servo_arg == "X":
-	servo = 1
 
-if servo_arg == "y" or servo_arg == "Y":
-	servo = 0
+# The callback for when a PUBLISH message is received from the server.
+def on_message(client, userdata, msg):
+    print(msg.topic+" "+str(msg.payload))
+    if msg.topic == "video/servo1":
+        servo_arg = msg.payload.split()[0]
+        try:
+            offset = int(msg.payload.split()[1])
+        except:
+            return
+        servo = -1
 
-if servo == -1:
-	print("Servo invalido")
-	exit(2)
+        if servo_arg.lower() == "x":
+            servo = 1
 
+        if servo_arg.lower() == "y":
+            servo = 0
 
-filename="/tmp/servo_%s" % servo
-old_val = ""
+        if servo == -1:
+            print("Servo invalido")
+            return
+        update_servo(servo, offset)
 
-if path.isfile(filename):
-	valfile = open(filename, "r+")
-	old_val = valfile.read()
-else:
-	valfile = open(filename, "w")
 
-try:
-	old_val=int(old_val)
-except:
-	print("Can't convert. Defaulting")
-	old_val = 1500
+client = mqtt.Client()
+client.on_connect = on_connect
+client.on_message = on_message
 
-print(old_val)
-valfile.seek(0)
+client.connect("iot.eclipse.org", 1883, 60)
 
-new_val = old_val+offset
-new_val = min( max( new_val, 900 ), 1750 )
-print(new_val, file=valfile)
-print(new_val)
 
-steps = int(abs(new_val-old_val)/10)
-if steps == 0:
-	exit(0)
-delta = int(offset/steps)
+def update_servo(servo, offset):
+    filename = "/tmp/servo_%s" % servo
+    old_val = ""
 
-if delta > 0:
-	delta="+%d" % delta
+    if path.isfile(filename):
+        valfile = open(filename, "r+")
+        old_val = valfile.read()
+    else:
+        valfile = open(filename, "w")
 
-servoblaster = open("/dev/servoblaster","w")
-for i in range(0,steps):
-	#print("%d=%sus" % ( servo, delta ))
-	print("%d=%sus" % ( servo, delta ), file=servoblaster)
-	servoblaster.flush()
-	sleep(0.12)
+    try:
+        old_val = int(old_val)
+    except:
+        print("Can't convert. Defaulting")
+        old_val = 1500
+
+    print(old_val)
+    valfile.seek(0)
+
+    new_val = old_val+offset
+    new_val = min(max(new_val, 900), 1750)
+    print(new_val, file=valfile)
+    print(new_val)
+
+    steps = int(abs(new_val-old_val)/10)
+    if steps == 0:
+        exit(0)
+    delta = int(offset/steps)
+
+    if delta > 0:
+        delta = "+%d" % delta
+
+    servoblaster = open("/dev/servoblaster", "w")
+    for _ in range(0, steps):
+        # print("%d=%sus" % ( servo, delta ))
+        print("%d=%sus" % (servo, delta), file=servoblaster)
+        servoblaster.flush()
+        sleep(0.12)
+
+
+t = threading.Thread(target=client.loop_forever)
+t.start()
+t.join()