snowmix/camera1/rotate-servo.py

#!/usr/bin/env python3
import paho.mqtt.client as mqtt
import threading
import json
from time import sleep
from os import path


# 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))

    # Subscribing in on_connect() means that if we lose the connection and
    # reconnect then subscriptions will be renewed.
    client.subscribe("video/servo1/#")


# 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/restart":
        restart_servos()
    if msg.topic == "video/servo1/rotate":
        try:
            payload = json.loads(msg.payload.decode("utf-8", errors="ignore"))
            servo_arg = payload.split()[0]
            offset = int(payload.split()[1])
        except Exception as e:
            print("Exception", e)
            return
        print(servo_arg, offset)
        servo = -1

        if servo_arg.lower() == "x":
            servo = 1

        if servo_arg.lower() == "y":
            servo = 0

        if servo == -1:
            print("Servo invalido")
            return
        update_servo(servo, offset)


client = mqtt.Client()
client.on_connect = on_connect
client.on_message = on_message

client.connect("192.168.2.123", 1883, 60)


def update_servo(servo, offset):
    print("rotating", servo, offset)
    filename = "/tmp/servo_%s" % servo
    old_val = ""

    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

    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)
    servoblaster.close()


def restart_servos():
    servoblaster = open("/dev/servoblaster", "w")
    minv = 900
    maxv = 1750

    servo = 0
    percentage = 66
    filename = "/tmp/servo_%d" % servo
    print("%d=%d%%" % (servo, percentage), file=servoblaster)
    print(int((maxv-minv) * (percentage/100))+minv, file=open(filename, "w"))

    servo = 1
    percentage = 61
    filename = "/tmp/servo_%d" % servo
    s = "%d=%d%%" % (servo, percentage)
    print(s)
    print(s, file=servoblaster)
    print(int((maxv-minv) * (percentage/100))+minv, file=open(filename, "w"))

    servoblaster.flush()
    servoblaster.close()


t = threading.Thread(target=client.loop_forever)
t.start()
t.join()