def __init__(self, pwm_name, dir_a, dir_b, eqep):
# Initialize the thread base
super(controller, self).__init__()
# Create stop event object
self.stop_event = threading.Event()
self.stop_confirm = threading.Event()
# Store parameters
self.pwm = pwm_name
self.dir_a = dir_a
self.dir_b = dir_b
# Create a PID controller
self.pid = PID(0.01, 0.01, 0.0, -100.0, 100.0, 0.0)
# Load the eQEP interface
self.eqep = eQEP(eqep, eQEP.MODE_RELATIVE)
self.eqep.set_period(10000000)
# Setup the GPIO
gpio.setup(self.dir_a, gpio.OUT)
gpio.setup(self.dir_b, gpio.OUT)
gpio.output(self.dir_a, gpio.LOW)
gpio.output(self.dir_b, gpio.LOW)
# Setup the PWM
pwm.start(self.pwm, 100.0)
def moveUP(self):
GPIO.add_event_detect(motor.upperSW, GPIO.RISING, bouncetime=1000)
PWM.start(self.pwmPIN, self.duty, self.freq, 0)
GPIO.output(self.upDIR, GPIO.HIGH)
status = 1
print "Moving motor UP"
return 0, time.time()
def moveDOWN(self):
GPIO.add_event_detect(motor.lowerSW, GPIO.RISING, bouncetime=1000)
PWM.start(self.pwmPIN, self.duty, self.freq, 0)
GPIO.output(self.downDIR, GPIO.HIGH)
status = 0
print "Moving motor DOWN"
return 1, time.time()
def motor_setup(dir1_pin, dir2_pin, pwm_pin):
"""
Sets up context for operating a motor.
"""
# Initialize GPIO pins
GPIO.setup(dir1_pin, GPIO.OUT)
GPIO.setup(dir2_pin, GPIO.OUT)
# Initialize PWM pins: PWM.start(channel, duty, freq=2000, polarity=0)
PWM.start(pwm_pin, 0)
def run_motor(speed):
if speed > 100:
speed = 100
elif speed < -100:
speed = -100
if speed > 0:
GPIO.output(dir1_pin, GPIO.LOW)
GPIO.output(dir2_pin, GPIO.HIGH)
PWM.set_duty_cycle(pwm_pin, abs(speed))
elif speed < 0:
GPIO.output(dir1_pin, GPIO.HIGH)
GPIO.output(dir2_pin, GPIO.LOW)
PWM.set_duty_cycle(pwm_pin, abs(speed))
else:
GPIO.output(dir1_pin, GPIO.LOW)
GPIO.output(dir2_pin, GPIO.LOW)
PWM.set_duty_cycle(pwm_pin, 0)
yield run_motor
GPIO.cleanup()
PWM.cleanup()
def _set_low_speed(self, speed, enable_fan=True):
"""
Sets the speed of the fan in "low speed" mode
:param speed: The new desired speed from 0-:const:`LOW_SPEED`
:type speed: int
:param enable_fan: Whether to enable or disable the fan
:type enable_fan: bool
"""
ioloop = tornado.ioloop.IOLoop.instance()
period = float(100) / speed
if enable_fan:
# This should be 1 second but the spin-up takes half a second
timeout_len = 2
else:
timeout_len = period-1
PWM.start(self._blower_pin, 100 if enable_fan else 0, PWM_FREQUENCY, 0)
toggle_partial = functools.partial(
self._set_low_speed,
speed,
not enable_fan)
self._low_speed_handle = ioloop.add_timeout(
datetime.timedelta(seconds=timeout_len),
toggle_partial)
def update(self):
#
if ((millis(self.inittime) - self.timelastal) > 1500):
PWM.start(self.m1pin, self.getpercent(7.5, 50)
PWM.start(self.m2pin, self.getpercent(7.5, 50)
else:
PWM.start(self.m1pin, self.getpercent(self.m1val / 100.), 50)
PWM.start(self.m2pin, self.getpercent(self.m2val / 100.), 50)
#
def spin(self):
#
# rospy.Rate y sleep permiten que el nodo pare su ejecución (cuidado, los subscriptores
# NO están restringidos por el resto del nodo más allá de la función de callback) e
# intente ejecutarse cada tantos ms, dados por el rate declarado en el constructor de la
# clase. Ahorra recursos
r = rospy.Rate(self.rate)
#
while not rospy.is_shutdown():
self.update()
r.sleep()
#
# Invocación del constructor. Básicamente, si este archivo fuera a ejecutarse como "top level", el
# intérprete le asignaría el "nombre" (protegido) __main__ y ejecutaría las siguientes lineas como
# LO PRIMERO en el archivo (salvo imports y declaraciones)
def onclose():
PWM.start("P9_22", 7.5, 50)
PWM.start("P9_21", 7.5, 50)
if __name__ == '__main__':
""" main """
atexit.register(onclose())
servos = Servos()
servos.spin()
def turn_left():
global ENABLED, RUNNING
if not ENABLED:
return
PWM.start(LEFT_SERVO_TX, 98, 33, 1)
PWM.start(RIGHT_SERVO_TX, 98, 33, 1)
RUNNING = True
def __init__(self, baseIP, robotIP):
# Initialize GPIO pins
GPIO.setup(self.dir1Pin[LEFT], GPIO.OUT)
GPIO.setup(self.dir2Pin[LEFT], GPIO.OUT)
GPIO.setup(self.dir1Pin[RIGHT], GPIO.OUT)
GPIO.setup(self.dir2Pin[RIGHT], GPIO.OUT)
GPIO.setup(self.ledPin, GPIO.OUT)
# Initialize PWM pins: PWM.start(channel, duty, freq=2000, polarity=0)
PWM.start(self.pwmPin[LEFT], 0)
PWM.start(self.pwmPin[RIGHT], 0)
# Set motor speed to 0
self.setPWM([0, 0])
# Initialize ADC
ADC.setup()
self.encoderRead = encoderRead(self.encoderPin)
# Set IP addresses
self.baseIP = baseIP
self.robotIP = robotIP
self.robotSocket.bind((self.robotIP, self.port))
def moveDOWN(self):
PWM.start(self.pwmPIN, self.duty, self.freq, 0)
GPIO.output(self.downDIR, GPIO.HIGH)
status = 0
print "Moving motor DOWN"
time.sleep(1.5)
return status
def turn_on_pwm(pin):
PWM.start(pin, 50)
PWM.set_duty_cycle(pin, 25.5)
PWM.set_frequency(pin, 10)
print sys._getframe().f_code.co_name,
print(' '+pin)
raw_input('press a key to continue')
def __speaker(self):
for dir in [-1,2]:
for x in range(3,20):
PWM.start(self.SPEAKER_PIN, 50, 3000 + (dir * x * 100))
sleep(0.05)
PWM.start(self.SPEAKER_PIN, 0, 1)
return
def rotate(self, angle): PWM.start(channel,dutyCycle, PWM_freq) steps = 0 num_steps = angle / 1.846 time.sleep(stepTime*num_steps) PWM.stop(channel) return
def init(dev=(0,0),speed=4000000, brightness=256, contrast=CONTRAST):
if BITBANG:
for pin in [SCE, SCLK, DIN]:
GPIO.setup(pin, GPIO.OUT)
else:
spi.open(dev[0],dev[1])
#spi.max_speed_hz=speed
# Set pin directions.
for pin in [DC, RST]:
GPIO.setup(pin, GPIO.OUT)
# Toggle RST low to reset.
GPIO.output(RST, GPIO.LOW)
time.sleep(0.100)
GPIO.output(RST, GPIO.HIGH)
# Extended mode, bias, vop, basic mode, non-inverted display.
set_contrast(contrast)
# if LED == 1 set pin mode to PWM else set it to OUT
if LED == 1:
PWM.start(LED, 0)
else:
GPIO.setup(LED, GPIO.OUT)
GPIO.output(LED, GPIO.LOW)
def ignite(debuglevel):
threshold=340 #150mv threshold
threshigh=1024 #450mv upper threshold
ADC.setup()
ADC.read_raw("AIN4") #Flush this
baseline=ADC.read_raw("AIN4")
GPIO.setup("P8_14",GPIO.OUT) #This pin fires the ignition
GPIO.output("P8_14",GPIO.LOW)
PWM.start("P8_19",15,49500) #works best with an 11 turn primary
time.sleep(0.05) #50ms Settling time for the analogue front end
ADC.read_raw("AIN4")
selftest=ADC.read_raw("AIN4")
if selftest>threshold and selftest<threshigh and baseline<128:
GPIO.output("P8_14",GPIO.HIGH)
if debuglevel:
print "Igniting"
time.sleep(2) #plenty of time for ignition
failure=0
else:
if debuglevel:
print "Failed"
failure=1
GPIO.output("P8_14",GPIO.LOW)
PWM.stop("P8_19")
PWM.cleanup()
#Debug output
if debuglevel:
print baseline
print selftest
return {'failure':failure, 'baseline':baseline ,'selftest':selftest }
def start(self, speed, direction):
""" method to start a motor
Arguments:
speed : speed of the motor 0-100 (as percentage of max)
direction : CW or CCW, for clockwise or counterclockwise
"""
# Standby pin should go high to enable motion
GPIO.output(self.STBYpin, GPIO.HIGH)
# x01 and x02 have to be opposite to move, toggle to change direction
if direction in ('cw','CW','clockwise'):
GPIO.output(self.ControlPin1, GPIO.LOW)
GPIO.output(self.ControlPin2, GPIO.HIGH)
elif direction in ('ccw','CCW','counterclockwise'):
GPIO.output(self.ControlPin1, GPIO.HIGH)
GPIO.output(self.ControlPin2, GPIO.LOW)
else:
raise ValueError('Please enter CW or CCW for direction.')
# Start the motor
# PWM.start(channel, duty, freq=2000, polarity=0)
if 0 <= speed <= 100:
PWM.start(self.PWMpin, speed, self.PWMfreq)
else:
raise ValueError("Please enter speed between 0 and 100, \
representing a percentage of the maximum \
motor speed.")
# set the status attributes
self.isRunning = True
self.currentDirection = direction
self.currentSpeed = speed
def moveUP(self):
PWM.start(self.pwmPIN, self.duty, self.freq, 0)
GPIO.output(self.upDIR, GPIO.HIGH)
status = 1
print "Moving motor UP"
time.sleep(1.5)
return status
def stop():
global ENABLED, RUNNING
if not ENABLED:
return
PWM.start(LEFT_SERVO_TX, LEFT_SERVO_OFF, 33, 0)
PWM.start(RIGHT_SERVO_TX, RIGHT_SERVO_OFF, 33, 1)
RUNNING = False
def moveDOWN(self, device):
PWM.start(self.pwmPIN, self.duty, self.freq, 0)
GPIO.output(self.downDIR, GPIO.HIGH)
timer = time.time()
print "Moving motor %r DOWN" %device
#time.sleep(1.5)
return 0, timer # status
def __init__(self, motor_number=1, max_speed=100, min_speed=0):
self.speed = 0
# self.motor_pins_list = [["P9_14", "P9_16"],
# ["P8_19", "P8_13"],
# ["P9_22", "P9_21"],
# ["P9_42", "P9_28"]]
# self.motor_pins_list = ["P9_14", "P9_21","P9_22", "P9_42"]
self.motor_pins_list = ["P9_16", "P8_13", "P9_21", "P9_28"]
if (motor_number > 4) or (motor_number < 1):
raise Exception("Motor number provided out of bounds! ([1-4])")
self.motor_number = motor_number # 1, 2, 3 or 4
self.max_speed = max_speed
self.min_speed = min_speed
# self.frequency = 2000
self.frequency = 943
self.motor_pin = self.motor_pins_list[self.motor_number - 1]
# perform PWM initialization
# DC Brushed motors
# self.duty_IN1 = 0 # duty input 1 of the motor controller IC
# self.duty_IN2 = 0
# PWM.start(self.motor_pin[0], self.duty_IN1, self.frequency)
# PWM.start(self.motor_pin[1], self.duty_IN2, self.frequency)
# DC Brushless motors
self.duty = 0
PWM.start(self.motor_pin, self.duty, self.frequency)
def test_start_pwm_with_polarity_one(self):
PWM.cleanup()
PWM.start("P9_14", 0, 2000, 1)
pwm_dir = get_pwm_dir()
assert os.path.exists(pwm_dir)
if kernel >= '4.1.0':
duty = open(pwm_dir + '/duty_cycle').read()
else:
duty = open(pwm_dir + '/duty').read()
period = open(pwm_dir + '/period').read()
polarity = open(pwm_dir + '/polarity').read()
assert int(duty) == 0
assert int(period) == 500000
# TEMPORARY FIX: disable polarity check
# due to issue in 4.9.x+ kernels
# refer to issue #170:
# https://github.com/adafruit/adafruit-beaglebone-io-python/issues/170
# and commit c35e4cb from pull request #173:
# "source/c_pwm.c: disable pwm_set_polarity (broken in v4.9.x/v4.14.x)"
# https://github.com/adafruit/adafruit-beaglebone-io-python/pull/173/commits/c35e4cb98a1f14c85aca7259132bcc97e93d78f8
#if kernel >= '4.1.0':
# assert polarity == "inversed\n"
#else:
# assert int(polarity) == 1
PWM.cleanup()
def move_forward():
global ENABLED, RUNNING
if not ENABLED:
return
PWM.start(LEFT_SERVO_TX, 10, 33, 0)
PWM.start(RIGHT_SERVO_TX, 98, 33, 1)
RUNNING = True
def __init__(self, arm, axes):
self.arm = arm
self.polaraxes = [ PolarAxis(memp, pwm) for memp, pwm in axes ]
self.updatedutycount = 0
self.t0 = time.time()
if self.arm:
GPIO.setup(self.arm, GPIO.OUT)
GPIO.output(self.arm, GPIO.LOW)
for memp, pwm in axes:
if pwm:
print("starting", pwm)
PWM.start(pwm, 50, 100000, 1)
if self.arm:
GPIO.output(self.arm, GPIO.HIGH)
def settostationary(armaxes):
arm, axes = armaxes
if arm:
GPIO.output(arm, GPIO.LOW)
for memp, pwm in axes:
print("set stationary", pwm, arm)
if pwm:
PWM.start(pwm, 50, 100000, 1)
atexit.register(settostationary, (arm, axes)) # values kept here by closure
self.readeqeps()
def configure(self, config):
if not config:
raise PrinterError("No printer config given!")
self.config = config
printer_config = config['printer']
print_queue_config = printer_config["print-queue"]
self.print_queue_min_length = print_queue_config['min-length']
self.print_queue_max_length = print_queue_config['max-length']
self._homing_timeout = printer_config['homing-timeout']
self._default_homing_retraction = printer_config['home-retract']
self.default_speed = printer_config['default-speed']
# todo this is the fan and should be configured
PWM.start(self._FAN_OUTPUT, printer_config['fan-duty-cycle'], printer_config['fan-frequency'], 0)
if 'heated-bed' in printer_config:
bed_heater_config = printer_config['heated-bed']
self.heated_bed = self._configure_heater(bed_heater_config)
extruder_heater_config = config['extruder']['heater']
# we do not care if it the extruder heate may not be given in the config
# # - the whole point of additive printing is pretty dull w/o an heated extruder
self.extruder_heater = self._configure_heater(extruder_heater_config)
for axis_name, config_name in _axis_config.iteritems():
_logger.info("Configuring axis \'%s\' according to conf \'%s\'", axis_name, config_name)
axis = {'name': axis_name}
self.axis[axis_name] = axis
self._configure_axis(axis, config[config_name])
self._postconfig()
def turn_right():
global ENABLED, RUNNING
if not ENABLED:
return
PWM.start(LEFT_SERVO_TX, 0, 33, 0)
PWM.start(RIGHT_SERVO_TX, 5, 33, 0)
RUNNING = True
def servo():
rospy.init_node('servo', anonymous=True)
rospy.Subscriber("cmd_vel", Twist, callback)
PWM.start(servo_pin, duty_span * 0.5 + duty_min, 60.0)
rospy.spin()
def enable():
global ENABLED, RUNNING
if ENABLED:
return
PWM.start(LEFT_SERVO_TX, LEFT_SERVO_OFF, 33, 0)
PWM.start(RIGHT_SERVO_TX, RIGHT_SERVO_OFF, 33, 1)
ENABLED = True
RUNNING = False
def testp:
while 1:
PWM.start("P8_19",15,49500)
time.sleep(2)
PWM.stop("P8_19")
time.sleep(0.15)
PWM.cleanup()
time.sleep(0.1)
def start(self):
# Step 1: turn on 5v power line (will need to know GPIO pin)
print "Starting the servo! Turn on the 5v line"
# Step 2: start PWM
PWM.start(self.servo_pin, self.angle, 60.0)
# Step 3: set running flag to true
self.running = True
return
def triggerStepper(self):
time.sleep(0.5)
PWM.start("P9_16", 25, 100, 1)
time.sleep(2)
PWM.set_frequency("P9_16", 250)
time.sleep(90)
PWM.stop("P9_16")
PWM.cleanup()
def main():
pin = "P9_14"
PWM.start(pin, 7.5, 50, 1)
keys_map = {"l": left, "r": right}
while 1:
left(pin)
right(pin)
time.sleep(1)
def main():
# Arm the motor
PWM.start("P1_33", 0, 100) # Initialize all the pins with 0 duty cycle
PWM.start("P1_36", 0, 100)
PWM.start("P2_1", 0, 100)
PWM.start("P2_3", 0, 100)
time.sleep(1.0) # Hold for one second
PWM.set_duty_cycle("P1_33", 10) # Start the pins a a l;ow duty cycle
PWM.set_duty_cycle("P1_36", 10)
PWM.set_duty_cycle("P2_1", 10)
PWM.set_duty_cycle("P2_3", 10)
time.sleep(1.0)
# Run the motor for 5 seconds
PWM.set_duty_cycle("P1_33", 11) # Run the pins at high duty cycle to
PWM.set_duty_cycle("P1_36", 11) # start the motors
PWM.set_duty_cycle("P2_1", 11)
PWM.set_duty_cycle("P2_3", 11)
time.sleep(1.0)
PWM.set_duty_cycle("P1_33", 14) # Ramp up motors for power/current test
PWM.set_duty_cycle("P1_36", 14)
PWM.set_duty_cycle("P2_1", 14)
PWM.set_duty_cycle("P2_3", 14)
time.sleep(5.0)
PWM.stop("P1_33") # Stop PWM signals and clean pins
PWM.stop("P1_36")
PWM.stop("P2_1")
PWM.stop("P2_3")
PWM.cleanup
def open_door():
"""Makes the servo open the dispensing door and close it again."""
PWM.start(servo_pin, (100), 20.0)
PWM.set_duty_cycle(servo_pin, 1.5)
time.sleep(2.0)
PWM.start(servo_pin, (100), 20.0)
PWM.set_duty_cycle(servo_pin, 3.5)
time.sleep(1.0)
PWM.stop(servo_pin)
PWM.cleanup()
def start():
GPIO.setup( MY_PWM_DIR_PIN_1, GPIO.OUT)
GPIO.output(MY_PWM_DIR_PIN_1, GPIO.LOW)
GPIO.setup( MY_PWM_DIR_PIN_2, GPIO.OUT)
GPIO.output(MY_PWM_DIR_PIN_2, GPIO.LOW)
GPIO.setup( MY_PWM_STBY, GPIO.OUT)
GPIO.output(MY_PWM_STBY, GPIO.LOW) # stby low = off
# start PWM
PWM.start(MY_PWM_PIN,0,50000) # duty cycle 0%, freq 1000HZ.
GPIO.output(MY_PWM_STBY, GPIO.HIGH)
def test_start_pwm_ecap0(self):
print("test_start_pwm_ecap0\n");
PWM.cleanup()
PWM.start("P9_42", 0)
pwm_dir = get_pwm_dir()
assert os.path.exists(pwm_dir)
if kernel >= '4.1.0':
duty = open(pwm_dir + '/duty_cycle').read()
else:
duty = open(pwm_dir + '/duty').read()
period = open(pwm_dir + '/period').read()
assert int(duty) == 0
assert int(period) == 500000
PWM.cleanup()
def __init__(self, pin_forward, pin_backward):
""" Initialize the motor with its control pins and start pulse-width
modulation """
self.pin_forward = pin_forward
self.pin_backward = pin_backward
self.reverse_delay = config.delay
GPIO.setup(self.pin_forward, GPIO.OUT)
GPIO.setup(self.pin_backward, GPIO.OUT)
GPIO.output(self.pin_forward, GPIO.LOW)
GPIO.output(self.pin_backward, GPIO.LOW)
PWM.start(self.pin_forward, 0.0)
PWM.start(self.pin_backward, 0.0)
def send_encoder():
global distance
pub = rospy.Publisher('l_en_num', Int64, queue_size=20)
rospy.init_node('laser_motor', anonymous=True)
rate = rospy.Rate(100)
PWM.start("P9_16", 20, 50, 0)
GPIO.output("P9_12", GPIO.LOW)
while not rospy.is_shutdown():
msg = Int64()
msg.data = distance
#int64(encoder_distance = distance)
rospy.loginfo(msg.data)
pub.publish(msg.data)
rate.sleep()
def tuneThread(self, tune):
for t in tune:
if t[0] == 0:
t[0] = 20000
try:
if self.duty > 0:
GPIO.output(self.sd_pin, GPIO.LOW)
PWM.start(self.pwm_pin, 70 - self.duty * 0.65, t[0])
print(self.duty)
else:
GPIO.output(self.sd_pin, GPIO.HIGH)
except:
pass
time.sleep(t[1])
def blink(led, timer):
if led == 0:
GPIO.output(led0, GPIO.HIGH)
PWM.start(BUZZER, 50, NOTES[0])
time.sleep(timer)
PWM.stop(BUZZER)
GPIO.output(led0, GPIO.LOW)
elif led == 1:
GPIO.output(led1, GPIO.HIGH)
PWM.start(BUZZER, 50, NOTES[1])
time.sleep(timer)
PWM.stop(BUZZER)
GPIO.output(led1, GPIO.LOW)
elif led == 2:
GPIO.output(led2, GPIO.HIGH)
PWM.start(BUZZER, 50, NOTES[2])
time.sleep(timer)
PWM.stop(BUZZER)
GPIO.output(led2, GPIO.LOW)
else:
GPIO.output(led3, GPIO.HIGH)
PWM.start(BUZZER, 50, NOTES[3])
time.sleep(timer)
PWM.stop(BUZZER)
GPIO.output(led3, GPIO.LOW)
time.sleep(timer)
def run(self):
try:
LetterWriterThingy.display_setup()
LetterWriterThingy.ada_setup()
PWM.start(self.motorPin, (100 - self.motor_duty_min),
self.motor_pwm_frequency)
while True:
#self.set_motor_speed()
time.sleep(self.motor_update_time)
self.hexDisplay()
except KeyboardInterrupt: #Control c to end the program
PWM.stop(self.motorPin)
return
def rearright(rrcmd):
rate=6.8461703882*abs(rrcmd.velocity[0]) + 4.7414065119 #emperically determined
if rate > 99.9: #truncate PWM if velocity is too high
rate = 100
if rrcmd.velocity[0] == 0:
rate = 0
PWM.start("P8_19", 50)
if rrcmd.velocity[0] > 0:
#counterclockwise motion (backward)
GPIO.output("P8_10", GPIO.HIGH)
GPIO.output("P8_11", GPIO.LOW)
if rrcmd.velocity[0] < 0:
#clockwise motion (forward)
GPIO.output("P8_10", GPIO.LOW)
GPIO.output("P8_11", GPIO.HIGH)
def sendArmCommand(jtarget):
angle1 = jtarget.joint1
angle2 = jtarget.joint2
#test code
#angle1= 90.0
#angle2 = 0.0
rospy.loginfo("Received angle1 =%f, angle2 =%f", angle1, angle2)
#calculate the proper duty cycle here
dutycyc1 = MINDUT + (MAXDUT - MINDUT) * (angle1 / JOINT1MAX) #2.5 to 12.5
rospy.loginfo("Duty cycle set to %f", dutycyc1)
dutycyc2 = MINDUT + (MAXDUT - MINDUT) * (angle2 / JOINT2MAX
) #test different duty cycles
#PWM.start("PIN",dutycycle,frequency,polarity(1 is inverted))
PWM.start("P9_14", dutycyc1, SERVO_FREQUENCY, 0) #joint 1
PWM.start("P9_16", dutycyc2, SERVO_FREQUENCY, 0) #joint 2
def __init__(self, pwm_pin, dir1_pin, dir2_pin, max_speed=100):
"""
"""
self.speed = 0
self.max_speed = max_speed
self._pwm_pin = pwm_pin
self._dir1_pin = dir1_pin
self._dir2_pin = dir2_pin
GPIO.setup(self._dir1_pin, GPIO.OUT)
GPIO.setup(self._dir2_pin, GPIO.OUT)
PWM.start(self._pwm_pin, 0)
PWM.set_duty_cycle(self._pwm_pin, 0)
def set_pwm(pin):
"""Configure la pin en sortie PWM.
"""
# Vérifie si la pin est déjà configurée
if pins[pin] is not None and pins[pin] != "pwm":
logging.error("pins > set_pwm: {} est déjà configurée en '{}'".format(
pin, pins[pin]))
else:
pins[pin] = "pwm"
# Démarre le PWM avec un "duty cycle" de 0%
PWM.start(pin, 0)
logging.info("pins > set_pwm: {} est configurée en '{}'".format(
pin, pins[pin]))
def __init__(self):
# Right side motors. Pin definitions for H-bridge control
self.right_wheel = "P9_16" # controls speed of right front motor via PWM
self.right_wheel_dir = "P9_12" # combine M1 pins on upper and lower motor controllers into a single pin
# Left side motors. Pin definitions for H-bridge control
self.left_wheel = "P9_14" # controls speed of left front motor via PWM
self.left_wheel_dir = "P8_11" # combine M2 pins on upper and lower motor controllers into a single pin
# set pin directions as output
GPIO.setup(self.right_wheel, GPIO.OUT)
GPIO.setup(self.right_wheel_dir, GPIO.OUT)
GPIO.setup(self.left_wheel, GPIO.OUT)
GPIO.setup(self.left_wheel_dir, GPIO.OUT)
PWM.start(self.left_wheel, 0)
PWM.start(self.right_wheel, 0)
def set_pwm(
self, pwml, pwmr
): #if pwml<0 pwmr>0, turn right, otherwise left, abs(pwml/r) decide speed
# Check bounds
if pwml > 40:
pwml = 40
elif pwml < -40:
pwml = -40
if pwmr > 40:
pwmr = 40
elif pwmr < -40:
pwmr = -40
print 'setting pwm = ', pwml, ', ', pwmr
PWM.start(
self.leftMotorPinList[2], 0
) #what's the information in the left/rightMotorPinList? Ans:4 switch signals for H bridge
PWM.start(self.rightMotorPinList[2],
0) #what's the function of third Pinlish?
# set directions
#check if left motor is to be negative pwm
if pwml < 0:
# inputs for backward motion of left motor
GPIO.output(self.leftMotorPinList[1], GPIO.LOW)
GPIO.output(self.leftMotorPinList[0], GPIO.HIGH)
else:
# inputs for forward motion of left motor
GPIO.output(self.leftMotorPinList[1], GPIO.HIGH)
GPIO.output(self.leftMotorPinList[0], GPIO.LOW)
if pwmr < 0:
# inputs for backward motion of right motor
GPIO.output(self.rightMotorPinList[1], GPIO.LOW)
GPIO.output(self.rightMotorPinList[0], GPIO.HIGH)
else:
# inputs for forward motion of right motor
GPIO.output(self.rightMotorPinList[1], GPIO.HIGH)
GPIO.output(self.rightMotorPinList[0], GPIO.LOW)
#make robot stop
if pwml == 0 and pwmr == 0:
# self.stop()
GPIO.output(self.rightMotorPinList[1], GPIO.LOW)
GPIO.output(self.rightMotorPinList[0], GPIO.LOW)
# set absolute values of pwm for both motors
PWM.set_duty_cycle(self.leftMotorPinList[2], abs(pwml)) # left motor
PWM.set_duty_cycle(self.rightMotorPinList[2], abs(pwmr)) # right motor
def test_start_pwm(self):
PWM.start("P9_14", 0)
files = os.listdir('/sys/devices')
ocp = '/sys/devices/' + [s for s in files if s.startswith('ocp')][0]
files = os.listdir(ocp)
pwm_test = ocp + '/' + [
s for s in files if s.startswith('pwm_test_P9_14')
][0]
assert os.path.exists(pwm_test)
duty = open(pwm_test + '/duty').read()
period = open(pwm_test + '/period').read()
assert int(duty) == 0
assert int(period) == 500000
PWM.cleanup()
def initMotor(self):
print "Initializing DG012-SV motors.."
time.sleep(0.1)
DIO.setup(M1, DIO.OUT)
DIO.setup(M2, DIO.OUT)
DIO.setup(M3, DIO.OUT)
DIO.setup(M4, DIO.OUT)
DIO.setup(LMOT_PWM, DIO.OUT)
DIO.setup(RMOT_PWM, DIO.OUT)
PWM.start(LMOT_PWM, 0)
PWM.start(RMOT_PWM, 0)
return
def gotoLeft(): #turn on 2nd motor (right motor) #set pins high/low # 1a high and 2a low =right # 1a low and 2a high=left # both high/both low =stop GPIO.output(input_1A,GPIO.LOW) GPIO.output(input_2A,GPIO.LOW) GPIO.output(input_3A,GPIO.HIGH) GPIO.output(input_4A,GPIO.LOW) duty=30 PWM.start(motor2, duty) sleep(0.2) PWM.stop(motor2) #disable pwm immediately to rotate few angles PWM.cleanup()
def start(PWM_freq=1e7, PWM_duty_cycle=50, clock_divider=2):
GPIO.setup("P8_30", GPIO.OUT) #SPI CS for AWG
GPIO.output("P8_30", GPIO.HIGH) #Set CS high initally - i.e. disabled
GPIO.setup("P9_23",
GPIO.OUT) #AFG External trigger. NB check JP1 setting too!
GPIO.setup(
"P9_24", GPIO.OUT
) #What is this for? Possibly old code for the old SPI non bit banged bus?
PWM.start(
"P8_34", PWM_duty_cycle, PWM_freq
) # setup ADC master clock 13e6 = 25khz ~ NCO sees 50MHz clock - connects to J10 on AFG
# NCO removed P8_46 because of a device tree conflict with SPI_overlay_PRU1
# PWM.start("P8_46",50,50e6) # 100e5
pypruss.modprobe() # This only has to be called once pr boot
def unlock(self):
"""Unlock the lock.
- Turn off red LED; Turn on green LED
- Set servo to open
- Set display to "----"
"""
# Set LEDs
GPIO.output(self.red_led, GPIO.LOW)
GPIO.output(self.green_led, GPIO.HIGH)
# Set servo
PWM.start(self.servo, SG90_OPEN, SG90_FREQ, SG90_POL)
# Set display to dash
self.set_display_dash()
def dim_current_and_move_next(self):
duty_cycle = 0.0
PWM.start(self.current_pwm(), duty_cycle, 1000, 1)
while duty_cycle <= 100.0:
self.key_pressed = self.screen.getch()
if self.key_pressed == ord(QUIT_CHARACTER):
PWM.stop(self.current_pwm())
return
PWM.set_duty_cycle(self.current_pwm(), duty_cycle)
curses.napms(DIM_DELAY_MS)
duty_cycle += DUTY_CYCLE_INCREMENT
PWM.stop(self.current_pwm())
self.move_next()
def player_turn():
global score
time_start = time.time()
time_end = time.time()
bad_move = False
good_move = 0
index = 0
while ((time_end - time_start) <
current_round + 3) and (not bad_move) and good_move < current_round:
if GPIO.input(button0):
if game_array[index] != 0:
bad_move = True
else:
good_move += 1
index += 1
PWM.start(BUZZER, 50, NOTES[0])
time.sleep(0.5)
PWM.stop(BUZZER)
if GPIO.input(button1):
if game_array[index] != 1:
bad_move = True
else:
good_move += 1
index += 1
PWM.start(BUZZER, 50, NOTES[1])
time.sleep(0.5)
PWM.stop(BUZZER)
if GPIO.input(button2):
if game_array[index] != 2:
bad_move = True
else:
good_move += 1
index += 1
PWM.start(BUZZER, 50, NOTES[2])
time.sleep(0.5)
PWM.stop(BUZZER)
if GPIO.input(button3):
if game_array[index] != 3:
bad_move = True
else:
good_move += 1
index += 1
PWM.start(BUZZER, 50, NOTES[3])
time.sleep(0.5)
PWM.stop(BUZZER)
time_end = time.time()
if good_move == current_round:
score += 1
lcd.lcd_display_string("Current Score: " + str(score), 1)
return True
else:
return False
def main():
PWM.start(servoPinObjetos, 3, 50)
PWM.start(servoPinCopos, 5, 50)
tirarFoto()
#(T, L)
cor = analisarPixels(T_FOTO, L_FOTO)
#BGR
if cor == 1:
print "Eh azul!"
Motor_Copos(POSICAO_AZUL)
time.sleep(3)
Motor_Liberar_Objeto(LIBERAR)
time.sleep(3)
Motor_Liberar_Objeto(FECHAR)
elif cor == 2:
print "Eh verde!"
Motor_Copos(POSICAO_VERDE)
time.sleep(3)
Motor_Liberar_Objeto(LIBERAR)
time.sleep(3)
Motor_Liberar_Objeto(FECHAR)
elif cor == 3:
print "Eh vermelho!"
Motor_Copos(POSICAO_VERMELHO)
time.sleep(3)
Motor_Liberar_Objeto(LIBERAR)
time.sleep(3)
Motor_Liberar_Objeto(FECHAR)
def callback(data):
#GPIO.setup(outPin,GPIO.OUT)
PWM.start(outPWM, 0, 1000)
for i in range(0, 3):
#GPIO.output(outPin, GPIO.LOW)
#sleep(3)
print data.pointdata.x
V = data.pointdata.x
DC = V / 3.365 * 10
if DC > 100:
DC = 100
PWM.set_duty_cycle(outPWM, DC)
sleep(3)
#GPIO.output(outPin, GPIO.HIGH)
#sleep(3)
PWM.stop(outPWM)
PWM.cleanup()
def _setup(self):
"""Setup the hardware components."""
# Initialize Display
self.set_display_dash()
# Initialize Button
GPIO.setup(self.button, GPIO.IN)
# Initialize LEDs
GPIO.setup(self.red_led, GPIO.OUT)
GPIO.setup(self.green_led, GPIO.OUT)
# Initialize Analog Input
ADC.setup()
# Initialize Servo; Servo should be "off"
PWM.start(self.servo, SG90_OFF, SG90_FREQ, SG90_POL)
def __init__(self, pin_dir, pin_en):
print "Initializing motor driver"
self._pin_dir = pin_dir
self._pin_en = pin_en
# First, turn off the enable pin
#PWM.start(channel, duty, freq=200, polarity=0)
PWM.start(self._pin_en, 0)
# I'm concerned about the enable pin not being stable when we startup.
# For the sake of safety give it a moment to stop.
time.sleep(0.5)
# Now set the direction pin as an output and initialize it
GPIO.setup(self._pin_dir, GPIO.OUT)
GPIO.output(self._pin_dir, GPIO.LOW)
print "Motor driver init complete"
def start():
# Starting servo controller
global servo
servo = Servo() # using default values
servo.start()
# Start motor control
global forward
global reverse
global PWM
forward = "P9_14"
reverse = "P9_22"
PWM.start(forward, 0, 200)
PWM.start(reverse, 0, 200)
# initializing the servo
init_servo()
# Subscribe to ROS Joy node
rospy.Subscriber("joy", Joy, callback)
rospy.init_node("Joy2Servo")
rospy.spin()
def run_servo():
setup()
print("Servo Control Program Start")
try:
while True:
PWM.start(SERVO_OUTPUT, (100 - servo_duty_min),
servo_pwm_frequency)
"""Uses the span of the servo duty: max - min duty"""
set_servo_angle(ANALOG_INPUT, SERVO_OUTPUT)
time.sleep(1)
PWM.stop(SERVO_OUTPUT)
time.sleep(servo_update_time
) #Not necessary to make computer work at max speed
cleanup()
except KeyboardInterrupt:
PWM.stop(SERVO_OUTPUT)
print("Servo Control Program Finished")
def goBackward(): #set pins high/low # 1a high and 2a low =right # 1a low and 2a high=left # both high/both low =stoP GPIO.output(input_1A,GPIO.LOW) GPIO.output(input_2A,GPIO.HIGH) GPIO.output(input_3A,GPIO.LOW) GPIO.output(input_4A,GPIO.HIGH) duty=30 PWM.start(motor1, duty) PWM.start(motor2, duty) sleep(0.4) PWM.stop(motor1) #disable pwm immediately to rotate few angles PWM.stop(motor2) PWM.cleanup()
def __init__(self, *vars, **kwargs):
# PWM1 PINS
self.pwm_pin = kwargs.pop('pwm_pin', 'P9_14')
self.dir_A = kwargs.pop('dir_A', 'P9_15')
self.dir_B = kwargs.pop('dir_B', 'P9_23')
self.duty_cycle = kwargs.pop('duty_cycle', 60)
self.enable_pin = kwargs.pop('enable_pin', None)
# call super
super().__init__(*vars, **kwargs)
# initialize pwm1
PWM.start(self.pwm_pin)
PWM.set_duty_cycle(self.pwm_pin, self.duty_cycle)
GPIO.setup(self.dir_A, GPIO.OUT)
GPIO.setup(self.dir_B, GPIO.OUT)
if self.enable_pin:
GPIO.setup(self.enable_pin, GPIO.OUT)
