def home2Handle_Control(t1_f, t2_f):
print('Drive Arm to needed position')
rospy.init_node('pwmNode', anonymous=True)
pwmPublisher1 = rospy.Publisher('pwm1', Float32, queue_size=1)
pwmPublisher2 = rospy.Publisher('pwm2', Float32, queue_size=1)
rospy.Subscriber("joint1_theta", Float32, joint1Callback)
rospy.Subscriber("joint2_theta", Float32, joint2Callback)
secs = 3
print("Waiting", secs, "seconds...")
time.sleep(secs)
t1_f = n.interp(112, [35, 275], [-120, 120])
t2_f = n.interp(140, [60, 300], [120, -120])
t1_i = -90 #deg
t2_i = 90 #deg
e1 = abs(t1_f - t1_i)
e2 = abs(t2_f - t2_i)
#error = n.transpose([e1,e2])
#Joint Control independently for now:
#Tune for each joint
P = 12
I = 0.1
D = 10
eTot = 0
eOld = e1
dir = 0
tX_new = 0
while (e1 >= 1):
#tX = measured angle
tX_new = n.interp(joint1_queue.pop(), [35, 275], [-120, 120])
print("Popped from t1: " + str(tX_new))
if (abs(t1_f - tX_new) <= 1):
break
if (t1_f < tX_new):
dir = 0
else:
dir = 1
Calc_PID = C.PID_EE(P, I, D, t1_f, tX_new, eTot, eOld, dir)
#Returns eTot, eNew, signal, and tX
eOld = Calc_PID[1]
eTot = Calc_PID[0]
signal_j1 = Calc_PID[2]
#send this to PWM converted representation via Arduino
pwmPublisher1.publish(signal_j1)
time.sleep(0.1)
#tX_new = n.interp(joint1_queue.pop(),[35,275],[-120,120]) # + signal_j1/1000 #TEMPORARY FOR TESTING
#e1 = abs(t1_f-tX_new)
#print("Error: " + str(e1) + " Signal: " + str(signal_j1))
#Kill Signal
signal_j1 = 0
pwmPublisher1.publish(signal_j1)
P = 9
#I = 0.01
I = 0
D = 5
eTot = 0
eOld = e2
tX_new = 0
while (e2 >= 1):
#tX = measured angle
tX_new = n.interp(joint2_queue.pop(), [60, 300], [120, -120])
print("Joint 2: " + str(tX_new))
if (abs(t2_f - tX_new) <= 1):
break
if (t2_f < tX_new):
dir = 0
else:
dir = 1
Calc_PID = C.PID_EE2(P, I, D, t2_f, tX_new, eTot, eOld, dir)
#Returns eTot, eNew, signal, and tX
eOld = Calc_PID[1]
eTot = Calc_PID[0]
signal_j2 = Calc_PID[2]
#send this to PWM converted representation via Arduino
pwmPublisher2.publish(signal_j2)
time.sleep(0.01)
#Kill Signal 2
signal_j2 = 0
pwmPublisher2.publish(signal_j2)
def rotateEE():
# Creating a ROS node to publish PWM signals to an Arduino subscriber
rospy.init_node('pwmNode', anonymous=True)
pwmPublisher3 = rospy.Publisher('pwm3', Float32, queue_size=1)
rospy.Subscriber("joint3_theta", Float32, joint3Callback)
secs = 3
print("Waiting", secs, "seconds...")
time.sleep(secs)
print('Control EE to open handle')
tX = 0 #Degrees
tF = 130 #Degrees
P = 50
I = 0.1
D = 15
eTot = 0
eOld = abs(tF - tX)
e3 = abs(tF - tX)
tX_new = 0
while (e3 >= 3):
#tX = measured angle
tX_new = joint3_queue.pop()
if (abs(tF - tX_new) <= 3):
print('Abort')
break
if (tF < tX_new):
dir = 1
else:
dir = 0
Calc_PID = C.PID_EE(P, I, D, tF, tX_new, eTot, eOld, dir)
#Returns eTot, eNew, signal, and tX
eOld = Calc_PID[1]
eTot = Calc_PID[0]
signal_j3 = Calc_PID[2]
#send this to PWM converted representation via Arduino
pwmPublisher3.publish(signal_j3)
time.sleep(0.1) # TODO need to change time delay
print("Joint 3: " + str(signal_j3) + "angle: " + str(tX_new))
#print("Error: " + str(e3) + " Signal: " + str(signal_j3))
signal_j3 = 0
pwmPublisher3.publish(signal_j3)
nudge = False
while (nudge == False):
rospy.init_node('pwmNode', anonymous=True)
pwmPublisher1 = rospy.Publisher('pwm1', Float32, queue_size=1)
pwmPublisher2 = rospy.Publisher('pwm2', Float32, queue_size=1)
rospy.Subscriber("joint1_theta", Float32, joint1Callback)
rospy.Subscriber("joint2_theta", Float32, joint2Callback)
signal_j1 = -85
signal_j2 = 50
pwmPublisher1.publish(signal_j1)
pwmPublisher2.publish(signal_j2)
time.sleep(0.25)
signal_j1 = 0
signal_j2 = 0
pwmPublisher1.publish(signal_j1)
pwmPublisher2.publish(signal_j2)
#Perform forward kinematics to 'nudge'door and prevent it relocking
nudge = True
tF = 0
tX = tX_new
e4 = abs(tF - tX)
print(e4)
eOld = e4
eTot = 0
P = 50
I = 0.5
D = 10
while (e4 >= 3):
#tX = measured angle
tX_new = joint3_queue.pop()
if (abs(tF - tX_new) <= 3):
break
if (tF < tX_new):
dir = 1
else:
dir = 0
Calc_PID2 = C.PID_EE(P, I, D, tF, tX_new, eTot, eOld, dir)
#Returns eTot, eNew, signal, and tX
eOld = Calc_PID2[1]
eTot = Calc_PID2[0]
signal_j3 = Calc_PID2[2]
pwmPublisher3.publish(signal_j3)
time.sleep(0.1) # TODO need to change time delay
#tX_new = joint3_queue.pop() #- signal_j3/1000 #TEMPORARY FOR TESTING
#e4 = abs(tF-tX_new)
#print("Error: " + str(e4) + " Signal: " + str(signal_j3))
#send this to PWM converted representation via Arduino
signal_j3 = 0
pwmPublisher3.publish(signal_j3)
while True:
continue
