38 lines
1.1 KiB
Python
38 lines
1.1 KiB
Python
|
#! /usr/bin/python3
|
||
|
|
import time
|
||
|
|
import socket
|
||
|
|
import math
|
||
|
|
|
||
|
|
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||
|
|
host ="192.168.1.101"
|
||
|
|
port =30002
|
||
|
|
s.connect((host,port))
|
||
|
|
|
||
|
|
|
||
|
|
c=0
|
||
|
|
ycenter=0.2
|
||
|
|
zcenter=0.5
|
||
|
|
|
||
|
|
scale=0.2
|
||
|
|
|
||
|
|
frequency=20
|
||
|
|
lookahead_time= 0.05 # range [0.03,0.2]
|
||
|
|
servo_t = 1.0/frequency*2 + lookahead_time
|
||
|
|
gain = 100 #range [100,2000]
|
||
|
|
|
||
|
|
last_send = 0
|
||
|
|
while True:
|
||
|
|
|
||
|
|
if time.time()>=last_send + (1.0/frequency):
|
||
|
|
last_send=time.time()
|
||
|
|
#text="movel(p[-0.5,"+str(ycenter+math.cos(c)*scale)+","+str(zcenter+math.sin(c)*scale)+",0.0,3.14159,0.0], a=1.2, v=0.1, t=0.2, r=0.02)\n"
|
||
|
|
#text="movep(p[-0.5,"+str(ycenter+math.cos(c)*scale)+","+str(zcenter+math.sin(c)*scale)+",0.0,3.14159,0.0], a=0.5, v=0.1, r=0.05)\n"
|
||
|
|
text="servoj(get_inverse_kin(p[-0.5,"+str(ycenter+math.cos(c)*scale)+","+str(zcenter+math.sin(c)*scale)+",0.0,3.14159,0.0]), t="+str(servo_t)+", lookahead_time="+str(lookahead_time)+", gain="+str(gain)+")\n"
|
||
|
|
s.send(text.encode())
|
||
|
|
print(text)
|
||
|
|
|
||
|
|
#time.sleep(1.0/frequency)
|
||
|
|
c+=0.1
|
||
|
|
c%=2*3.1415
|
||
|
|
|
||
|
|
s.close ()
|