Example #1
0
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()
Example #2
0
#!/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)
Example #3
0
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)
Example #4
0
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)
Example #5
0
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)
Example #6
0
#!/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)
Example #7
0
# 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