def grip():
i = 500
while i != 2450:
pi.set_servo_pulsewidth(25, i)
sleep(0.001)
i += 5
print('gripping', i)
return
def release():
i = 2450
while i != 710:
pi.set_servo_pulsewidth(25, i)
sleep(0.001)
i += -5
print('release', i)
return
def initialize_servos():
global initial_PWM_servo1 #zero pos
global initial_PWM_servo2
global initial_PWM_servo3
global initial_PWM_servo4
initial_PWM_servo1 = 1500 #zero pos of base
initial_PWM_servo2 = 700 # elbow 900
initial_PWM_servo3 = 500 #grip
initial_PWM_servo4 = 1500 # 1500 2nd elbow close to 500 extends
global curr_PWM_servo1 #zero pos
global curr_PWM_servo2
global curr_PWM_servo3
curr_PWM_servo1 = 1500 #zero pos
curr_PWM_servo2 = 900
curr_PWM_servo3 = 500
sleep(1)
pi.set_servo_pulsewidth(18, initial_PWM_servo1)
# print "initialize base servo"
sleep(1) #goes to zero position
pi.set_servo_pulsewidth(23, initial_PWM_servo2) #goes to zero position
# print "initialize lowering servo"
pi.set_servo_pulsewidth(24, initial_PWM_servo4) #goes to zero position
# print "initialize elbow servo"
pi.set_servo_pulsewidth(25, initial_PWM_servo3)
sleep(1)
def lower(temp):
global curr_PWM_servo2
Next_PWM_servo2 = temp
print(Next_PWM_servo2)
if (Next_PWM_servo2 < curr_PWM_servo2):
while curr_PWM_servo2 != Next_PWM_servo2:
pi.set_servo_pulsewidth(23, curr_PWM_servo2)
sleep(0.001)
curr_PWM_servo2 += -2.5
# print "lowering servo2 1", curr_PWM_servo2
elif (Next_PWM_servo2 > curr_PWM_servo2):
while curr_PWM_servo2 != Next_PWM_servo2:
pi.set_servo_pulsewidth(23, curr_PWM_servo2)
sleep(0.001)
curr_PWM_servo2 += 2.5
print("lowering servo2 2", curr_PWM_servo2)
else:
curr_PWM_servo2 = Next_PWM_servo2
pi.set_servo_pulsewidth(23, curr_PWM_servo2)
def back2origin():
global initial_PWM_servo1 #zero pos
global initial_PWM_servo2
global initial_PWM_servo3
global initial_PWM_servo4
global curr_PWM_servo1 #zero pos
global curr_PWM_servo2
global curr_PWM_servo3
if (initial_PWM_servo1 > curr_PWM_servo1):
while curr_PWM_servo1 != initial_PWM_servo1:
pi.set_servo_pulsewidth(18, curr_PWM_servo1)
sleep(0.00125)
curr_PWM_servo1 += 2.5
print("rotating to servo1 position 1", curr_PWM_servo1)
elif (initial_PWM_servo1 < curr_PWM_servo1):
while curr_PWM_servo1 != initial_PWM_servo1:
pi.set_servo_pulsewidth(18, curr_PWM_servo1)
sleep(0.00125)
curr_PWM_servo1 += -2.5
print("rotating to servo1 position 2", curr_PWM_servo1)
else:
curr_PWM_servo1 = initial_PWM_servo1
pi.set_servo_pulsewidth(18, curr_PWM_servo1)
def movebase(x, y):
Next_PWM_servo1 = object_location_PWM(x, y)
global curr_PWM_servo1
if (Next_PWM_servo1 > curr_PWM_servo1):
while curr_PWM_servo1 != Next_PWM_servo1:
pi.set_servo_pulsewidth(18, curr_PWM_servo1)
sleep(0.001)
curr_PWM_servo1 += 2.5
print("rotating to servo1 position 1", curr_PWM_servo1)
elif (Next_PWM_servo1 < curr_PWM_servo1):
while curr_PWM_servo1 != Next_PWM_servo1:
pi.set_servo_pulsewidth(18, curr_PWM_servo1)
sleep(0.001)
curr_PWM_servo1 += -2.5
print("rotating to servo1 position 2", curr_PWM_servo1)
else:
curr_PWM_servo1 = Next_PWM_servo1
pi.set_servo_pulsewidth(18, curr_PWM_servo1)
def movebasetobin(bin_num):
Next_bin_location_PWM = bin_location_PWM(bin_num)
global curr_PWM_servo1
if (Next_bin_location_PWM > curr_PWM_servo1):
while curr_PWM_servo1 != Next_bin_location_PWM:
pi.set_servo_pulsewidth(18, curr_PWM_servo1)
sleep(0.001)
curr_PWM_servo1 += 2.5
print("rotating to bin position 1", curr_PWM_servo1)
elif (Next_bin_location_PWM < curr_PWM_servo1):
while curr_PWM_servo1 != Next_bin_location_PWM:
pi.set_servo_pulsewidth(18, curr_PWM_servo1)
sleep(0.001)
curr_PWM_servo1 += -2.5
print("rotating to bin position 2", curr_PWM_servo1)
else:
curr_PWM_servo1 = Next_bin_location_PWM
pi.set_servo_pulsewidth(18, curr_PWM_servo1)
def rise():
global initial_PWM_servo2
global curr_PWM_servo2
if (initial_PWM_servo2 < curr_PWM_servo2):
while curr_PWM_servo2 != initial_PWM_servo2:
pi.set_servo_pulsewidth(23, curr_PWM_servo2)
sleep(0.002)
curr_PWM_servo2 += -5
# print "Raising servo2 1", curr_PWM_servo2
elif (initial_PWM_servo2 > curr_PWM_servo2):
while curr_PWM_servo2 != initial_PWM_servo2:
pi.set_servo_pulsewidth(23, curr_PWM_servo2)
sleep(0.002)
curr_PWM_servo2 += 5
print("Raising servo2 2", curr_PWM_servo2)
else:
curr_PWM_servo2 = initial_PWM_servo2
pi.set_servo_pulsewidth(23, curr_PWM_servo2)