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()
#!/usr/bin/env python # license removed for brevity ## 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)
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)
The library was created by Achu Wilson (mailto:[email protected]) for the internal projects of Sastra Robotics This free software is distributed under the GNU General Public License. See http://www.gnu.org/licenses/gpl.html for details. For usage of this code for commercial purposes contact Sastra Robotics India Pvt. Ltd. (mailto:[email protected]) This is an example script to connect to a Herkulex bus & scan for the servos 0x00-OFF 0x02-BLUE 0x03-CYAN 0x04-RED 0x05-ORANGE 0x06-VIOLET 0x07-WHITE """ import herkulex from herkulex import servo #connect to the serial port herkulex.connect("/dev/ttyUSB0", 115200) print("Connected") #scan for servos, it returns a tuple with servo id & model number servos = herkulex.scan_servos(doPrint=True) print(servos) #herkulex.torque_on(); herkulex.set_led(servos, 0x05)
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)
#!/usr/bin/env python # license removed for brevity ## 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)
# code to interface the hardware # TODO - Integrate JointTrajectoryAction server # - Enable & disable torque service # - Set PID of joints as service or param import rospy import herkulex import math from sensor_msgs.msg import JointState num_joints = 4 herkulex.connect("/dev/ttyUSB0", 115200) herkulex.torque_off(1) herkulex.torque_off(2) def joint_state_publisher(): pub = rospy.Publisher("joint_states", JointState, queue_size=5) rospy.init_node("sr_jnt_stt") r = rospy.Rate(300) while not rospy.is_shutdown(): msg = JointState() msg.header.stamp = rospy.Time.now() msg.position = num_joints * [0.0] msg.velocity = num_joints * [0.0] # msg.effort = num_joints * [0.0] msg.name = ["joint1", "joint2", "joint3", "gripper_con"] msg.position[0] = math.radians(herkulex.get_servo_angle(1)) msg.position[1] = math.radians(herkulex.get_servo_angle(2)) msg.position[2] = 0.0 msg.position[3] = 0.0