def setServoPulse(channel, pulse): print pulse pulseLength = 1000000 # 1,000,000 us per second pulseLength /= 100 # 100 Hz #print "%d us per period" % pulseLength pulseLength /= 4096 # 12 bits of resolution #print "%d us per bit" % pulseLength pulse *= 1000 pulse /= pulseLength print pulse pulse = round(pulse,0) pwm.setPWM(channel, 0, pulse) print "Les pulse sont :", pulse
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 RefreshServo (tickvalue,channel): pwm.setPWM(channel,0,tickvalue)
def update(yaw,pitch):
if yaw == 'up':
pwm.setPWM(channel_yaw,0, yaw_up)
elif yaw == 'down':
pwm.setPWM(channel_yaw,0, yaw_down)
elif yaw == 'hold':
pwm.setPWM(channel_yaw,0, yaw_hold)
# print "Victor est le plus nul !!"
else:
pwm.setPWM(channel_yaw,0, yaw_hold)
if pitch == 'up':
pwm.setPWM(channel_pitch,0, pitch_up)
elif pitch == 'down':
pwm.setPWM(channel_pitch,0, pitch_down)
elif pitch == 'hold':
pwm.setPWM(channel_pitch,0, pitch_hold)
# print "Abde est le plus nul !!"
else:
pwm.setPWM(channel_pitch,0, pitch_hold)
def setServoPulse(channel, pulse): print pulse pulseLength = 1000000 # 1,000,000 us per second pulseLength /= 100 # 100 Hz #print "%d us per period" % pulseLength pulseLength /= 4096 # 12 bits of resolution #print "%d us per bit" % pulseLength pulse *= 1000 pulse /= pulseLength print pulse pulse = round(pulse,0) pwm.setPWM(channel, 0, pulse) print "Les pulse sont :", pulse pwm =pwm.PWM(0x41) pwm.setPWMFreq(100) channel_yaw = 0 channel_pitch = 1 yaw_up = 700 yaw_hold = 614 yaw_down = 550 pitch_up = 700 pitch_hold = 614 pitch_down = 550 def update(yaw,pitch): if yaw == 'up':
def setup_pwm(freq=200):
pwm = Adafruit_PWM_Servo_Driver.PWM()
pwm.setPWMFreq(freq)
return pwm
