def __reset(self):
for i in range(6):
self.__body_pub.publish(
ServoCommand(index=i, angle=90, duration=0.1))
self.__shin_pub.publish(
ServoCommand(index=i, angle=self.__shin_down, duration=0.1))
self.__foot_pub.publish(
ServoCommand(index=i, angle=self.__foot_down, duration=0.1))
def rotate(dir):
global up_servos, down_servos
for j in range(1):
# lift legs up
time = get_rottime()
for i in up_servos:
shin_pub.publish(
ServoCommand(index=i, angle=140, duration=0.1 * time))
rospy.sleep(0.05 * time)
for i in up_servos:
foot_pub.publish(
ServoCommand(index=i, angle=50, duration=0.1 * time))
rospy.sleep(0.05 * time)
# push up legs back
for i in up_servos:
if i >= 3:
body_pub.publish(
ServoCommand(index=i,
angle=90 - (10 * dir),
duration=0.1 * time))
else:
body_pub.publish(
ServoCommand(index=i,
angle=90 + (10 * dir),
duration=0.1 * time))
# push forward
for i in down_servos:
if i >= 3:
body_pub.publish(
ServoCommand(index=i,
angle=90 + (10 * dir),
duration=0.1 * time))
else:
body_pub.publish(
ServoCommand(index=i,
angle=90 - (10 * dir),
duration=0.1 * time))
rospy.sleep(0.15 * time)
# put legs down
for i in up_servos:
shin_pub.publish(
ServoCommand(index=i, angle=100, duration=0.1 * time))
foot_pub.publish(
ServoCommand(index=i, angle=70, duration=0.1 * time))
rospy.sleep(0.15 * time)
temp_servos = up_servos
up_servos = down_servos
down_servos = temp_servos
#!/usr/bin/env python
import math
import rospy
from hexapod_servo.msg import ServoCommand
if __name__ == '__main__':
body_pub = rospy.Publisher('/hexapod/move_body', ServoCommand)
shin_pub = rospy.Publisher('/hexapod/move_shin', ServoCommand)
foot_pub = rospy.Publisher('/hexapod/move_foot', ServoCommand)
rospy.init_node('servo_control_test', anonymous=True)
theta = 0
while rospy.is_shutdown():
pass
rospy.sleep(1)
for i in range(6):
body_pub.publish(ServoCommand(index=i, angle=90))
shin_pub.publish(ServoCommand(index=i, angle=90))
foot_pub.publish(ServoCommand(index=i, angle=90))
rospy.sleep(1)
def __init__(self):
self.__servo_pub = rospy.Publisher('direct', ServoCommand)
self.__body_pub = rospy.Publisher('/hexapod/servo/joint/body',
ServoCommand)
self.__shin_pub = rospy.Publisher('/hexapod/servo/joint/shin',
ServoCommand)
self.__foot_pub = rospy.Publisher('/hexapod/servo/joint/foot',
ServoCommand)
self.__body_offset_pub = rospy.Publisher(
'/hexapod/servo/joint/offset/body', ServoCommand)
self.__shin_offset_pub = rospy.Publisher(
'/hexapod/servo/joint/offset/shin', ServoCommand)
self.__foot_offset_pub = rospy.Publisher(
'/hexapod/servo/joint/offset/foot', ServoCommand)
rospy.init_node('joint_keyboard_calibration')
while rospy.is_shutdown():
pass
stdscr = curses.initscr()
curses.cbreak()
stdscr.keypad(1)
offset = {'b': [0] * 6, 's': [0] * 6, 'f': [0] * 6}
mode = 'b'
servo = 0
key = ''
while key != ord('q'):
key = stdscr.getch()
clear = False
if key in [ord('b'), ord('s'), ord('f')]:
mode = chr(key)
clear = True
elif key == ord('x'):
pickle.dump(offset, open("joint_offsets.dat", "wb"))
elif key == ord('l'):
offset = pickle.load(open("joint_offsets.dat", "rb"))
elif key == curses.KEY_UP:
offset[mode][servo] += 1
clear = True
elif key == curses.KEY_DOWN:
offset[mode][servo] -= 1
clear = True
elif key == curses.KEY_RIGHT:
servo += 1
clear = True
elif key == curses.KEY_LEFT:
servo -= 1
clear = True
if servo < 0:
servo = 0
elif servo > 5:
servo = 5
if clear:
stdscr.clear()
stdscr.addstr(0, 0, str(offset[mode][servo]))
stdscr.addstr(1, 0, mode)
stdscr.addstr(2, 0, str(servo))
stdscr.refresh()
for i in range(6):
cmd = ServoCommand(index=i, angle=90, duration=0.1)
self.__shin_pub.publish(cmd)
self.__body_pub.publish(cmd)
self.__foot_pub.publish(cmd)
self.__shin_offset_pub.publish(
ServoCommand(index=i, angle=offset['s'][i],
duration=0))
self.__body_offset_pub.publish(
ServoCommand(index=i, angle=offset['b'][i],
duration=0))
self.__foot_offset_pub.publish(
ServoCommand(index=i, angle=offset['f'][i],
duration=0))
rospy.sleep(0.1)
curses.endwin()
#!/usr/bin/env python
from random import randrange
import rospy
from hexapod_servo.msg import ServoCommand
if __name__ == '__main__':
pub = rospy.Publisher('/hexapod/servo_command', ServoCommand)
rospy.init_node('servo_serial_test', anonymous=True)
flip = False
while not rospy.is_shutdown():
flip = not flip
for i in range(32):
servo_command = ServoCommand(index=i + 1,
angle=90 - 45 if flip else 180 - 45)
pub.publish(servo_command)
#rospy.sleep(0.1)
rospy.sleep(3)
def __walk(self, twist):
#linear_offset = self.__body_linear_swing * twist.linear.x
#angular_offset = self.__body_angular_swing * twist.angular.z
#augh
#if (abs(twist.linear.x) >= 0.25):
# linear_vel = math.copysign(1.0, twist.linear.x)
#else:
# linear_vel = 0
#if (abs(twist.angular.z) >= 0.25):
# angular_vel = math.copysign(1.0, twist.angular.z)
#else:
# angular_vel = 0
linear_vel = twist.linear.x * 4.0
angular_vel = twist.angular.z * 4.0
total_shares = abs(linear_vel) + abs(angular_vel)
angular_ratio = angular_vel / total_shares
linear_ratio = linear_vel / total_shares
linear_offset = self.__body_linear_swing * (linear_ratio)
angular_offset = self.__body_angular_swing * (angular_ratio)
#t = angular_offset
#angular_offset = angular_offset - math.copysign(linear_offset / 2.0, angular_offset)
#linear_offset = angular_offset - math.copysign(angular_offset / 2.0, linear_offset)
# self.__update_odometry(linear_offset, angular_offset, self.__cycle_time)
# Lift legs up.
for i in self.__up:
self.__shin_pub.publish(
ServoCommand(index=i,
angle=self.__shin_up,
duration=self.__lift_time))
self.__foot_pub.publish(
ServoCommand(index=i,
angle=self.__foot_up,
duration=self.__lift_time))
# Wait for completion.
rospy.sleep(self.__lift_time)
# Rotate up legs into position for next cycle.
for i in self.__up:
if i >= 3:
angle = 90 - linear_offset + angular_offset
else:
angle = 90 - linear_offset - angular_offset
self.__body_pub.publish(
ServoCommand(index=i, angle=angle, duration=self.__swing_time))
# Push down legs forward.
for i in self.__down:
if i >= 3:
angle = 90 + linear_offset - angular_offset
else:
angle = 90 + linear_offset + angular_offset
self.__body_pub.publish(
ServoCommand(index=i, angle=angle, duration=self.__swing_time))
# Wait for completion.
rospy.sleep(self.__swing_time)
# Bring up legs down.
for i in self.__up:
self.__shin_pub.publish(
ServoCommand(index=i,
angle=self.__shin_down,
duration=self.__lift_time))
self.__foot_pub.publish(
ServoCommand(index=i,
angle=self.__foot_down,
duration=self.__lift_time))
# Wait for completion.
rospy.sleep(self.__lift_time)
# Swap servos for next cycle.
temp_servos = self.__up
self.__up = self.__down
self.__down = temp_servos
def __move(self, index, angle, duration):
self.__servo_pub.publish(
ServoCommand(index=index, angle=angle, duration=duration))
def rotate(y_left, y_right, x_left, x_right):
global up_servos, down_servos
#time = 1 + ((1 - ((abs(y_left) + abs(y_right)) / 2))*2)
time = 1.25
for j in range(1):
for i in up_servos:
shin_pub.publish(
ServoCommand(index=i,
angle=140 + legs_angle,
duration=0.1 * time))
rospy.sleep(0.075 * time)
for i in up_servos:
foot_pub.publish(
ServoCommand(index=i,
angle=40 - legs_angle,
duration=0.1 * time))
rospy.sleep(0.075 * time)
# push up legs back
for i in up_servos:
if i >= 3:
body_pub.publish(
ServoCommand(index=i,
angle=90 - (10 * y_left),
duration=0.1 * time))
else:
body_pub.publish(
ServoCommand(index=i,
angle=90 + (10 * y_right),
duration=0.1 * time))
# push forward
for i in down_servos:
if i >= 3:
body_pub.publish(
ServoCommand(index=i,
angle=90 + (10 * y_left),
duration=0.1 * time))
else:
body_pub.publish(
ServoCommand(index=i,
angle=90 - (10 * y_right),
duration=0.1 * time))
rospy.sleep(0.175 * time)
# put legs down
for i in up_servos:
if i >= 3:
shin_pub.publish(
ServoCommand(index=i,
angle=100 + legs_angle + (5 * x_left),
duration=0.1 * time))
else:
shin_pub.publish(
ServoCommand(index=i,
angle=100 + legs_angle - (5 * x_right),
duration=0.1 * time))
foot_pub.publish(
ServoCommand(index=i,
angle=65 - legs_angle,
duration=0.1 * time))
rospy.sleep(0.175 * time)
temp_servos = up_servos
up_servos = down_servos
down_servos = temp_servos
foot_pub = rospy.Publisher('/hexapod/servo/joint/foot', ServoCommand)
rospy.init_node('servo_control_test', anonymous=True)
js = pygame.joystick.Joystick(0)
js.init()
theta = 0
while rospy.is_shutdown():
pass
rospy.sleep(0)
legs_angle = 0
for i in range(6):
body_pub.publish(ServoCommand(index=i, angle=90))
shin_pub.publish(ServoCommand(index=i, angle=100 + legs_angle))
foot_pub.publish(ServoCommand(index=i, angle=65 - legs_angle))
rospy.sleep(1)
def get_speed():
global js
return (-js.get_axis(1), js.get_axis(4))
def rotate(y_left, y_right, x_left, x_right):
global up_servos, down_servos
#time = 1 + ((1 - ((abs(y_left) + abs(y_right)) / 2))*2)
time = 1.25
#!/usr/bin/env python
from random import randrange
import rospy
from hexapod_servo.msg import ServoCommand
if __name__ == '__main__':
pub = rospy.Publisher('/hexapod/servo_command', ServoCommand)
rospy.init_node('servo_serial_test', anonymous=True)
flip = False
while not rospy.is_shutdown():
flip = not flip
for i in range(32):
servo_command = ServoCommand(index=i, angle=90)
pub.publish(servo_command)
rospy.sleep(0.1)
#rospy.sleep(1)