From 95892796edafc945f10d5d1c4c5694f459754bd9 Mon Sep 17 00:00:00 2001 From: pk23 Date: Sat, 24 Nov 2018 21:23:19 +0200 Subject: [PATCH] Merged the chat parser with the robot control --- movearm/moveCmdTester copy.py | 412 ++++++++++++++++++++++++++++++++++ 1 file changed, 412 insertions(+) create mode 100644 movearm/moveCmdTester copy.py diff --git a/movearm/moveCmdTester copy.py b/movearm/moveCmdTester copy.py new file mode 100644 index 0000000..89ce2f6 --- /dev/null +++ b/movearm/moveCmdTester copy.py @@ -0,0 +1,412 @@ +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 + return + + 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): + print("gripping\n") + #dType.SetEndEffectorGripper(self.api, 1, 1, isQueued=0) + + def letgo(self): + print("drops\n") + #dType.SetEndEffectorGripper(self.api, 1, 0, isQueued=0) + + def stop(self): + print("stops\n") + #dType.SetJOGCmd(self.api, 1, 0) + + def stopcmp(self): + print("stops compressor\n") + 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': + print("Change speed rail " + str(speed) +"\n" ) + #dType.SetJOGLParams(self.api, speed, 200, isQueued=0) + + else: + speeds = [0,0,0,0] + speeds[lookup[name]] = speed + print("change speed " + name + str(speed)) + #dType.SetJOGJointParams(self.api, speeds[0], 200, speeds[1], 200, speeds[2], 200, speeds[3], 200, isQueued = 1) + + def setJOGcmd(self, cmd): + print("Joint "+ str(cmd) + "is moving \n") + #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 = 100 + +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 * (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) + print("TURNING RIGHT!") + +def lt(): + adjustSpeed(1, -5) + print("TURNING LEFT!") + +def fw(): + adjustSpeed(2, 5) + print("GOING FORWARD!") + +def bc(): + adjustSpeed(2, -5) + print("GOING BACK!") + +def up(): + adjustSpeed(3, 5) + print("UP!") + +def dn(): + adjustSpeed(3, -5) + print("DOWN!") + +def lr(): + adjustSpeed(4, 10) + print("GOING RIGHT!") + +def ll(): + adjustSpeed(4, -10) + print("GOING LEFT!") + +updateLoop() + + +def stopCmp(): + candy.stopcmp() + +def grab(): + candy.gripp() + print("GRASP!") + +def drop(): + candy.letgo() + print("DROPPING!") + +def stop(): + candy.panic() + + + ############################### 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!") + + # removing the first item from queue + queue = queue[1:] + + sleep(1) + + + + + + +