def __init__(self):
self.name = rospy.get_name()
self.publisher = rospy.Publisher('joint_states',
JointState,
queue_size=10)
# Set up action client
self.action_server = actionlib.SimpleActionServer(
'%s/follow_joint_trajectory' % self.name,
FollowJointTrajectoryAction, self.do_action_callback, False)
self.action_server.start()
self.rate = rospy.Rate(10) # 10hz
self.joint_state = JointState()
self.joint_state.header = Header()
self.joint_state.name = ['hip', 'shoulder', 'elbow', 'wrist']
self.joint_state.position = [
1.57, 1.57, 1.57, 1.57
] # will be updated in home() before publication in the loop
self.joint_state.velocity = []
self.joint_state.effort = []
# Initialise the PCA9685 using the default address (0x40) and the bus
self.pwm = Adafruit_PWM_Servo_Driver.PWM(address=0x40, busnum=1)
self.pwm.setPWMFreq(cal.PWM_FREQUENCY)
self.home()
import time
from Adafruit_I2C import Adafruit_I2C
import Adafruit_PWM_Servo_Driver
pwm = Adafruit_PWM_Servo_Driver.PWM(address=0x40, busnum=1)
#pwm = Adafruit_PWM_Servo_Driver.PWM()
servo_min = 320
servo_max = 570
count = servo_min
pwm.setPWMFreq(500)
time.sleep(1)
pwm.setPWM(0, 0, 2550)
#pwm.setPWM(1,0,320)
print("initializing")
time.sleep(2)
print("spining")
pwm.setPWM(0, 0, 2610)
#pwm.setPWM(1,0,330)
time.sleep(5)
print("Stop")
pwm.setPWM(0, 0, 2550)
#pwm.setPWM(1,0,320)
#while count < servo_max:
# pwm.setPWM(0, 0, count)
# time.sleep(1)
# count = count + 25
def setup_pwm(freq=200):
pwm = Adafruit_PWM_Servo_Driver.PWM()
pwm.setPWMFreq(freq)
return pwm
