def pub_angles():
pub = rospy.Publisher(
'angles_topic', angles_msg,
queue_size=10) #node is publishing on the topic, angles_topic
rospy.init_node(
'pub_angles',
anonymous=True) #pub_angles is the name of the publishing node
rate = rospy.Rate(50) # 40hz
msg = angles_msg()
x.set_servo_angle(10, 200, 4)
time.sleep(0.007)
y.set_servo_angle(10, 200, 4)
time.sleep(0.007)
while not rospy.is_shutdown():
print "b"
while time.time() < (timer + delay):
#if time.time() < (timer + delay):
#break
msg.pitch = herkulex.servo(20).get_servo_angle()
time.sleep(0.0014)
print "a"
msg.yaw = herkulex.servo(19).get_servo_angle()
time.sleep(0.0014)
rospy.loginfo(msg)
print "c"
pub.publish(msg)
#10ms #100Hz
herkulex.close()
def exitProgram():
print("Exiting...type1\n\n\n")
herkulex.alive = 0
camera_linedetection.alive = 0
stepper_output.alive = 0
coldblock_output.alive = 0
hotend_output.alive = 0
aircon_output.alive = 0
sensors.alive = 0
herkulex.clear_errors()
servo1.torque_off()
servo2.torque_off()
servo3.torque_off()
sensors.ambience_sensor_enabled = 0
sensors.adc1_sensor_enabled = 0
sensors.adc2_sensor_enabled = 0
sensors.adc3_sensor_enabled = 0
aircon_output.aircon_enabled = 0
coldblock_output.coldblock_enabled = 0
hotend_output.hotend_enabled = 0
stepper_output.motor_enabled = 0
herkulex.servo_enabled = 0
time.sleep(1.0)
grovepi.analogWrite(peltierfanpin1,0)
grovepi.analogWrite(peltierfanpin2,0)
grovepi.ledCircle_off(ledcirclepin)
time.sleep(0.2)
peltier1.start(0)
peltier2.start(0)
heater.start(0)
GPIO.output(motor_enable_pin,1) #set H to disable
GPIO.output(motor_dir_pin,0) #set H to disable
GPIO.output(motor_step_pin,0) #set H to disable
herkulex.close()
def talker():
while 1:
for y in range(0, 10): #bend
herkulex.servo(motor_id[y]).set_servo_angle(angles[33][y], 200, 4)
time.sleep(0.0007)
time.sleep(2.5)
for x in range(18, 49): #shift
for y in range(0, 10):
herkulex.servo(motor_id[y]).set_servo_angle(angles[x][y], 3, 4)
#print motor_id[y]
time.sleep(0.0007)
time.sleep(0.025)
for y in range(0, 10): #swing_max
herkulex.servo(motor_id[y]).set_servo_angle(angles[107][y], 150, 4)
time.sleep(0.0007)
time.sleep(delay)
for y in range(0, 10): #swing_min
herkulex.servo(motor_id[y]).set_servo_angle(angles[158][y], 100, 4)
time.sleep(0.0007)
time.sleep(delay)
## second_step ##
for y in range(0, 10): #zero_second_step
herkulex.servo(motor_id[y]).set_servo_angle(angles[193][y], 80, 4)
time.sleep(0.0007)
time.sleep(0.8)
for x in range(194, 213): #shift2
for y in range(0, 10):
herkulex.servo(motor_id[y]).set_servo_angle(angles[x][y], 3, 4)
time.sleep(0.0007)
time.sleep(0.025)
for y in range(0, 10): #swing_max2
herkulex.servo(motor_id[y]).set_servo_angle(angles[264][y], 150, 4)
time.sleep(0.0007)
time.sleep(delay)
for y in range(0, 10): #swing_min2
herkulex.servo(motor_id[y]).set_servo_angle(angles[313][y], 150, 4)
time.sleep(0.0007)
time.sleep(delay)
for y in range(0, 10): #zero_second step
herkulex.servo(motor_id[y]).set_servo_angle(angles[33][y], 80, 4)
time.sleep(0.0007)
time.sleep(0.8)
######################
herkulex.close()
def talker():
while 1:
for y in range (0,10): #bend
herkulex.servo(motor_id[y]).set_servo_angle(angles[33][y],200,4)
time.sleep(0.0007)
time.sleep(2.5)
for x in range (18,49): #shift
for y in range (0,10):
herkulex.servo(motor_id[y]).set_servo_angle(angles[x][y],3,4)
#print motor_id[y]
time.sleep(0.0007)
time.sleep(0.025)
for y in range (0,10): #swing_max
herkulex.servo(motor_id[y]).set_servo_angle(angles[107][y],150,4)
time.sleep(0.0007)
time.sleep(delay)
for y in range (0,10): #swing_min
herkulex.servo(motor_id[y]).set_servo_angle(angles[158][y],100,4)
time.sleep(0.0007)
time.sleep(delay)
## second_step ##
for y in range (0,10): #zero_second_step
herkulex.servo(motor_id[y]).set_servo_angle(angles[193][y],80,4)
time.sleep(0.0007)
time.sleep(0.8)
for x in range (194,213): #shift2
for y in range (0,10):
herkulex.servo(motor_id[y]).set_servo_angle(angles[x][y],3,4)
time.sleep(0.0007)
time.sleep(0.025)
for y in range (0,10): #swing_max2
herkulex.servo(motor_id[y]).set_servo_angle(angles[264][y],150,4)
time.sleep(0.0007)
time.sleep(delay)
for y in range (0,10): #swing_min2
herkulex.servo(motor_id[y]).set_servo_angle(angles[313][y],150,4)
time.sleep(0.0007)
time.sleep(delay)
for y in range (0,10): #zero_second step
herkulex.servo(motor_id[y]).set_servo_angle(angles[33][y],80,4)
time.sleep(0.0007)
time.sleep(0.8)
######################
herkulex.close()
def talker(): for y in range (0,10): herkulex.servo(motor_id[y]).set_servo_angle(right[y],50,4) time.sleep(0.0007) time.sleep(5) for y in range (0,10): herkulex.servo(motor_id[y]).set_servo_angle(left[y],50,4) time.sleep(0.0007) time.sleep(5) #for y in range (0,10): #print herkulex.servo(motor_id[y]).get_servo_angle() ###################### herkulex.close()
def talker():
while 1:
if (r_ankle_pitch.get_servo_status() != 0):
r_knee_pitch.torque_off()
elif (r_ankle_roll.get_servo_status() != 0):
r_knee_pitch.torque_off()
elif (r_knee_pitch.get_servo_status() != 0):
r_knee_pitch.torque_off()
elif (r_hip_roll.get_servo_status() != 0):
r_hip_roll.torque_off()
elif (r_hip_pitch.get_servo_status() != 0):
r_hip_pitch.torque_off()
elif (l_hip_roll.get_servo_status() != 0):
l_hip_roll.torque_off()
elif (l_hip_pitch.get_servo_status() != 0):
l_hip_pitch.torque_off()
elif (l_knee_pitch.get_servo_status() != 0):
l_knee_pitch.torque_off()
elif (l_ankle_roll.get_servo_status() != 0):
l_ankle_roll.torque_off()
elif (l_ankle_pitch.get_servo_status() != 0):
l_ankle_pitch.torque_off()
motor_id = [4, 2, 10, 5, 7, 12, 8, 1, 11, 6]
#l_hip_roll_id ,l_hip_pitch_id ,l_knee_pitch_id ,l_ankle_pitch_id, l_ankle_roll_id,
#r_hip_roll_id , r_hip_pitch_id, r_knee_pitch_id, r_ankle_pitch_id, r_ankle_roll_id
###########################################
for c in range(0, 5): #for five steps
time.sleep(0.056)
for x in range(0, 260):
for y in range(0, 10):
herkulex.servo(motor_id[y]).set_servo_angle(
angles[x][y], 2, 4)
print("angle received by")
print(motor_id[y])
time.sleep(0.0007)
time.sleep(0.013)
if c == 4:
break
###########################################
herkulex.close()
def talker():
while 1:
for y in range(0, 10):
herkulex.servo(motor_id[y]).set_servo_angle(
down[y] + zero_offset[y], 100, 4)
time.sleep(0.0007)
time.sleep(1)
for y in range(0, 10):
herkulex.servo(motor_id[y]).set_servo_angle(
up[y] + zero_offset[y], 100, 4)
time.sleep(0.0007)
time.sleep(1)
#for y in range (0,10):
#print herkulex.servo(motor_id[y]).get_servo_angle()
######################
herkulex.close()
def talker():
for y in range(0, motor_no):
herkulex.servo(motor_id[y]).set_servo_angle(init[y], 200, 4)
time.sleep(0.0007)
time.sleep(3)
while 1:
for y in range(0, motor_no):
herkulex.servo(motor_id[y]).set_servo_angle(right_bend[y], 150, 4)
time.sleep(0.0007)
time.sleep(2)
for y in range(0, motor_no):
herkulex.servo(motor_id[y]).set_servo_angle(left_bend[y], 150, 4)
time.sleep(0.0007)
time.sleep(2)
######################
herkulex.close()
def talker():
while 1:
if (r_ankle_pitch.get_servo_status()!=0):
r_knee_pitch.torque_off()
elif (r_ankle_roll.get_servo_status()!=0):
r_knee_pitch.torque_off()
elif (r_knee_pitch.get_servo_status()!=0):
r_knee_pitch.torque_off()
elif (r_hip_roll.get_servo_status()!=0):
r_hip_roll.torque_off()
elif (r_hip_pitch.get_servo_status()!=0):
r_hip_pitch.torque_off()
elif (l_hip_roll.get_servo_status()!=0):
l_hip_roll.torque_off()
elif (l_hip_pitch.get_servo_status()!=0):
l_hip_pitch.torque_off()
elif (l_knee_pitch.get_servo_status()!=0):
l_knee_pitch.torque_off()
elif (l_ankle_roll.get_servo_status()!=0):
l_ankle_roll.torque_off()
elif (l_ankle_pitch.get_servo_status()!=0):
l_ankle_pitch.torque_off()
motor_id = [4, 2, 10, 5, 7, 12, 8, 1, 11, 6]
#l_hip_roll_id ,l_hip_pitch_id ,l_knee_pitch_id ,l_ankle_pitch_id, l_ankle_roll_id,
#r_hip_roll_id , r_hip_pitch_id, r_knee_pitch_id, r_ankle_pitch_id, r_ankle_roll_id
###########################################
for c in range (0, 5):#for five steps
time.sleep(0.056)
for x in range (0, 260):
for y in range (0, 10):
herkulex.servo(motor_id[y]).set_servo_angle(angles[x][y],2,4)
print("angle received by")
print (motor_id[y])
time.sleep(0.0007)
time.sleep(0.013)
if c == 4:
break
###########################################
herkulex.close()
def talker(): ###################### herkulex.close()
def talker():
pub = rospy.Publisher(
'angles', Float64,
queue_size=10) #node is publishing to the angles topic
rospy.init_node(
'test_motor_behavior',
anonymous=True) #plot_graphs is the name of the publishing node
t_init = rospy.get_time()
rate = rospy.Rate(40) # 40hz
while not rospy.is_shutdown():
#hiii#
while (1):
if i < 1:
l_hand_pitch.set_servo_angle(0, 100, 4)
time.sleep(0.0007)
l_hand_roll.set_servo_angle(90, 100, 4)
time.sleep(0.0007)
l_low_arm.set_servo_angle(0, 100, 4)
time.sleep(0.0007)
r_hand_pitch.set_servo_angle(0, 100, 4)
time.sleep(0.0007)
r_hand_roll.set_servo_angle(-90, 100, 4)
time.sleep(0.0007)
r_low_arm.set_servo_angle(0, 100, 4)
time.sleep(1)
print("yoo")
i = i + 1
l_hand_pitch.set_servo_angle(0, 100, 4)
time.sleep(0.0007)
l_hand_roll.set_servo_angle(90, 100, 4)
time.sleep(0.0007)
l_low_arm.set_servo_angle(0, 100, 4)
time.sleep(0.5)
l_low_arm.set_servo_angle(-45, 100, 4)
time.sleep(0.5)
l_hand_roll.set_servo_angle(135, 100, 4)
time.sleep(0.0007)
l_low_arm.set_servo_angle(90, 200, 4)
time.sleep(1.5)
l_low_arm.set_servo_angle(-45, 200, 4)
time.sleep(0.0007)
l_hand_roll.set_servo_angle(68, 100, 4)
time.sleep(0.5)
l_hand_pitch.set_servo_angle(0, 100, 4)
time.sleep(0.0007)
l_hand_roll.set_servo_angle(90, 100, 4)
time.sleep(0.0007)
l_low_arm.set_servo_angle(0, 100, 4)
time.sleep(0.0007)
time.sleep(1)
######################################################################################
r_hand_pitch.set_servo_angle(0, 100, 4)
time.sleep(0.007)
r_hand_roll.set_servo_angle(-90, 100, 4)
time.sleep(0.007)
r_low_arm.set_servo_angle(0, 100, 4)
time.sleep(0.5)
r_low_arm.set_servo_angle(-90, 100, 4)
time.sleep(0.007)
r_hand_roll.set_servo_angle(-135, 100, 4)
time.sleep(1)
r_low_arm.set_servo_angle(-90, 200, 4)
time.sleep(0.007)
r_hand_roll.set_servo_angle(-45, 100, 4)
time.sleep(0.0007)
r_low_arm.set_servo_angle(75, 200, 4)
time.sleep(2)
r_hand_roll.set_servo_angle(-90, 100, 4)
time.sleep(0.0007)
r_low_arm.set_servo_angle(0, 200, 4)
time.sleep(0.5)
rate.sleep()
herkulex.close()
