You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

435 lines
10 KiB

import time
import threading
import DobotDllType as dType
import json
import socket, select
import re
from time import sleep
"""
Six actuators:
linear rail
base rotation
rear arm
forarm
gripperrotation
gripper
First five takes argument 1 or -1 (direction)
Example:
#initialisation
c = Candybot()
c.linear(1) # move linear in positive direction
c.gripp() # gripp candy
c.letgo() # let go of candy
c.stopcmp() # stoop compressor
c.panic() # stops everything
"""
class Candybot:
def __init__(self):
self.jointspeed = 100
self.lastjointspeed = 100
self.linearspeed = 100
self.lastleinearspeed = 100
CON_STR = {
dType.DobotConnect.DobotConnect_NoError: "DobotConnect_NoError",
dType.DobotConnect.DobotConnect_NotFound: "DobotConnect_NotFound",
dType.DobotConnect.DobotConnect_Occupied: "DobotConnect_Occupied"}
#Load Dll
self.api = dType.load()
#Connect Dobot
state = dType.ConnectDobot(self.api, "", 115200)[0]
print("Connect status:",CON_STR[state])
if (state == dType.DobotConnect.DobotConnect_NoError):
#Clean Command Queued
dType.SetQueuedCmdClear(self.api)
#Async Motion Params Setting
dType.SetHOMEParams(self.api, 250, 0, 50, 0, isQueued = 1)
dType.SetJOGJointParams(self.api, 2, 200, 2, 200, 2, 200, 2, 200, isQueued = 1)
dType.SetJOGCommonParams(self.api, 100, 100, isQueued = 1)
#linear rail
dType.SetDeviceWithL(self.api, 1)
dType.SetJOGLParams(self.api, 200, 200, isQueued=0)
#Async Home
dType.SetHOMECmd(self.api, temp = 0, isQueued = 1)
def disconnect(self):
#Disconnect Dobot
dType.DisconnectDobot(self.api)
def base(self, direction):
if direction == 1:
dType.SetJOGCmd(self.api,1,1)
elif direction == -1:
dType.SetJOGCmd(self.api,1,2)
else :
return
threading.Timer(0.1,self.stop).start()
#dType.SetWAITCmd(self.api, 1, isQueued=0)
#dType.SetJOGCmd(self.api, 1, 0)
def rearArm(self, direction):
if direction == 1:
dType.SetJOGCmd(self.api,1,3)
elif direction == -1:
dType.SetJOGCmd(self.api,1,4)
else :
return
#dType.SetWAITCmd(self.api, 1, isQueued=0)
#dType.SetJOGCmd(self.api, 1, 0)
threading.Timer(0.1,self.stop).start()
def forearm(self, direction):
if direction == 1:
dType.SetJOGCmd(self.api,1,5)
elif direction == -1:
dType.SetJOGCmd(self.api,1,6)
else :
return
#dType.SetWAITCmd(self.api, 1, isQueued=0)
#dType.SetJOGCmd(self.api, 1, 0)
threading.Timer(0.1,self.stop).start()
def gripperRotation(self, direction):
if direction == 1:
dType.SetJOGCmd(self.api,1,7)
elif direction == -1:
dType.SetJOGCmd(self.api,1,8)
else :
return
#dType.SetWAITCmd(self.api, 1, isQueued=0)
#dType.SetJOGCmd(self.api, 1, 0)
threading.Timer(0.1,self.stop).start()
def linear(self, direction):
if direction == 1:
dType.SetJOGCmd(self.api,1,9)
elif direction == -1:
dType.SetJOGCmd(self.api,1,10)
else :
return
#dType.SetWAITCmd(self.api, 1, isQueued=0)
#dType.SetJOGCmd(self.api, 1, 0)
self.lint = threading.Timer(0.2,self.stop)
self.lint.start()
def gripp(self):
dType.SetEndEffectorGripper(self.api, 1, 1, isQueued=0)
def letgo(self):
dType.SetEndEffectorGripper(self.api, 1, 0, isQueued=0)
def stop(self):
dType.SetJOGCmd(self.api, 1, 0)
def stopcmp(self):
dType.SetEndEffectorGripper(self.api, 0, 0, isQueued=0)
def panic(self):
dType.SetQueuedCmdForceStopExec(self.api)
def getCurrentCmd(self):
return dType.GetQueuedCmdCurrentIndex(self.api)
def JOGtest(self):
dType.SetJOGCmd(self.api,1,1)
dType.SetJOGCmd(self.api,1,9)
def JOGtest2(self):
dType.SetJOGLParams(self.api, 100, 200, isQueued=0)
dType.SetJOGCmd(self.api,1,9)
time.sleep(1)
for speed in range(100,10,-10):
time.sleep(0.3)
dType.SetJOGLParams(self.api, speed, 200, isQueued=0)
def setparams(self, name, speed):
lookup = {'base' : 0, 'reararm' : 1, 'forearm' : 2, 'gripperroation' :3}
if name == 'rail':
dType.SetJOGLParams(self.api, speed, 200, isQueued=0)
else:
speeds = [0,0,0,0]
speeds[lookup[name]] = speed
dType.SetJOGJointParams(self.api, speeds[0], 200, speeds[1], 200, speeds[2], 200, speeds[3], 200, isQueued = 1)
def setJOGcmd(self, cmd):
dType.SetJOGCmd(self.api,1,cmd)
candy = Candybot()
startTime = time.time()
nowTime = 0
currentMotor = 0
motorcfg = [0] * 10
motorcfg[1] = {"name" : 'base' , "positive":1, "negative":2, 'speedslot':2}
motorcfg[2] = {"name" : 'reararm' ,"positive":3, "negative":4, 'speedslot':4}
motorcfg[3] = {"name" : 'forearm' ,"positive":5, "negative":6, 'speedslot':6}
motorcfg[4] = {"name" : 'rail' ,"positive":9, "negative":10, 'speedslot':1}
friction = 0.95
movement_time = 3
max_movement_time = 5
min_speed = 1
max_speed = 50
speed = 0
stopTime = 0
isRunning = False
def adjustSpeed(motor, adjustment):
global nowTime, stopTime, currentMotor, speed, motorcfg, movement_time, max_speed, max_movement_time
if currentMotor==motor:
if (adjustment>0 and speed>0) or (adjustment<0 and speed<0):
speed += adjustment * 1-(abs(speed)/max_speed)
else:
stopMotorFunctions()
stopTime += movement_time/2 * max_movement_time/(stopTime - nowTime)
else:
currentMotor = motor
speed = adjustment
stopTime = nowTime + movement_time
if(speed>0):
candy.setJOGcmd(motorcfg[currentMotor]['positive'])
#print("JOG command {}".format(motorcfg[currentMotor]['positive']))
else:
candy.setJOGcmd(motorcfg[currentMotor]['negative'])
# print("JOG command {}".format(motorcfg[currentMotor]['negative']))
def stopMotorFunctions():
global currentMotor, speed
currentMotor = 0
speed = 0
candy.stop()
def updateLoop():
global nowTime, stopTime, currentMotor, speed, friction, min_speed, motorcfg
nowTime = (time.time() - startTime)
threading.Timer(1, updateLoop).start()
if currentMotor != 0:
if nowTime > stopTime:
stopMotorFunctions()
return
else:
speed *= friction
if abs(speed) < min_speed:
stopMotorFunctions()
return
candy.setparams(motorcfg[currentMotor]['name'], abs(speed))
#print("JOG parameter {} to {}".format( motorcfg[currentMotor]['speedslot'], abs(speed)) )
def rt():
adjustSpeed(1, 5)
def lt():
adjustSpeed(1, -5)
def fw():
adjustSpeed(2, 5)
def bc():
adjustSpeed(2, -5)
def up():
adjustSpeed(3, 5)
def dn():
adjustSpeed(3, -5)
def lr():
adjustSpeed(4, 10)
def ll():
adjustSpeed(4, -10)
updateLoop()
#
#c = Candybot()
#
#
#def slide_left():
# c.linear(-1)
#
#def slide_right():
# c.linear(1)
#
#def rotate_left():
# c.base(-1)
#
#def rotate_right():
# c.base(1)
#
#def forward():
# c.forearm(1)
#
#def back():
# c.forearm(-1)
#
#def up():
# c.rearArm(1)
#
#def down():
# c.rearArm(-1)
#
def stopCmp():
candy.stopcmp()
def grab():
candy.gripp()
# c.stopcmp()
#
def drop():
candy.letgo()
# c.stopcmp()
#
def stop():
candy.panic()
#
#def getCurrentCmd():
# return c.getCurrentCmd()
#
#def updateLinearSpeed():
# pass
#
############################### Chat Parser Section ###############################
with open('/Users/pawel/Documents/Junction2018/twitch_key.json', 'rb') as json_data:
data = json.load(json_data)
primary_stream_key = data["primary_stream_key"]
oauth_key = data["oauth_key"]# bytes('oauth:gcpsl3csq85rf3lk8c1ijzer8deuat', "utf8")
print(primary_stream_key)
print(oauth_key)
HOST = "irc.chat.twitch.tv"
PORT = 6667
PASS = oauth_key
NICK = "TwitchGrabsCandy"
CHAN = "#twitchgrabscandy"
s = socket.socket()
s.connect((HOST, PORT))
s.send("PASS {}\r\n".format(PASS).encode("utf-8"))
s.send("NICK {}\r\n".format(NICK).encode("utf-8"))
s.send("JOIN {}\r\n".format(CHAN).encode("utf-8"))
s.setblocking(0)
queue = []
while True:
response = ""
ready = select.select([s], [], [], 0.1)
if ready[0]:
print("Trying")
response = s.recv(1024).decode("utf-8") # blocking
# print(response)
if re.search("up", response) != None:
queue = queue + ["up"]
if re.search("down", response) != None:
queue = queue + ["down"]
if re.search("go left", response) != None:
queue = queue + ["gleft"]
if re.search("go right", response) != None:
queue = queue + ["gright"]
if re.search("turn left", response) != None:
queue = queue + ["tleft"]
if re.search("turn right", response) != None:
queue = queue + ["tright"]
if re.search("forward", response) != None:
queue = queue + ["forward"]
if re.search("back", response) != None:
queue = queue + ["back"]
if re.search("grasp", response) != None:
queue = queue + ["grasp"]
if re.search("drop", response) != None:
queue = queue + ["drop"]
# checking the motion that is first in the FIFO queue
if len(queue):
if queue[0] == "up":
up()
elif queue[0] == "down":
dn()
elif queue[0] == "gleft":
ll()
elif queue[0] == "gright":
lr()
elif queue[0] == "tleft":
lt()
elif queue[0] == "tright":
rt()
elif queue[0] == "forward":
fw()
elif queue[0] == "back":
bc()
elif queue[0] == "grasp":
grab()
elif queue[0] == "drop":
drop()
else:
print("Something is TERRIBLY wrong!")
print("Location 3")
print(queue)
# removing the first item from queue
queue = queue[1:]
print("Location 4")
print(queue)
sleep(1)