def run (self):
# s_joint = Servo(channel=0, min=250, max=327, freq=50)
# s_joint = Servo(channel=0, min=250, max=530, freq=100) # mg995
# time.sleep(4)
s_tilt = Servo(channel=1, min=250, max=327, freq=50)
time.sleep(1)
# s_pan = Servo(channel=2, min=250, max=380, freq=50)
s_pan = Servo(channel=2, min=300, max=380, freq=50)
# put your init and global variables here
# main loop
while 1:
headPosition = self.getInputs().headPosition
lampPosition = self.getInputs().lampPosition
s_tilt.move_to(1 - headPosition.y)
s_pan.move_to(1- headPosition.x)
# s_joint.move_to(lampPosition.z)
time.sleep(0.5)
pwm = PWM(0x40)
pwm.setPWMFreq(50)
pwm.softwareReset()
class GimbalServo:
def __init__(self, channelYaw=0, channelPitch=1, I2C_address = 0x40, MinPosYaw=200, MaxPosYaw=300, MinPosPitch=200, MaxPosPitch=300):
# init and configure PWM module
self.pwm = PWM(I2C_address) # Create the PWM object
self.pwm.setPWMFreq(60) # Set frequency to 60 Hz
# store arguments
# channels
self.channelYaw = channelYaw
self.channelPitch = channelPitch
# servos min and max positions
# check if argument are positive values
if MinPosYaw > 0 and \
MaxPosYaw > 0 and \
MinPosPitch > 0 and \
MaxPosPitch > 0 :
# yaw
# check if min<max
if MinPosYaw < MaxPosYaw:
self.MinPosYaw = MinPosYaw
self.MaxPosYaw = MaxPosYaw
else:
self.MinPosYaw = MaxPosYaw
self.MaxPosYaw = MinPosYaw
# pitch
# check if min<max
if MinPosPitch < MaxPosPitch:
self.MinPosPitch = MinPosPitch
self.MaxPosPitch = MaxPosPitch
else:
self.MinPosPitch = MaxPosPitch
self.MaxPosPitch = MinPosPitch
else:
#if position values are negative affect dummy values
self.MinPosYaw=200
self.MaxPosYaw=300
self.MinPosPitch=200
self.MaxPosPitch=300
#Compute values from radian to servo PWM unit
self.gainYaw = (self.MaxPosYaw - self.MinPosYaw) / 3.1415 # pi -> 180 degree
self.offsetYaw = (self.MaxPosYaw + self.MinPosYaw) / 2 # zero position
self.gainPitch = (self.MaxPosPitch - self.MinPosPitch) / 3.1415 # pi -> 180 degree
self.offsetPitch = (self.MaxPosPitch + self.MinPosPitch) / 2 # zero position
#print "Min Yaw: ", self.MinPosYaw, "Max Yaw: ", self.MaxPosYaw, "Min Pitch: ", self.MinPosPitch, "Max Pitch: ", self.MaxPosPitch # debug
#print "Gain Yaw: ", self.gainYaw, "Offset Yaw: ", self.offsetYaw, "Gain Pitch: ", self.gainPitch, "Offset Pitch: ", self.offsetPitch # debug
#update servos position uppon yaw and pitch values given in radian, and tilt given in raw channel value
def updatePosition(self, yaw, pitch, tilt):
# Check if yaw and pitch value are good ( must be from -pi/2 to +pi/2)
if abs(yaw) < 2 and abs(pitch) < 2:
# Compute tilt offset (tilt channel varies from 1000 to 1520 respectively 0 and 90 degree
tiltOffset = (tilt - 1000)/4
# Compute servo values
yawValue = self.offsetYaw + ( self.gainYaw * yaw)
pitchValue = self.offsetPitch + tiltOffset + ( self.gainPitch * pitch)
#clamp values to min and max servo position
if yawValue > self.MaxPosYaw:
yawValue = self.MaxPosYaw
if yawValue < self.MinPosYaw:
yawValue = self.MinPosYaw
if pitchValue > self.MaxPosPitch:
pitchValue = self.MaxPosPitch
if pitchValue < self.MinPosPitch:
pitchValue = self.MinPosPitch
self.pwm.setPWM(self.channelYaw,0, int(yawValue))
self.pwm.setPWM(self.channelPitch,0, int(pitchValue))
#print "Yaw: ", int(yawValue), "Pitch: ", int(pitchValue) #debug
#update RGB led Color
def initRGB(self, channelRed=13, channelGreen=14, channelBlue=15):
# channels
self.channelRed = channelRed
self.channelGreen = channelGreen
self.channelBlue = channelBlue
#update RGB led colors
def updateRGBColor(self, R, G, B):
self.pwm.setPWM(self.channelRed,0, int(R))
self.pwm.setPWM(self.channelGreen,0, int(G))
self.pwm.setPWM(self.channelBlue,0, int(B))
def __del__(self):
# Release servo power on garbage collect
self.pwm.softwareReset()
servo2 = Servo(channel=0, min=150, max=530, freq=100) # mg995
# sg90
# Servo pan
# servo2 = Servo(channel=2, min=250, max=380, freq=50)
# sg90 2
# servo tilt
# servo3 = Servo(channel=1, min=240, max=327, freq=50)
time.sleep(4)
servo2.move_to(0)
# # servo3.move_to(0)
# #
time.sleep(4)
servo2.move_to(1)
#
# time.sleep(0.1)
# servo2.move_to(0)
# # servo3.move_to(1)
#
# #
time.sleep(4)
# servo2.move_to(0.5)
# time.sleep(1)
# servo3.move_to(0.5)
pwm = PWM(0x40)
pwm.setPWMFreq(50)
pwm.softwareReset()
print('done')
def sreset(self):
PWM.softwareReset()
print 'breaking servo process', e
break
end = False
# end_position = need to checkout each servo i think
# def move_servo(on, end_position):
# end = 0
# while True
servos, keyboard = Pipe()
t1 = Process(target=move_servos, args=(servos, ))
t1.daemon = True
t1.start()
end = False
while True:
x = raw_input("?")
if x == 'q':
end = True
pwm.softwareReset()
break
elif x == 's':
s = Process(target=move_servos, args=(servos, ))
s.daemon = True
s.start()
else:
keyboard.send(x)
# time.sleep(2)
if end:
break
class GimbalServo:
def __init__(self, channelYaw, channelPitch, I2C_address, MinPosYaw, MaxPosYaw, MinPosPitch, MaxPosPitch,ButeeMinPitch,ButeeMaxPitch,tiltOffsetFactor,globalLogger):
# init and configure PWM module
self.pwm = PWM(I2C_address) # Create the PWM object
self.pwm.setPWMFreq(50) # Set frequency to 50 Hz = > Ne pas modifier !!!
# store arguments
self.channelYaw = channelYaw
self.channelPitch = channelPitch
self.MinPosYaw = MinPosYaw
self.MaxPosYaw = MaxPosYaw
self.MinPosPitch = MinPosPitch
self.MaxPosPitch = MaxPosPitch
self.ButeeMinPitch = ButeeMinPitch
self.ButeeMaxPitch = ButeeMaxPitch
self.tiltOffsetFactor = tiltOffsetFactor
self.logger = globalLogger
#Compute values from radian to servo PWM unit
self.gainYaw = (self.MaxPosYaw - self.MinPosYaw)*3.2727 / 3.1415 # pi/1.1034 -> 55 degrees de course
self.offsetYaw = (self.MaxPosYaw + self.MinPosYaw) / 2 # zero position
self.gainPitch = (self.MaxPosPitch - self.MinPosPitch) * 2 / 3.1415 # pi/2 -> 90 degree
#Update servos position uppon yaw and pitch values given in radian, and tilt given in raw channel value
def updatePosition(self, yaw, pitch, tilt):
# Check if yaw and pitch value are good ( must be approximatively from -pi/2 to +pi/2)
if abs(yaw) < 2 and abs(pitch) < 2:
# Compute tilt offset (tilt channel varies from 1000 to 1520 respectively 0 and 90 degree and clamp if rc off
if 999 < tilt < 1521:
tiltOffset = ((1520-tilt)*self.tiltOffsetFactor)
elif tilt == 0: #Si la telecommande n'est pas allumee / detectee
tiltOffset = 0
else: #Si valeur absurde
tiltOffset = 0
# Compute servo values
yawValue = self.offsetYaw + ( self.gainYaw * yaw)
pitchValue = self.MinPosPitch + tiltOffset + ( self.gainPitch * pitch)
#Eviter de venir en butee physique
if pitchValue > self.ButeeMaxPitch:
pitchValue = self.ButeeMaxPitch
if pitchValue < self.ButeeMinPitch:
pitchValue = self.ButeeMinPitch
#Eviter de venir en butee physique
if yawValue > self.MaxPosYaw:
yawValue = self.MaxPosYaw
if yawValue < self.MinPosYaw:
yawValue = self.MinPosYaw
#Envoi des commandes aux moteurs
try :
self.pwm.setPWM(self.channelPitch,0, int(pitchValue))
self.pwm.setPWM(self.channelYaw,0, int(yawValue))
except Exception as e:
self.logger.error('PWM servo command crashed : %s', e)
#initialize RG led Color
def initRG(self, channelRed, channelGreen):
# channels
self.channelRed = channelRed
self.channelGreen = channelGreen
# #update RGB led colors
def updateRGColor(self, R, G):
try:
self.pwm.setPWM(self.channelRed,0, int(R))
self.pwm.setPWM(self.channelGreen,0, int(G))
except Exception as err:
self.logger.error('PWM Led command crashed : %s',err)
# Fait clignoter la LED en rouge pendant le temps qu'elle prend en parametre
def RedTwinkle(self,deltaT):
i = 0
while i < int(deltaT / 2 + 1):
self.updateRGColor(4095,0) #LED rouge
time.sleep(1.5)
self.updateRGColor(0,0) #LED eteinte
time.sleep(0.5)
i+=1
def __del__(self):
# Release servo power on garbage collect
self.pwm.softwareReset()
