TurBoDRO.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import hal
import sys
import signal
import linuxcnc
import copy
import time

from serial import Serial

port = "/dev/ttyACM0"
bauds = 1000000

ser = Serial(port, bauds)

running = True


def signal_handler(signal, frame):
    global running
    
    running = False


def send_data(pos, vel):
    global ser
    
    data = "{0:+07.2f}{1:+07.2f}{2:+07.2f}{3:+07.2f}{4:+07.2f}{5:+07.2f}\n" \
        .format(pos[2], pos[1], pos[0], vel[2], vel[1], vel[0]) \
        .replace(".", "") \
        .replace("+", " ")

    ser.write(data) 


def main():
    
    signal.signal(signal.SIGTERM, signal_handler)
    signal.signal(signal.SIGINT, signal_handler)
    
    h = hal.component("turbodro")
    
    h.ready()
    
    previous_millis = 0
    interval = 50
    
    s = linuxcnc.stat() # create a connection to the status channel
    
    position = [0.0, 0.0, 0.0]
    
    actual = [0.0, 0.0, 0.0]
    
    offset = [0.0, 0.0, 0.0]
    
    velocity = [0.0, 0.0, 0.0]
    
    while running:
        
        current_millis = time.time() * 1000
        
        if current_millis - previous_millis >= interval:
            previous_millis = current_millis
        
            s.poll() # get current values
            
            actual = s.joint_actual_position
            offset = s.g5x_offset
            
            for i in range(3):
                position[i] = actual[i] - offset[i]
            
            velocity = [ s.joint[0]["velocity"],
                         s.joint[1]["velocity"],
                         s.joint[2]["velocity"] ]
            
            send_data(position, velocity)
    
    ser.close()


if __name__ == "__main__":
    main()