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 talker(): for i in range(0,4) : for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(hand_in[y]+zero_offset[y],50,4) time.sleep(0.0007) time.sleep(0.5) for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(hand_out[y]+zero_offset[y],50,4) time.sleep(0.0007) time.sleep(0.5)
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: 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(): 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: 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 main(): global servo1 rospy.init_node('servo_controller', anonymous=True) rate = rospy.Rate(10) # 10hz #connect to the serial port herkulex.connect("/dev/ttyUSB0", 115200) #scan for servos, it returns a tuple with servo id & model number servos = herkulex.scan_servos() print servos servo1 = servo(servos.servoid) servo_status = servo1.get_servo_status() print "servo status: ", servo_status rospy.Subscriber('/servo/cmdpos', Int16, moveServo_cb) pub = rospy.Publisher('/servo/pos', Int16, queue_size=10) servo_position = servo1.get_servo_position() print "servo pos: ", servo_position pub.publish(servo_position) rospy.spin()
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()
#!/usr/bin/env python # license removed for brevity from beginner_tutorials.msg import angles_msg import rospy import serial import herkulex import time herkulex.connect('/dev/ttyUSB0', 115200) x = herkulex.servo(20) y = herkulex.servo(19) angles = [20, 19] ## MOTOR_TORQUE_ON ## x.torque_on() y.torque_on() angles_arr = ['pitch', 'yaw'] timer = time.time() delay = 1.5 * 60 def pub_angles(): pub = rospy.Publisher( 'angles_topic', angles_msg, queue_size=10) #node is publishing on the topic, angles_topic
import herkulex import time herkulex.connect("/dev/ttyAMA0", 115200) DROP = herkulex.servo(50) ALL = herkulex.servo(0xfe) #power = 1000 power = 1000 init_a = -512 end_a = 512 ALL.torque_on() DROP.set_servo_angle(0, 1, 0x08) time.sleep(1) DROP.set_servo_angle(-95, 1, 0x08) time.sleep(1) DROP.set_servo_speed(1, 0x06) ALL.set_led(0x06)
#!/usr/bin/env python # license removed for brevity import rospy from std_msgs.msg import String import serial import herkulex import time herkulex.connect('/dev/ttyUSB0',115200) #biped l_hip_roll = herkulex.servo(4) l_hip_pitch = herkulex.servo(2) l_knee_pitch = herkulex.servo(10) l_ankle_pitch = herkulex.servo(5) l_ankle_roll = herkulex.servo(7) r_hip_roll = herkulex.servo(12) r_hip_pitch = herkulex.servo(8) r_knee_pitch = herkulex.servo(1) r_ankle_pitch = herkulex.servo(11) r_ankle_roll = herkulex.servo(6) l_low_arm_roll = herkulex.servo(18) l_arm_roll = herkulex.servo(17) l_shoulder_pitch = herkulex.servo(16) r_shoulder_pitch = herkulex.servo(13) r_arm_roll = herkulex.servo(14) r_low_arm_roll = herkulex.servo(15) #new=herkulex.servo(10) #torso
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()
## Date of attempt1: 12|07|2016 ## STATIC WALKING ## ## INPUT FROM EXCEL SHEET, ANGLES PRECALCULATED IN MATLAB ## ## frequency = 40Hz (=2X20Hz(20Hz is the assumed bandwidth for the motor, So the nyquist criteria is applied to get a minimal sampling frequency of 40Hz between two consecutive angles to a motor)) ## import rospy from std_msgs.msg import String import serial import herkulex import time import xlrd herkulex.connect('/dev/ttyUSB0',115200) ## MOTOR_ID ## #biped l_hip_roll = herkulex.servo(4) l_hip_pitch = herkulex.servo(2) l_knee_pitch = herkulex.servo(10) l_ankle_pitch = herkulex.servo(5) l_ankle_roll = herkulex.servo(7) r_hip_roll = herkulex.servo(12) r_hip_pitch = herkulex.servo(8) r_knee_pitch = herkulex.servo(1) r_ankle_pitch = herkulex.servo(11) r_ankle_roll = herkulex.servo(6) #torso l_hand_pitch = herkulex.servo(13) l_hand_roll = herkulex.servo(14) l_hand_yaw = herkulex.servo(15) r_hand_pitch = herkulex.servo(16) r_hand_roll = herkulex.servo(17)
#!/usr/bin/env python # license removed for brevity import rospy from std_msgs.msg import String import serial import herkulex import time herkulex.connect('/dev/ttyUSB0', 115200) #biped l_hip_roll = herkulex.servo(4) l_hip_pitch = herkulex.servo(2) l_knee_pitch = herkulex.servo(10) l_ankle_pitch = herkulex.servo(5) l_ankle_roll = herkulex.servo(7) r_hip_roll = herkulex.servo(12) r_hip_pitch = herkulex.servo(8) r_knee_pitch = herkulex.servo(1) r_ankle_pitch = herkulex.servo(11) r_ankle_roll = herkulex.servo(6) l_low_arm_roll = herkulex.servo(18) l_arm_roll = herkulex.servo(17) l_shoulder_pitch = herkulex.servo(16) r_shoulder_pitch = herkulex.servo(13) r_arm_roll = herkulex.servo(14) r_low_arm_roll = herkulex.servo(15) #new=herkulex.servo(10) #torso
#!/usr/bin/env python # license removed for brevity import rospy from std_msgs.msg import String import serial import herkulex import time herkulex.connect('/dev/ttyUSB0', 115200) l_low_arm_roll = herkulex.servo(18) l_arm_roll = herkulex.servo(17) l_shoulder_pitch = herkulex.servo(16) r_shoulder_pitch = herkulex.servo(13) r_arm_roll = herkulex.servo(14) r_low_arm_roll = herkulex.servo(15) motor_id = [18, 17, 16, 13, 14, 15] motor_no = 6 ## MOTOR_TORQUE_ON ## l_low_arm_roll.torque_on() l_arm_roll.torque_on() l_shoulder_pitch.torque_on() r_shoulder_pitch.torque_on() r_arm_roll.torque_on() r_low_arm_roll.torque_on() init = [0, 90, 80, -80, -90, 0] right_bend = [0, 90, 80, -70, 7, -70] left_bend = [70, -27, 70, -80, -90, 0]
## Date of attempt1: 12|07|2016 ## STATIC WALKING ## ## INPUT FROM EXCEL SHEET, ANGLES PRECALCULATED IN MATLAB ## ## frequency = 40Hz (=2X20Hz(20Hz is the assumed bandwidth for the motor, So the nyquist criteria is applied to get a minimal sampling frequency of 40Hz between two consecutive angles to a motor)) ## import rospy from std_msgs.msg import String import serial import herkulex import time import xlrd herkulex.connect('/dev/ttyUSB0', 115200) ## MOTOR_ID ## #biped l_hip_roll = herkulex.servo(4) l_hip_pitch = herkulex.servo(2) l_knee_pitch = herkulex.servo(10) l_ankle_pitch = herkulex.servo(5) l_ankle_roll = herkulex.servo(7) r_hip_roll = herkulex.servo(12) r_hip_pitch = herkulex.servo(8) r_knee_pitch = herkulex.servo(1) r_ankle_pitch = herkulex.servo(11) r_ankle_roll = herkulex.servo(6) #torso l_hand_pitch = herkulex.servo(13) l_hand_roll = herkulex.servo(14) l_hand_yaw = herkulex.servo(15) r_hand_pitch = herkulex.servo(16) r_hand_roll = herkulex.servo(17)
## Date : 12|08|2016 ## ## Author : Surabhi Verma ## ## TORSO_STATIC_WALKING ## ## INPUT FROM EXCEL SHEET, ANGLES PRECALCULATED IN MATLAB ## import rospy from std_msgs.msg import String import serial import herkulex import time import xlrd import math herkulex.connect('/dev/ttyUSB0',115200) #biped l_hip_roll = herkulex.servo(4) l_hip_pitch = herkulex.servo(2) l_knee_pitch = herkulex.servo(10) l_ankle_pitch = herkulex.servo(5) l_ankle_roll = herkulex.servo(7) r_hip_roll = herkulex.servo(12) r_hip_pitch = herkulex.servo(8) r_knee_pitch = herkulex.servo(1) r_ankle_pitch = herkulex.servo(11) r_ankle_roll = herkulex.servo(6) #torso #l_hand_pitch = herkulex.servo(13) #l_hand_roll = herkulex.servo(14) #l_hand_yaw = herkulex.servo(15) #r_hand_pitch = herkulex.servo(16) #r_hand_roll = herkulex.servo(17)
## Date : 12|08|2016 ## ## Author : Surabhi Verma ## ## TORSO_STATIC_WALKING ## ## INPUT FROM EXCEL SHEET, ANGLES PRECALCULATED IN MATLAB ## import rospy from std_msgs.msg import String import serial import herkulex import time import xlrd import math herkulex.connect('/dev/ttyUSB0', 115200) #biped l_hip_roll = herkulex.servo(4) l_hip_pitch = herkulex.servo(2) l_knee_pitch = herkulex.servo(10) l_ankle_pitch = herkulex.servo(5) l_ankle_roll = herkulex.servo(7) r_hip_roll = herkulex.servo(12) r_hip_pitch = herkulex.servo(8) r_knee_pitch = herkulex.servo(1) r_ankle_pitch = herkulex.servo(11) r_ankle_roll = herkulex.servo(6) #torso #l_hand_pitch = herkulex.servo(13) #l_hand_roll = herkulex.servo(14) #l_hand_yaw = herkulex.servo(15) #r_hand_pitch = herkulex.servo(16) #r_hand_roll = herkulex.servo(17)
import herkulex import time herkulex.connect("/dev/ttyAMA0", 115200) FR = herkulex.servo(0xfd) FL = herkulex.servo(16) BR = herkulex.servo(10) BL = herkulex.servo(20) ALL = herkulex.servo(0xfe) #power = 1000 power = 1000 rtime = 1.54 #rtime = 20 calib = 0 calib = -50 corrpow = -300 ALL.torque_on() FR.set_servo_speed(-power + corrpow, 0x06) FL.set_servo_speed(power + corrpow + calib, 0x06) BR.set_servo_speed(-power - corrpow, 0x06) BL.set_servo_speed(power - corrpow + calib, 0x06) time.sleep(rtime) ALL.set_servo_speed(1, 0x06) ALL.set_led(0x06)
peltierpin2 = 20 #Cold Block heaterpin = 16 #HotEnd peltierfanpin1 = 5 #D5 AC peltierfanpin2 = 3 #D3 Cold block motor_dir_pin = 26 #Stepper motor motor_step_pin = 19 motor_enable_pin = 13 buzzerpin = 4 #buzzer ledcirclepin = 6 #led circular #Servo Motor Configuration herkulex.connect("/dev/ttyS0", 115200) herkulex.clear_errors() #servos = herkulex.scan_servos(0x01,0x02) #min and max range of ServoID #print(servos) servo1=servo(0x01,0x02) #ServoID, Model servo2=servo(0x02,0x02) #ServoID, Model servo3=servo(0x03,0x02) #ServoID, Model #servo1.set_servo_angle(50, 1, 0x00) #goaltime is 1 to 255 time.sleep(0.1) #Configuration of Pin IO GPIO.setmode(GPIO.BCM) GPIO.setwarnings(False) #disable warning #Stepper Motor GPIO.setup(motor_step_pin, GPIO.OUT) GPIO.setup(motor_dir_pin, GPIO.OUT) GPIO.setup(motor_enable_pin, GPIO.OUT) #Aircon GPIO.setup(peltierpin1, GPIO.OUT) peltier1 = GPIO.PWM(peltierpin1, 50) peltier1.start(0)
def talker(): for i in range(0,4): for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(l_hand_up[y]+zero_offset[y],100,4) time.sleep(0.0007) time.sleep(1) for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(both[y]+zero_offset[y],100,4) time.sleep(0.0007) time.sleep(1) for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(l_hand_up[y]+zero_offset[y],100,4) time.sleep(0.0007) time.sleep(1) for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(straight[y]+zero_offset[y],100,4) time.sleep(0.0007) time.sleep(1) for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(r_hand_up[y]+zero_offset[y],100,4) time.sleep(0.0007) time.sleep(1) for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(both[y]+zero_offset[y],100,4) time.sleep(0.0007) time.sleep(1) for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(r_hand_up[y]+zero_offset[y],100,4) time.sleep(0.0007) time.sleep(1) for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(straight[y]+zero_offset[y],100,4) time.sleep(0.0007) time.sleep(1)
import herkulex import time herkulex.connect("/dev/ttyAMA0", 115200) FR = herkulex.servo(0xfd) FL = herkulex.servo(16) BR = herkulex.servo(10) BL = herkulex.servo(20) ALL = herkulex.servo(0xfe) #power = 1000 power = 1000 #rtime = 1.54 rtime = 20 calib = 0 calib = -50 ALL.torque_on() FR.set_servo_speed(-power, 0x06) FL.set_servo_speed(power + calib, 0x06) BR.set_servo_speed(-power, 0x06) BL.set_servo_speed(power + calib, 0x06) time.sleep(rtime) ALL.set_servo_speed(1, 0x06) ALL.set_led(0x06)
## Date : 12|08|2016 ## ## Author : Surabhi Verma ## ## TORSO_STATIC_WALKING ## ## INPUT FROM EXCEL SHEET, ANGLES PRECALCULATED IN MATLAB ## import rospy from std_msgs.msg import Float64 import serial import herkulex import time import xlrd import math herkulex.connect('/dev/ttyUSB4', 115200) l_hand_roll = herkulex.servo(17) l_hand_pitch = herkulex.servo(16) l_low_arm = herkulex.servo(18) r_hand_roll = herkulex.servo(14) r_hand_pitch = herkulex.servo(13) r_low_arm = herkulex.servo(15) motor_id = [4, 2, 10, 5, 7, 12, 8, 1, 11, 6, 13, 14, 15, 16, 17, 18] delay = 1.5 ############################ ## MOTOR_TORQUE_ON ## l_hand_pitch.torque_on()
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()
import herkulex import time herkulex.connect("/dev/ttyAMA0", 115200) DROP = herkulex.servo(50) ALL = herkulex.servo(0xfe) #power = 1000 power = 1000 init_a = -512 end_a = 512 ALL.torque_on() DROP.set_servo_angle(0, 1, 0x08) time.sleep(1) DROP.set_servo_angle(-95, 1, 0x08) time.sleep(1) DROP.set_servo_speed(1, 0x06) ALL.set_led(0x06)