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