def motor(self):
#Instantiation of the servo object
self.servo = Servo(0x40)
#setting frequency for the PWM to interact with the PWM object
self.servo.set_frequency(self.frequencyValuse)
#setting the servo to move
self.servo.move(1, 125, 250)
def __init__(self, my_t0):
# Currently value
self.xdeg = 150
self.ydeg = 150
self.thetadeg = 150
self.x_amp = 50
self.y_amp = 50
self.th_amp = 50
self.set_robot(ergo_servo_config)
self.number_servos = len(self.servos)
print(" Number of servos = ", self.number_servos)
self.servo = Servo(0x40)
self.servo.output_enable()
self.servoMin = 180 # Min pulse length out of 4096
self.servoMed = 400 # Min pulse length out of 4096
self.servoMax = 500 # Max pulse length out of 4096
self.servo.set_low_limit(0.5) #1.0
self.servo.set_high_limit(2.0) #2.2
self.curr_pos = [0, 0, 0]
self.padding = 2
self.t0 = my_t0
self.variable_of_time_change = 3
print(" Servo Head on Control = Ok")
def init():
"""
Main program function
"""
# create an instance of the servo class on I2C address 0x40
global servo
servo = Servo(0x40)
# set the servo minimum and maximum limits in milliseconds
# the limits for a servo are typically between 1ms and 2ms.
servo.set_low_limit(0.5)
servo.set_high_limit(2.5)
# Enable the outputs
servo.output_enable()
class servoDriver(object):
servoFrequencyValue = 50
PWMFrequencyValue = 200
def __init__(self):
#creating objects for the PWM and Motor
self.pwm = pulseWidthModulation()
self.ServoMotor = motor()
def pulseWidthModulation(self):
#Pulse Width Modulation(PWM) instatiation
self.pwm = PWM(0x40)
#Sets the PWM to a frequency of 200 Hz
self.pwm.set_pwm_freq(self.PWMFrequencyValue)
#Allow the output to be displayed
self.pwm.output_enable()
#Setting the channel for data stream to 1, time period to 1024 out of 4095.
self.pwm.set_pwm(1, 0, 1024)
def motor(self):
#Instantiation of the servo object
self.servo = Servo(0x40)
#setting frequency for the PWM to interact with the PWM object
self.servo.set_frequency(self.frequencyValuse)
#setting the servo to move
self.servo.move(1, 125, 250)
def set_PWM_frequency(self, value):
self.PWMFrequencyValue = value
self.pwm.set_pwm_freq(self.PWMFrequencyValue)
def get_PWM_frequency(self):
return self.PWMFrequencyValue
def set_servo_frequency(self, value):
self.servoFrequencyValue = value
self.servo.set_frequency(self.servoFrequencyValue)
def get_servo_frequency(self):
return self.servoFrequencyValue
import time
try:
from ServoPi import Servo
except ImportError:
print("Failed to import ServoPi from python system path")
print("Importing from parent folder instead")
try:
import sys
sys.path.append("..")
from ServoPi import Servo
except ImportError:
raise ImportError("Failed to import library from parent folder")
# SETUP
servo = Servo(0x40)
servo.set_low_limit(0.6) #was 1.0
servo.set_high_limit(2.4) #was 2.0
servo.output_enable()
semaphore = {
'A': (4, 0),
'B': (4, 1),
'C': (4, 2),
'D': (4, 3),
'E': (4, 4),
'F': (3, 0),
'G': (3, 1),
'H': (3, 2),
'I': (3, 3),
'J': (3, 4),
class servo(object):
def __init__(self, my_t0):
# Currently value
self.xdeg = 150
self.ydeg = 150
self.thetadeg = 150
self.x_amp = 50
self.y_amp = 50
self.th_amp = 50
self.set_robot(ergo_servo_config)
self.number_servos = len(self.servos)
print(" Number of servos = ", self.number_servos)
self.servo = Servo(0x40)
self.servo.output_enable()
self.servoMin = 180 # Min pulse length out of 4096
self.servoMed = 400 # Min pulse length out of 4096
self.servoMax = 500 # Max pulse length out of 4096
self.servo.set_low_limit(0.5) #1.0
self.servo.set_high_limit(2.0) #2.2
self.curr_pos = [0, 0, 0]
self.padding = 2
self.t0 = my_t0
self.variable_of_time_change = 3
print(" Servo Head on Control = Ok")
def set_robot(self, servo_configuration):
self.servo_robot = servo_configuration
self.servos = list(self.servo_robot["motors"])
for key in self.servos:
item = self.servo_robot["motors"][key]
code = item['code']
#max_value_servo = item["max"]
#min_value_servo = item["min"]
#mid_value = (max_value_servo + min_value_servo)/2.0
mid_value = item["mid"]
amp_value_servo = item["amp"]
item["max"] = mid_value + amp_value_servo
item["min"] = mid_value - amp_value_servo
#print("el valor max es ", item["max"])
#print("el valor min es ", item["min"])
if code == 'xdeg':
self.xdeg = mid_value
self.x_amp = amp_value_servo
elif code == 'ydeg':
self.ydeg = mid_value
self.y_amp = amp_value_servo
elif code == 'thetadeg':
self.thetadeg = mid_value
self.th_amp = amp_value_servo
def get_deg_values(self):
return self.xdeg, self.ydeg, self.thetadeg
#*************************************************************************************************
#
# MAIN FUNCTION----> thread
#
#
#*************************************************************************************************
def run_normal(self): #this function is always running
print(" working servo head terminal......... everything OK")
t = time.time() - self.t0
if t > self.variable_of_time_change:
self.variable_of_time_change = t + int(
random.uniform(0, 1) * 10.0) + 3.0
self.random_pos()
def hor_move(self, my_value):
last_move = self.curr_pos
last_move[0] = my_value
self.motion_managent(last_move, 1)
time.sleep(.5)
def ver_move(self, my_value):
last_move = self.curr_pos
last_move[1] = my_value
self.motion_managent(last_move, 1)
time.sleep(.5)
def hor_ver_move(self, my_h_value, my_v_value):
last_move = self.curr_pos
last_move[0] = my_h_value
last_move[1] = my_v_value
self.motion_managent(last_move, 1)
time.sleep(.5)
def random_pos(self):
##pos = int(random.uniform(-1, 1)*15.0)
##pos = [int(1000*random.random()) for i in range(10000)]
##pos1 = [int(random.uniform(-1, 1)*15.0) for i in range(3)]
#a = [ 250,100,160 ] # esde es centro[[60,200,250],[75,100,140],[140,160, 200]]
a1 = int(random.uniform(-1, 1) * 90.0) + 170
a2 = int(random.uniform(0, 1) * 20.0) + 100
a = [a1, a2, 160]
print("Randon move to ....", a)
self.motion_managent(a, 1)
time.sleep(.5)
#_to_( a)
def motion_managent(self, movement, f=0.1):
#my_servos = list(self.servo_robot["motors"])
for key in self.servos:
item = self.servo_robot["motors"][key]
servo = item['servo']
max_value_servo = item["max"]
min_value_servo = item["min"]
my_move = movement[servo - 1]
if my_move < min_value_servo:
my_move = min_value_servo
if my_move > max_value_servo:
my_move = max_value_servo
#print("maximo alcanzado ", max_value_servo, " del servo ", servo)
#print(servo)
self.servo.move(servo, my_move)
self.curr_pos = movement
#time.sleep(f)
def _to_(self, to_):
t0 = time.time()
pulse = 0.05
from_ = self.curr_pos
for i in np.arange(0.0, 1.0, pulse):
temp = ((1.0 - i) * from_[0] + i * to_[0])
temp2 = ((1.0 - i) * from_[1] + i * to_[1])
temp3 = ((1.0 - i) * from_[2] + i * to_[2])
self.servo_array[0].angle = temp
self.servo_array[1].angle = temp2
self.servo_array[2].angle = temp3
sleep(pulse)
self.curr_pos = to_
def close(self):
pass
def kill(self):
p.stop()
#GPIO.cleanup()
def ejecuta(self,
texto="",
value=0,
value2=0
): # value2 es porque algunas ordenes necesitan dos argumentos
getattr(self, texto)()
def Hi(self):
print("Head order Hi")
a = [self.xdeg, self.ydeg - 50,
self.thetadeg] #self.xdeg , [ 140,75,170 ]
b = [self.xdeg, self.ydeg - 10, self.thetadeg] # [ 140,120,170 ]
c = [self.xdeg, self.ydeg - 30, self.thetadeg] #[ 140,90,170 ]
self.motion_managent(b, 0.2)
time.sleep(1)
self.motion_managent(a, 0.2)
time.sleep(1)
self.motion_managent(c, 0.2)
time.sleep(1)
def yes(self):
a = [self.xdeg, self.ydeg - 30, self.thetadeg] #[ 140,80,170 ]
b = [self.xdeg, self.ydeg, self.thetadeg] # [ 140,100,170 ]
n = random.randrange(5) + 3
#print(n)
while n > 0:
n -= 1
self.motion_managent(a, 0.2)
time.sleep(.1)
self.motion_managent(b, 0.2)
time.sleep(.1)
def no(self):
a = [self.xdeg - 40, self.ydeg, self.thetadeg] #[ 90,100,170 ]
b = [self.xdeg + 40, self.ydeg, self.thetadeg] # [ 170,100,170 ]
n = random.randrange(5) + 1
#print(n)
while n > 0:
n -= 1
self.motion_managent(a, 0.2)
time.sleep(.1)
self.motion_managent(b, 0.2)
time.sleep(.1)
return
self.motion_managent(a, 0.2)
time.sleep(.1)
self.motion_managent(b, 0.2)
time.sleep(.1)
self.motion_managent(a, 0.2)
time.sleep(.1)
self.motion_managent(b, 0.2)
time.sleep(.1)
print(".......................no....................")
import time
from ServoPi import Servo
servo = Servo(0x40)
servo.set_low_limit(0.6)
servo.set_high_limit(2.4)
servo.set_frequency(50)
while True:
position = int(input('Position:'))
servo.move(1, position, 180)
def main():
"""
Main program function
"""
# create an instance of the servo class on I2C address 0x40
servo = Servo(0x40)
# set the servo minimum and maximum limits in milliseconds
# the limits for a servo are typically between 1ms and 2ms.
servo.set_low_limit(1.0)
servo.set_high_limit(2.0)
# Enable the outputs
servo.output_enable()
# move the servo across its full range in increments of 10
while True:
for i in range(0, 250, 10):
servo.move(1, i)
time.sleep(0.05)
for i in range(250, 0, -10):
servo.move(1, i)
try:
from ServoPi import Servo
except ImportError:
print("Failed to import ServoPi from python system path")
print("Importing from parent folder instead")
try:
import sys
sys.path.append("..")
from ServoPi import Servo
except ImportError:
raise ImportError("Failed to import library from parent folder")
# My testing mods
servo = Servo(0x40)
servo.set_low_limit(1.0)
servo.set_high_limit(2.0)
servo.output_enable()
'''
def main():
"""
Main program function
"""
# create an instance of the servo class on I2C address 0x40
servo = Servo(0x40)
# set the servo minimum and maximum limits in milliseconds
# the limits for a servo are typically between 1ms and 2ms.
servo.set_low_limit(1.0)
def toggle_flap():
flap_toggling = True
servo = Servo(0x40)
# set the servo minimum and maximum limits in milliseconds
# the limits for a servo are typically between 1ms and 2ms.
servo.set_low_limit(1.0)
servo.set_high_limit(2.0)
# Enable the outputs
servo.output_enable()
servo.move(1, 250)
time.sleep(0.15)
servo.move(1, 0)
flap_toggling = False