Merged the chat parser with the robot control

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)
+
+
+    
+
+
+    
+