# puma / Shuffleboard.py

 ``` 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113``` ```#!/usr/bin/python # # Shuffleboard Puma robot server. # Depends on mdc-telnet.py being running # import socket import threading import time import os import re import sys import SocketServer from Puma import Puma from math import sqrt, pi, sin, cos r = None # Map x distance to height # 250 -45 # 300 -40 # 350 -35 # 400 -30 # 450 -25 class ShuffleboardSocket(SocketServer.StreamRequestHandler): def handle(self): print "New client" self.alive = True self.timeout = 1 #self.rx_thread = threading.Thread(target = self.rx_loop) #self.rx_thread.daemon = True #self.rx_thread.start() self.tx_thread = threading.Thread(target = self.tx_loop) self.tx_thread.daemon = True self.tx_thread.start() # why doesn't this work as a thread? self.rx_loop() def map_z(self, x): if (x >= 400 ): return -20 if (x >= 350 ): return -25 if (x >= 300 ): return -30 if (x >= 250 ): return -40 if (x >= 200 ): return -40 return 0 def moveto(self,x,angle): # Compute approach vector based on the angle # For the swinging tool on the left side of the board, # the position is facing (approach) due X, # sliding in Z and the normal to Y angle = -angle * pi / 180 a = [ cos(angle), sin(angle), 0] s = [ 0, 0, 1] n = [-sin(angle), cos(angle), 0] # todo: Compute the Z position based on the X distance z = self.map_z(x) r.move_ik(5000, [x,130,z], a, s, n) def rx_loop(self): while self.alive: line = self.rfile.readline().strip() if not line: break print "Read line '" + line + "'" if line == "home": r.home() continue if line == "hold": r.hold() continue g = re.match("^move ([0-9]+) (-?[0-9.]+)", line) if g is not None: self.moveto(int(g.group(1)), float(g.group(2))) continue g = re.match("^swing ([0-9]+)", line) if g is not None: r.swing(int(g.group(1))) continue self.wfile.write("?\r\n") self.alive = False print "Client exited" def tx_loop(self): while self.alive: volts = r.volts pos = [str(round(x,2)) for x in r.fk.p] speed = str(r.counts[5]) self.wfile.write(str(round(volts,1)) + ' ' + ' '.join(pos) + ' ' + speed + '\n') time.sleep(0.5) class ServerThread(SocketServer.ThreadingMixIn, SocketServer.TCPServer): # Force SOREUSEADDR allways daemon_threads = True allow_reuse_address = True if __name__ == "__main__": host = '0.0.0.0' port = 27189 server = ServerThread((host,port), ShuffleboardSocket) import PumaConfig r = Puma(("localhost", 31415), PumaConfig.joints) server.serve_forever() ```
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.