from PyControl import * from xboxController import * import numpy as np zeke = PyControl() zeke.stop() maxStates = [6.58991792126, .3, 0.3, 6.61606370861, 0.0348490572685, 6.83086639225]; minStates = [0,.02,.01,0.183086039735,-.01,0.197775641646]; #POSITION CONTROL def kinematicControl(): c = XboxController([.06,.005,.01,.08,.002,.05]) target = np.array(zeke.getState()) j = 0; while True: j+=1 updates = c.getUpdates() #print updates if updates is None: print "Done" zeke.stop() break target = target+updates for i in range(0,6): if target[i]>maxStates[i]: target[i] = maxStates[i]
#print getSimpleState() time.sleep(.05) def simpleControl( controls ): # controls = rotation,extension,grip,turntable (run this in a loop) # this converts a list of 4 control inputs into the necessary 6 and sends them off izzy.control([controls[0]] + [0] + [controls[1]] + [0] + [controls[2]] + [0]) t.control([controls[4]]) def getSimpleState(): # this returns the state of the 4 axis state = izzy.getState() simpleState = [state[0], state[2], state[4]] + t.getState() return simpleState #IF YOU WANT TO AVOID UP AND DOWN, AND WRIST, SEND ZEROS FOR controls[1] and controls[3] #print getSimpleState() directControl() # these are needed to close serial connections setup in the begining izzy.stop() izzy.ser.close() t.stop() t.ser.close()
#print zeke.xyz() time.sleep(.01) #DIRECT CONTROL def directControl(): c = XboxController([100,155,155,155,70,100]) select = True while True: controls = c.getUpdates() if controls is "switch": select = not select elif controls is None: print "Done" zeke.stop() break elif select: zeke.control(controls) else: izzy.control(controls) time.sleep(.03) kinematicControl() zeke.stop() zeke.ser.close() izzy.stop() izzy.ser.close() t.stop() t.ser.close()
izzy.control(controls) t.control([controls[5]]) #print controls # store this however you please. (concatenate into array?) simpleControls = [controls[0], controls[2], controls[4], controls[5]] #print getSimpleState() time.sleep(.05) def simpleControl(controls): # controls = rotation,extension,grip,turntable (run this in a loop) # this converts a list of 4 control inputs into the necessary 6 and sends them off izzy.control( [controls[0]]+[0]+[controls[1]]+[0]+[controls[2]]+[0]) t.control([controls[4]]) def getSimpleState(): # this returns the state of the 4 axis state = izzy.getState() simpleState = [state[0],state[2],state[4]]+t.getState() return simpleState #IF YOU WANT TO AVOID UP AND DOWN, AND WRIST, SEND ZEROS FOR controls[1] and controls[3] #print getSimpleState() directControl() # these are needed to close serial connections setup in the begining izzy.stop() izzy.ser.close() t.stop() t.ser.close()
import time pi = 3.14159 r = PyControl("/dev/ttyACM0")#"COM6",115200, .04, [.505, .2601, .234-.0043, 0.0164], [.12, 0, -.08]) # #print r.getState() # a = r.getState() speed = 40*pi/180.0 r.traj(0,3,speed) #time.sleep(1) #r.sendStateRequest([1,2,3,4,5,6]) #time.sleep(.5) #print r.getTarget() #for i in range(0,100): # print r.getState() # r.sendStateRequest([1,2,3,4,5,6]) # print r.getTarget() r.traj(0,4,speed) #r.sendControls([30,0,0,0,0,0]) #time.sleep(.8) #time.sleep(.8) #time.sleep(2) r.stop() r.ser.close()