def P0(): # Process 0 controlles Pan servo
speed = .1 # Here we set some defaults:
RollCP = initroll - 1 # by making the current position and desired position unequal,-
RollDP = initroll # we can be sure we know where the servo really is. (or will be soon)
while True:
time.sleep(speed)
if RollCPQ.empty(
): # Constantly update RollCPQ in case the main process needs-
RollCPQ.put(RollCP) # to read it
if not RollDPQ.empty(
): # Constantly read read RollDPQ in case the main process-
RollDP = RollDPQ.get() # has updated it
if not RollSQ.empty(
): # Constantly read read RollSQ in case the main process-
RollS = RollSQ.get(
) # has updated it, the higher the speed value, the shorter-
speed = .1 / RollS # the wait between loops will be, so the servo moves faster
if RollCP < RollDP: # if RollCPQ less than RollDPQ
RollCP += 1 # incriment RollCPQ up by one
RollCPQ.put(RollCP) # move the servo that little bit
PWM.clear_channel_gpio(0, Roll)
PWM.add_channel_pulse(0, Roll, 0, RollCP)
if not RollCPQ.empty(): # throw away the old RollCPQ value,-
trash = RollCPQ.get() # it's no longer relevent
if RollCP > RollDP: # if RollCPQ greater than ServoPanDP
RollCP -= 1 # incriment RollCPQ down by one
RollCPQ.put(RollCP) # move the servo that little bit
PWM.clear_channel_gpio(0, Roll)
PWM.add_channel_pulse(0, Roll, 0, RollCP)
if not RollCPQ.empty(): # throw away the old ROllPanCPQ value,-
trash = RollCPQ.get() # it's no longer relevent
if RollCP == RollDP: # if all is good,-
RollS = 1 # slow the speed; no need to eat CPU just waiting
def __init__(self):
"""Initialise movement."""
PWM.setup()
#get rid of debug output
PWM.set_loglevel(PWM.LOG_LEVEL_ERRORS)
#pins
#i2c pins can be enabled high at boot - see http://www.raspberrypi.org/phpBB3/viewtopic.php?f=44&t=35321
self._l_enable_pin = 4
self._l_forward_pin = 3
self._l_backward_pin = 2
self._r_enable_pin = 17
self._r_forward_pin = 27
self._r_backward_pin = 22
#constants
self.LEFT = 1
self.RIGHT = 2
#setup the pins
RPIO.setup(self._l_forward_pin, RPIO.OUT)
RPIO.setup(self._r_forward_pin, RPIO.OUT)
RPIO.setup(self._l_backward_pin, RPIO.OUT)
RPIO.setup(self._r_backward_pin, RPIO.OUT)
#pwm setup
self._dma_l = 0
self._dma_r = 1
PWM.init_channel(self._dma_l)
PWM.init_channel(self._dma_r)
#this is silly, but otherwise pwm will complain if we try and clear a channel that hasn't been already used
PWM.add_channel_pulse(self._dma_l,self._l_enable_pin,0,0)
PWM.add_channel_pulse(self._dma_r,self._r_enable_pin,0,0)
def P0(): # Process 0 controlles Pan servo
speed = .1 # Here we set some defaults:
_ServoPanCP = initPan - 1 # by making the current position and desired position unequal,-
_ServoPanDP = initPan # we can be sure we know where the servo really is. (or will be soon)
while True:
time.sleep(speed)
if ServoPanCP.empty(
): # Constantly update ServoPanCP in case the main process needs-
ServoPanCP.put(_ServoPanCP) # to read it
if not ServoPanDP.empty(
): # Constantly read read ServoPanDP in case the main process-
_ServoPanDP = ServoPanDP.get() # has updated it
if not ServoPanS.empty(
): # Constantly read read ServoPanS in case the main process-
_ServoPanS = ServoPanS.get(
) # has updated it, the higher the speed value, the shorter-
speed = .1 / _ServoPanS # the wait between loops will be, so the servo moves faster
if _ServoPanCP < _ServoPanDP: # if ServoPanCP less than ServoPanDP
_ServoPanCP += 1 # incriment ServoPanCP up by one
ServoPanCP.put(_ServoPanCP) # move the servo that little bit
PWM.clear_channel_gpio(0, pPan)
PWM.add_channel_pulse(0, pPan, 0, _ServoPanCP)
if not ServoPanCP.empty(): # throw away the old ServoPanCP value,-
trash = ServoPanCP.get() # it's no longer relevent
if _ServoPanCP > _ServoPanDP: # if ServoPanCP greater than ServoPanDP
_ServoPanCP -= 1 # incriment ServoPanCP down by one
ServoPanCP.put(_ServoPanCP) # move the servo that little bit
PWM.clear_channel_gpio(0, pPan)
PWM.add_channel_pulse(0, pPan, 0, _ServoPanCP)
if not ServoPanCP.empty(): # throw away the old ServoPanCP value,-
trash = ServoPanCP.get() # it's no longer relevent
if _ServoPanCP == _ServoPanDP: # if all is good,-
_ServoPanS = 1 # slow the speed; no need to eat CPU just waiting
def setMotor(self,channel,speed):
if(channel=='A'):
print "set channel A PWM to "+str(speed);
PWM.add_channel_pulse(self.A_CHANNEL,self.AENBL_Pin,0,speed);
if(channel=='B'):
print "set channel B PWM to "+str(speed);
PWM.add_channel_pulse(self.B_CHANNEL,self.BENBL_Pin,0,speed);
def __init__(self, pin, location, rotation, name): #--------------------------------------------------------------------------- # The GPIO BCM numbered pin providing PWM signal for this ESC #--------------------------------------------------------------------------- self.bcm_pin = pin #--------------------------------------------------------------------------- # The location on the quad, and the direction of the motor controlled by this ESC #--------------------------------------------------------------------------- self.motor_location = location self.motor_rotation = rotation #--------------------------------------------------------------------------- # The PWM pulse width range required by this ESC in microseconds #--------------------------------------------------------------------------- self.min_pulse_width = 1000 self.max_pulse_width = 2000 #--------------------------------------------------------------------------- # The PWM pulse range required by this ESC #--------------------------------------------------------------------------- self.current_pulse_width = self.min_pulse_width self.name = name #--------------------------------------------------------------------------- # Initialize the RPIO DMA PWM #--------------------------------------------------------------------------- if not PWM.is_setup(): PWM.set_loglevel(PWM.LOG_LEVEL_ERRORS) PWM.setup(1) # 1us increment PWM.init_channel(RPIO_DMA_CHANNEL, 3000) # 3ms carrier period PWM.add_channel_pulse(RPIO_DMA_CHANNEL, self.bcm_pin, 0, self.current_pulse_width)
def P0(): # Process 0 controlles Pan servo speed = .1 # Here we set some defaults: RollCP = initroll - 1 # by making the current position and desired position unequal,- RollDP = initroll # we can be sure we know where the servo really is. (or will be soon) while True: time.sleep(speed) if RollCPQ.empty(): # Constantly update RollCPQ in case the main process needs- RollCPQ.put(RollCP) # to read it if not RollDPQ.empty(): # Constantly read read RollDPQ in case the main process- RollDP = RollDPQ.get() # has updated it if not RollSQ.empty(): # Constantly read read RollSQ in case the main process- RollS = RollSQ.get() # has updated it, the higher the speed value, the shorter- speed = .1 / RollS # the wait between loops will be, so the servo moves faster if RollCP < RollDP: # if RollCPQ less than RollDPQ RollCP += 1 # incriment RollCPQ up by one RollCPQ.put(RollCP) # move the servo that little bit PWM.clear_channel_gpio(0, Roll) PWM.add_channel_pulse(0, Roll, 0, RollCP) if not RollCPQ.empty(): # throw away the old RollCPQ value,- trash = RollCPQ.get() # it's no longer relevent if RollCP > RollDP: # if RollCPQ greater than ServoPanDP RollCP -= 1 # incriment RollCPQ down by one RollCPQ.put(RollCP) # move the servo that little bit PWM.clear_channel_gpio(0,Roll) PWM.add_channel_pulse(0, Roll, 0, RollCP) if not RollCPQ.empty(): # throw away the old ROllPanCPQ value,- trash = RollCPQ.get() # it's no longer relevent if RollCP == RollDP: # if all is good,- RollS = 1 # slow the speed; no need to eat CPU just waiting
def _set_led(self, pin, pwm, val):
LOG.debug('set LED on pin %d to %s', pin, val)
if val == 0 or val == 1:
PWM.clear_channel(pwm)
GPIO.output(pin, 1 - int(val))
else:
PWM.add_channel_pulse(pwm, pin, 0, 1999 - int(1999 * val))
def P1(): # Process 1 controlles Tilt servo using same logic as above speed = .1 _ServoTiltCP = initTilt - 1 _ServoTiltDP = initTilt while True: time.sleep(speed) if ServoTiltCP.empty(): ServoTiltCP.put(_ServoTiltCP) if not ServoTiltDP.empty(): _ServoTiltDP = ServoTiltDP.get() if not ServoTiltS.empty(): _ServoTiltS = ServoTiltS.get() speed = .1 / _ServoTiltS if _ServoTiltCP < _ServoTiltDP: _ServoTiltCP += 1 ServoTiltCP.put(_ServoTiltCP) PWM.clear_channel_gpio(0, pTilt) PWM.add_channel_pulse(0, pTilt, 0, _ServoTiltCP) if not ServoTiltCP.empty(): trash = ServoTiltCP.get() if _ServoTiltCP > _ServoTiltDP: _ServoTiltCP -= 1 ServoTiltCP.put(_ServoTiltCP) PWM.clear_channel_gpio(0, pTilt) PWM.add_channel_pulse(0, pTilt, 0, _ServoTiltCP) if not ServoTiltCP.empty(): trash = ServoTiltCP.get() if _ServoTiltCP == _ServoTiltDP: _ServoTiltS = 1
def P0(): # Process 0 controlles Pan servo
speed = .1 # Here we set some defaults:
_ServoPanCP = initPan - 1 # by making the current position and desired position unequal,-
_ServoPanDP = initPan # we can be sure we know where the servo really is. (or will be soon)
while True:
time.sleep(speed)
if ServoPanCP.empty(): # Constantly update ServoPanCP in case the main process needs-
ServoPanCP.put(_ServoPanCP) # to read it
if not ServoPanDP.empty(): # Constantly read read ServoPanDP in case the main process-
_ServoPanDP = ServoPanDP.get() # has updated it
if not ServoPanS.empty(): # Constantly read read ServoPanS in case the main process-
_ServoPanS = ServoPanS.get() # has updated it, the higher the speed value, the shorter-
speed = .1 / _ServoPanS # the wait between loops will be, so the servo moves faster
if _ServoPanCP < _ServoPanDP: # if ServoPanCP less than ServoPanDP
_ServoPanCP += 1 # incriment ServoPanCP up by one
ServoPanCP.put(_ServoPanCP) # move the servo that little bit
PWM.clear_channel_gpio(0, pPan)
PWM.add_channel_pulse(0, pPan, 0, _ServoPanCP)
if not ServoPanCP.empty(): # throw away the old ServoPanCP value,-
trash = ServoPanCP.get() # it's no longer relevent
if _ServoPanCP > _ServoPanDP: # if ServoPanCP greater than ServoPanDP
_ServoPanCP -= 1 # incriment ServoPanCP down by one
ServoPanCP.put(_ServoPanCP) # move the servo that little bit
PWM.clear_channel_gpio(0, pPan)
PWM.add_channel_pulse(0, pPan, 0, _ServoPanCP)
if not ServoPanCP.empty(): # throw away the old ServoPanCP value,-
trash = ServoPanCP.get() # it's no longer relevent
if _ServoPanCP == _ServoPanDP: # if all is good,-
_ServoPanS = 1 # slow the speed; no need to eat CPU just waiting
def P1(): # Process 1 controlles Tilt servo using same logic as above
speed = .1
_ServoTiltCP = initTilt - 1
_ServoTiltDP = initTilt
while True:
time.sleep(speed)
if ServoTiltCP.empty():
ServoTiltCP.put(_ServoTiltCP)
if not ServoTiltDP.empty():
_ServoTiltDP = ServoTiltDP.get()
if not ServoTiltS.empty():
_ServoTiltS = ServoTiltS.get()
speed = .1 / _ServoTiltS
if _ServoTiltCP < _ServoTiltDP:
_ServoTiltCP += 1
ServoTiltCP.put(_ServoTiltCP)
PWM.clear_channel_gpio(0, pTilt)
PWM.add_channel_pulse(0, pTilt, 0, _ServoTiltCP)
if not ServoTiltCP.empty():
trash = ServoTiltCP.get()
if _ServoTiltCP > _ServoTiltDP:
_ServoTiltCP -= 1
ServoTiltCP.put(_ServoTiltCP)
PWM.clear_channel_gpio(0, pTilt)
PWM.add_channel_pulse(0, pTilt, 0, _ServoTiltCP)
if not ServoTiltCP.empty():
trash = ServoTiltCP.get()
if _ServoTiltCP == _ServoTiltDP:
_ServoTiltS = 1
def P1(): # Process 1 controlles Tilt servo using same logic as above speed = .1 PitchCP = initpitch - 1 PitchDP = initpitch while True: time.sleep(speed) if PitchCPQ.empty(): PitchCPQ.put(Pitch) if not PitchDPQ.empty(): PitchDP = PitchDPQ.get() if not PitchSQ.empty(): PitchS = PitchSQ.get() speed = .1 / PitchS if PitchCP < PitchDP: PitchCP += 1 PitchCPQ.put(PitchCP) PWM.clear_channel_gpio(0, Pitch) PWM.add_channel_pulse(0, Pitch, 0,PitchCP) if not PitchCPQ.empty(): trash = PitchCPQ.get() if PitchCP > PitchDP: PitchCP -= 1 PitchCPQ.put(PitchCP) PWM.clear_channel_gpio(0, Pitch) PWM.add_channel_pulse(0, Pitch, 0, PitchCP) if not PitchCPQ.empty(): trash = PitchCPQ.get() if PitchCP == PitchDP: PitchS = 1
def P1(): # Process 1 controlles Tilt servo using same logic as above
speed = .1
PitchCP = initpitch - 1
PitchDP = initpitch
while True:
time.sleep(speed)
if PitchCPQ.empty():
PitchCPQ.put(Pitch)
if not PitchDPQ.empty():
PitchDP = PitchDPQ.get()
if not PitchSQ.empty():
PitchS = PitchSQ.get()
speed = .1 / PitchS
if PitchCP < PitchDP:
PitchCP += 1
PitchCPQ.put(PitchCP)
PWM.clear_channel_gpio(0, Pitch)
PWM.add_channel_pulse(0, Pitch, 0, PitchCP)
if not PitchCPQ.empty():
trash = PitchCPQ.get()
if PitchCP > PitchDP:
PitchCP -= 1
PitchCPQ.put(PitchCP)
PWM.clear_channel_gpio(0, Pitch)
PWM.add_channel_pulse(0, Pitch, 0, PitchCP)
if not PitchCPQ.empty():
trash = PitchCPQ.get()
if PitchCP == PitchDP:
PitchS = 1
def send_signal(signal):
for x in range(1, 1000, 5):
pwm.add_channel_pulse(0, IO_PIN, x, 3)
time.sleep(SIGNAL_LENGTHS[signal])
pwm.clear_channel_gpio(0, IO_PIN)
if DEBUG:
sys.stdout.write(signal)
sys.stdout.flush()
def send_signal(signal):
for x in range(1,1000,5):
pwm.add_channel_pulse(0, IO_PIN, x ,3)
time.sleep(SIGNAL_LENGTHS[signal])
pwm.clear_channel_gpio(0, IO_PIN)
if DEBUG:
sys.stdout.write(signal)
sys.stdout.flush()
def eyes(wink=3): for i in (1,wink): PWM.add_channel_pulse(0, 21, 0, 10) time.sleep(1) PWM.add_channel_pulse(0, 21, 0, 100) time.sleep(1) PWM.clear_channel_gpio(0, 21) GPIO.cleanup()
def setPulseWidth(self, pulseWidth):
# Sanity checking on the pulse
pulseWidth = min(max(pulseWidth, self.minPulseWidth),
self.maxPulseWidth)
# Add new pulse
PWM.add_channel_pulse(self.pwmChannel, self.gpioPin, self.pulseStart,
pulseWidth)
def beep(self, t=T_MED, duration=D_MED):
for x in range(0, self._range, t * 2):
PWM.add_channel_pulse(self._channel, self._pin, x, t)
sleep(duration)
PWM.clear_channel(self._channel)
def note(value, pin):
#PWM.setup()
#PWM.clear_channel_gpio(0, pin)
print "siren", value
#TODO - Add mapping between midi value and PWM speed
PWM.add_channel_pulse(0, pin, 0, value)
sleep(0.5)
PWM.clear_channel_gpio(0, pin)
def update(self, spin_rate): self.current_pulse_width = int(self.min_pulse_width + spin_rate) if self.current_pulse_width < self.min_pulse_width: self.current_pulse_width = self.min_pulse_width if self.current_pulse_width > self.max_pulse_width: self.current_pulse_width = self.max_pulse_width PWM.add_channel_pulse(RPIO_DMA_CHANNEL, self.bcm_pin, 0, self.current_pulse_width)
def stopMotor(self,channel):
if(channel=='A'):
print "stop channel A"
self.setDirection('A',0);
PWM.add_channel_pulse(self.A_CHANNEL,self.AENBL_Pin,0,0);
if(channel=='B'):
print "stop channel B"
self.setDirection('B',0);
PWM.add_channel_pulse(self.B_CHANNEL,self.BENBL_Pin,0,0);
def to_left():
global pos_x
global step
read_pos()
pos_x = int(pos_x) + int(step)
pos = open(pos_file_x,"w")
pos.write(str(pos_x))
pos.close()
PWM.add_channel_pulse(0, 22, 0, pos_x)
sleep(1)
def cool(cell, duty_cycle):
# computes variables based on duty cycle
subcycle_us = (1/cell[3])*1000000
width = (subcycle_us/10)*(duty_cycle/100)
# Stops the pulses on the colding side
PWM.clear_channel_gpio(cell[2], cell[0])
# Gerates pulses on heating side
PWM.add_channel_pulse(cell[2], cell[1], 0, width)
def SetDual7Seg(self, value):
# Split passed value into separate digit integer list
digits = map(int, "%02d" % value)
# Set pulses for segments A-G (both digits)
for i in range(7):
PWM.add_channel_pulse(0, 10 + i, 0,
self.pulse[self.num[digits[0]][i]])
PWM.add_channel_pulse(0, 10 + i, 1000,
self.pulse[self.num[digits[1]][i]])
def to_bottom():
global pos_y
global step
read_pos()
pos_y = int(pos_y) + int(step)
pos = open(pos_file_y,"w")
pos.write(str(pos_y))
pos.close()
PWM.add_channel_pulse(0, 4, 0, pos_y)
sleep(1)
def set_speed(pin, chan, speed, speed_0):
pulse_inc = PWM.get_pulse_incr_us()
cycle_dur = PWM.get_channel_subcycle_time_us(chan)
num_pulses = int(cycle_dur * speed / pulse_inc)
if speed >= 0.99:
num_pulses -= 1
PWM.add_channel_pulse(chan, pin, 0, num_pulses)
def set(self, channel, value):
if (channel < 0 or channel > 2):
return
if (value <= 0):
PWM.clear_channel(channel)
else:
if (value > 1000):
value = 1000
PWM.add_channel_pulse(channel, self.mapping[channel], 0, value)
def setspeed(): RPIO.output(dcen, False) if (Control.Speed < 0 ) : RPIO.output(dcdr, True) spd = - Control.Speed else: RPIO.output(dcdr, False) spd = Control.Speed PWM.add_channel_pulse(3, 10, 0, spd)
def initMotor(self):
RPIO.setup(self.MODE_Pin,RPIO.OUT,initial=RPIO.HIGH); #select PWM Mode;
PWM.setup(); #initialize channel 0 and 1 for motor A and B
PWM.init_channel(self.A_CHANNEL);
PWM.init_channel(self.B_CHANNEL);
RPIO.setup(self.APHASE_Pin,RPIO.OUT,initial=RPIO.LOW); #default A to fwd direction
PWM.add_channel_pulse(self.A_CHANNEL,self.AENBL_Pin,0,0);#default A to channel 0 and speed 0
RPIO.setup(self.BPHASE_Pin,RPIO.OUT,initial=RPIO.LOW); #default B to fwd direction
PWM.add_channel_pulse(self.B_CHANNEL,self.BENBL_Pin,0,0);#default B to channel 1 and speed 0
def setspeed():
RPIO.output(dcen, False)
if (Control.Speed < 0):
RPIO.output(dcdr, True)
spd = -Control.Speed
else:
RPIO.output(dcdr, False)
spd = Control.Speed
PWM.add_channel_pulse(3, 10, 0, spd)
def setSpeed(self, speed):
speed = int(speed / 100.0 * MAX_SPEED + 0.5)
if speed < 0:
speed = -speed
dir_value = 1
else:
dir_value = 0
if speed > MAX_SPEED:
speed = MAX_SPEED
PWM.add_channel_pulse(self.channel1, self.xin1, 0, dir_value * speed)
PWM.add_channel_pulse(self.channel2, self.xin2, 0, (1 - dir_value) * speed)
def __init__(self, gpio=18, period=20000, ch_count=6):
self.gpio = gpio
self.period = period
self.channel_count = ch_count
self.control_ppm = PWM.Servo()
PWM.setup()
# add channels and set init value as 150
for ch in range(1, self.channel_count):
PWM.init_channel(ch, self.period)
PWM.add_channel_pulse(ch, self.gpio, self.default_channel_width, self.default_channel_value)
self.default_channel_value.insert(ch, self.default_channel_value)
def RightMotor(dir):
if dir == DIR_FW:
PWM.add_channel_pulse(0, M2_B, 0, PWM_WIDTH)
PWM.clear_channel_gpio(0, M2_A)
webiopi.debug("Right FW")
elif dir == DIR_BW:
PWM.add_channel_pulse(0, M2_A, 0, PWM_WIDTH)
PWM.clear_channel_gpio(0, M2_B)
webiopi.debug("Right BW")
else:
PWM.clear_channel_gpio(0, M2_A)
PWM.clear_channel_gpio(0, M2_B)
def __init__(self, gpio=18, period=20000, ch_count=6):
self.gpio = gpio
self.period = period
self.channel_count = ch_count
self.control_ppm = PWM.Servo()
PWM.setup()
# add channels and set init value as 150
for ch in range(1, self.channel_count):
PWM.init_channel(ch, self.period)
PWM.add_channel_pulse(ch, self.gpio, self.default_channel_width,
self.default_channel_value)
self.default_channel_value.insert(ch, self.default_channel_value)
def setSpeed(self, speed):
speed = int(speed / 100.0 * MAX_SPEED + 0.5)
if speed < 0:
speed = -speed
dir_value = 1
else:
dir_value = 0
if speed > MAX_SPEED:
speed = MAX_SPEED
PWM.add_channel_pulse(self.channel1, self.xin1, 0, dir_value * speed)
PWM.add_channel_pulse(self.channel2, self.xin2, 0,
(1 - dir_value) * speed)
def RightMotor(dir):
if dir == DIR_FW:
PWM.add_channel_pulse(0, M2_B, 0, PWM_WIDTH)
PWM.clear_channel_gpio(0, M2_A)
webiopi.debug("Right FW")
elif dir == DIR_BW:
PWM.add_channel_pulse(0, M2_A, 0, PWM_WIDTH)
PWM.clear_channel_gpio(0, M2_B)
webiopi.debug("Right BW")
else:
PWM.clear_channel_gpio(0, M2_A)
PWM.clear_channel_gpio(0, M2_B)
def setup():
PWM.setup()
PWM.init_channel(DMA_LEFT,10000)
PWM.init_channel(DMA_RIGHT,10000)
PWM.add_channel_pulse(DMA_LEFT, LEFT_FORWARD, 0, 0)
PWM.add_channel_pulse(DMA_LEFT, LEFT_BACKWARD, 0, 0)
PWM.add_channel_pulse(DMA_RIGHT, RIGHT_FORWARD, 0, 0)
PWM.add_channel_pulse(DMA_RIGHT, RIGHT_BACKWARD, 0, 0)
def pwm_example2():
from RPIO import PWM
# Setup PWM and DMA channel 0
PWM.setup()
PWM.init_channel(0)
# Add some pulses to the subcycle
PWM.add_channel_pulse(0, 17, 0, 50)
PWM.add_channel_pulse(0, 17, 100, 50)
# Stop PWM for specific GPIO on channel 0
PWM.clear_channel_gpio(0, 17)
# Shutdown all PWM and DMA activity
PWM.cleanup()
def __setBrightness(self, intensity):
# If no backlight pin is assigned, just return
if not self.__backlight:
return
# Clamp our intensity to be between 0 and 100 percent
if intensity > 100:
intensity = 100
if intensity < 0:
intensity = 0
# Maximum width is 1999 based on the 20000us subcycle
width = int(1999.0 * (intensity / 100.0))
# Set PWM
PWM.add_channel_pulse(0, self.__backlight, 0, width)
def updatecolor (led_int, led_red, led_green, led_blue):
time.sleep(1)
PWM.clear_channel(0)
print('Set color using MOOD %1d: Intensity: %1d | Red: %1d | Green: %1d | Blue: %1d' % (mood, led_int, led_red, led_green, led_blue))
time.sleep(1)
PWM.add_channel_pulse(0, LI_PIN, 0, led_int)
PWM.add_channel_pulse(0, LR_PIN, 0, led_red)
PWM.add_channel_pulse(0, LG_PIN, 0, led_green)
PWM.add_channel_pulse(0, LB_PIN, 0, led_blue)
return
def main():
# Set up Frequency in Hertz
FREQUENCY = 1000
SUBCYCLE_US = ((1 / FREQUENCY) * 1000000)
CHANNEL = 0
# Set duty_cycle 0 -> 100
DUTY_CYCLE = 50
# Set Pin
PINO = PIN.CEL_A_1
# Setup PWM and DMA channel 0
PWM.setup()
PWM.init_channel(channel=CHANNEL, subcycle_time_us=SUBCYCLE_US)
# Test initialization
if not (PWM.is_channel_initialized(CHANNEL)):
print("ERROR: Channel could not be initialized!")
return -1
# Test Frequency
if not (PWM.get_channel_subcycle_time_us(CHANNEL) == SUBCYCLE_US):
print("ERROR: Frequency could not be setted!")
return -1
# Add pwm Pulse
PWM.add_channel_pulse(dma_channel=CHANNEL,
gpio=PINO,
start=0,
width=((SUBCYCLE_US / 10) * (DUTY_CYCLE / 100)))
# fake while
print("Press any key to stop")
input()
# Stop PWM for specific GPIO on channel 0
PWM.clear_channel_gpio(0, PINO)
# Shutdown all PWM and DMA activity
PWM.cleanup()
return 0
def __init__(self):
PWM.setup()
PWM.set_loglevel(PWM.LOG_LEVEL_ERRORS)
PWM.init_channel(0)
self.num = {
0: (1, 1, 1, 1, 1, 1, 0),
1: (0, 1, 1, 0, 0, 0, 0),
2: (1, 1, 0, 1, 1, 0, 1),
3: (1, 1, 1, 1, 0, 0, 1),
4: (0, 1, 1, 0, 0, 1, 1),
5: (1, 0, 1, 1, 0, 1, 1),
6: (1, 0, 1, 1, 1, 1, 1),
7: (1, 1, 1, 0, 0, 0, 0),
8: (1, 1, 1, 1, 1, 1, 1),
9: (1, 1, 1, 1, 0, 1, 1)
}
self.pulse = {0: 4, 1: 999}
PWM.add_channel_pulse(0, 20, 0, 999)
PWM.add_channel_pulse(0, 21, 1000, 999)
def set_brightness(self, percent):
# Make our percentage logarithmic from 0 to 100 for a more natural brightness curve
percent = (float(percent)/10)**2
# Divide the 20ms period into 4 pulses so we get 200Hz
# pulse width in 10us increments.
pulse_width = int((100-percent) * 5)
if (not PWM.is_setup()):
PWM.setup()
PWM.init_channel(0)
PWM.add_channel_pulse(0, self.lcd_backlight, start=0, width=pulse_width)
PWM.add_channel_pulse(0, self.lcd_backlight, start=499, width=pulse_width)
PWM.add_channel_pulse(0, self.lcd_backlight, start=999, width=pulse_width)
PWM.add_channel_pulse(0, self.lcd_backlight, start=1499, width=pulse_width)
def setLEDs(self):
if self.cnf.offline:
self.logger.info("TEST: topled=%d bottomled=%d",
self.topLedTransitionsPerCycle,
self.bottomLedTransitionsPerCycle)
return
PWM.clear_channel(ledChannel)
for pin, state in {
topLed: self.topLedTransitionsPerCycle,
bottomLed: self.bottomLedTransitionsPerCycle
}.iteritems():
if state:
ds = ledFull / (state * 2 - 1)
for i in range(0, state):
PWM.add_channel_pulse(ledChannel,
pin,
start=i * ds * 2,
width=ds)
else:
PWM.add_channel_pulse(ledChannel, pin, start=0, width=1)
def set_tilt(self, tilt_pct):
if tilt_pct < 0: #Tilt Down
if tilt_pct < -100:
tilt_pct = -100
tiltL = int(((self.tilt_centerL - self.tilt_minL) * tilt_pct) /
100) + self.tilt_centerL
tiltR = self.tilt_centerR - int(
((self.tilt_maxR - self.tilt_centerR) * tilt_pct) / 100)
else: #Tilt Up
if tilt_pct > 100:
tilt_pct = 100
tiltL = int(((self.tilt_maxL - self.tilt_centerL) * tilt_pct) /
100) + self.tilt_centerL
tiltR = self.tilt_centerR - int(
((self.tilt_centerR - self.tilt_minR) * tilt_pct) / 100)
print 'TILT=L:{0} ({1}/{2}/{3}) R:{4} ({5}/{6}/{7})'.format(
tiltL, self.tilt_minL, self.tilt_centerL, self.tilt_maxL, tiltR,
self.tilt_minR, self.tilt_centerR, self.tilt_maxR)
PWM.clear_channel_gpio(0, self.pTiltL)
PWM.add_channel_pulse(0, self.pTiltL, 0, tiltL)
PWM.clear_channel_gpio(0, self.pTiltR)
PWM.add_channel_pulse(0, self.pTiltR, 0, tiltR)
def set_velocity(self,pwmotor,motor_id): [pins,dma_ch] = self.motor_list[motor_id] c = [0,0] c[0] = PWM.get_channel_subcycle_time_us(dma_ch[0])/(100.0*PWM.get_pulse_incr_us()) #coefficient to convert duty to period c[1] = PWM.get_channel_subcycle_time_us(dma_ch[1])/(100.0*PWM.get_pulse_incr_us()) #coefficient to convert duty to period try: PWM.clear_channel_gpio(dma_ch[0], pins[0]) PWM.clear_channel_gpio(dma_ch[1], pins[1]) except: pass if pwmotor>0: PWM.add_channel_pulse(dma_ch[1],pins[1],0,0) PWM.add_channel_pulse(dma_ch[0],pins[0],0,int(abs(pwmotor)*c[0])) else: PWM.add_channel_pulse(dma_ch[0],pins[0],0,0) PWM.add_channel_pulse(dma_ch[1],pins[1],0,int(abs(pwmotor)*c[1]))
def set_pan(self, pan_pct):
if pan_pct < 0: #Pan Left
if pan_pct < -100:
pan_pct = -100
panL = int(((self.pan_centerL - self.pan_minL) * pan_pct) /
100) + self.pan_centerL
panR = int(((self.pan_centerR - self.pan_minR) * pan_pct) /
100) + self.pan_centerR
else: #Pan Right
if pan_pct > 100:
pan_pct = 100
panL = int(((self.pan_maxL - self.pan_centerL) * pan_pct) /
100) + self.pan_centerL
panR = int(((self.pan_maxR - self.pan_centerR) * pan_pct) /
100) + self.pan_centerR
print 'PAN=L:{0} ({1}/{2}/{3}) R:{4} ({5}/{6}/{7})'.format(
panL, self.pan_minL, self.pan_centerL, self.pan_maxL, panR,
self.pan_minR, self.pan_centerR, self.pan_maxR)
PWM.clear_channel_gpio(0, self.pPanL)
PWM.add_channel_pulse(0, self.pPanL, 0, panL)
PWM.clear_channel_gpio(0, self.pPanR)
PWM.add_channel_pulse(0, self.pPanR, 0, panR)
def drive(wheel,speed):
if wheel == LEFT:
print "left",
dma = dma_l
enable_pin = l_enable_pin
forward_pin = l_forward_pin
backward_pin = l_backward_pin
elif wheel == RIGHT:
print "right",
dma = dma_r
enable_pin = r_enable_pin
forward_pin = r_forward_pin
backward_pin = r_backward_pin
else:
error_msg( "unknown wheel")
return
if speed > 100 or speed < -100:
error_msg("speed should be > -100 and < 100")
return
if speed > 0:
RPIO.output(forward_pin, True)
RPIO.output(backward_pin, False)
print "forward",
elif speed < 0:
RPIO.output(forward_pin, False)
RPIO.output(backward_pin, True)
print "backward",
else:
print "stop",
pwm_amount = int(abs(speed * (1999/100)))
print "pwm:", pwm_amount
PWM.add_channel_pulse(dma,enable_pin,0,pwm_amount)
def start(self):
if not PWM.is_setup():
PWM.setup()
if not PWM.is_channel_initialized(0):
PWM.init_channel(0)
#same as look_forward() without clearing first
PWM.add_channel_pulse(0, self.pPanL, 0, self.pan_centerL)
PWM.add_channel_pulse(0, self.pTiltL, 0, self.tilt_centerL)
PWM.add_channel_pulse(0, self.pPanR, 0, self.pan_centerR)
PWM.add_channel_pulse(0, self.pTiltR, 0, self.tilt_centerR)
RPIO.setup(self.pPanicBtn, RPIO.IN)
RPIO.add_interrupt_callback(self.pPanicBtn,
edge='falling',
pull_up_down=RPIO.PUD_UP,
callback=self.cb_panic,
debounce_timeout_ms=200)
RPIO.wait_for_interrupts(threaded=True)
def api_colorchange2(red_raw,green_raw,blue_raw):
#PWM.print_channel(CHANNEL)
red = int(red_raw) * Correction_RED
print "red %s" % red
green = int(green_raw) * Correction_GREEN
print "green %s" % green
blue = int(blue_raw) * Correction_BLUE
print "blue %s" %blue
PWM.add_channel_pulse(CHANNEL, GPIO_RED, 0, int(red))
PWM.add_channel_pulse(CHANNEL, GPIO_GREEN, 0, int(green))
PWM.add_channel_pulse(CHANNEL, GPIO_BLUE, 0, int(blue))
return 'ok'
def set_velocity(self, pwmotor, motor_id):
[pins, dma_ch] = self.motor_list[motor_id]
c = [0, 0]
c[0] = PWM.get_channel_subcycle_time_us(
dma_ch[0]) / (100.0 * PWM.get_pulse_incr_us()
) #coefficient to convert duty to period
c[1] = PWM.get_channel_subcycle_time_us(
dma_ch[1]) / (100.0 * PWM.get_pulse_incr_us()
) #coefficient to convert duty to period
try:
PWM.clear_channel_gpio(dma_ch[0], pins[0])
PWM.clear_channel_gpio(dma_ch[1], pins[1])
except:
pass
if pwmotor > 0:
PWM.add_channel_pulse(dma_ch[1], pins[1], 0, 0)
PWM.add_channel_pulse(dma_ch[0], pins[0], 0,
int(abs(pwmotor) * c[0]))
else:
PWM.add_channel_pulse(dma_ch[0], pins[0], 0, 0)
PWM.add_channel_pulse(dma_ch[1], pins[1], 0,
int(abs(pwmotor) * c[1]))
def set(self, rgb, blink=False, fast=False):
"""Set desired output RGB values.
rgb: RGB vector of 0/1 or False/True.
[blink]: If the LED should blink.
"""
if not len(rgb) == 3:
raise ValueError("RGB vector must have three entries (rgb=" + str(rgb) + ")")
PWM.clear_channel(PWM_DMA)
PWM.clear_channel(PWM_DMA1)
timing_offset = 0 # cant set both low and high at the same time, using one channel
for value, pin in zip(rgb, self._pins):
log.debug("value=%s", str(value))
if value in (1, True, "HIGH"):
if blink:
if fast:
PWM.add_channel_pulse(PWM_DMA, pin, timing_offset, PWM_PRD / PWM_RES / 2)
else:
PWM.add_channel_pulse(PWM_DMA1, pin, timing_offset, PWM_PRD / PWM_RES / 2)
else:
PWM.add_channel_pulse(PWM_DMA, pin, timing_offset, PWM_PRD / PWM_RES - 3)
else:
PWM.add_channel_pulse(PWM_DMA, pin, timing_offset, 0)
timing_offset = timing_offset + 1
def power(self, state):
if self.cnf.offline:
self.logger.info("TEST: setting power=%d", state)
return
if state:
PWM.clear_channel(relayChannel)
PWM.add_channel_pulse(relayChannel,
relay,
start=0,
width=relayFull)
time.sleep(holdDelay)
PWM.clear_channel(relayChannel)
PWM.add_channel_pulse(relayChannel, relay, start=0, width=relayLow)
else:
PWM.add_channel_pulse(relayChannel, relay, start=0, width=0)
PWM.clear_channel(relayChannel)
def camdireita(): PWM.add_channel_pulse(0, ServoCam, 100, 220 )
from RPIO import PWM
PWM.setup()
PWM.init_channel(0)
PWM.add_channel_pulse(0, 12, 0, 50)
PWM.add_channel_pulse(0, 12, 100, 50)
PWM.clear_channel_gpio(0, 12)
input('waiting...')
PWM.cleanup()
def camesquerda(): PWM.add_channel_pulse(0, ServoCam, 100, 5 )
def camcentro(): PWM.add_channel_pulse(0, ServoCam, 100, 120 )
face = [
0, 0, 0, 0
] # This will hold the array that OpenCV returns when it finds a face: (makes a rectangle)
Cface = [0,
0] # Center of the face: a point calculated from the above variable
lastface = 0 # int 1-3 used to speed up detection. The script is looking for a right profile face,-
# a left profile face, or a frontal face; rather than searching for all three every time,-
# it uses this variable to remember which is last saw: and looks for that again. If it-
# doesn't find it, it's set back to zero and on the next loop it will search for all three.-
# This basically tripples the detect time so long as the face hasn't moved much.
PWM.setup()
PWM.init_channel(0)
#init servos to center
PWM.add_channel_pulse(0, pPan, 0, initPan)
PWM.add_channel_pulse(0, pTilt, 0, initTilt)
ServoPanCP = Queue(
) # Servo zero current position, sent by subprocess and read by main process
ServoTiltCP = Queue(
) # Servo one current position, sent by subprocess and read by main process
ServoPanDP = Queue(
) # Servo zero desired position, sent by main and read by subprocess
ServoTiltDP = Queue(
) # Servo one desired position, sent by main and read by subprocess
ServoPanS = Queue() # Servo zero speed, sent by main and read by subprocess
ServoTiltS = Queue() # Servo one speed, sent by main and read by subprocess
cv2.cv.NamedWindow("video", cv2.cv.CV_WINDOW_AUTOSIZE)
while True:
try:
# get joystick position from ADC
joy_x = readadc(joy_x_adc, SPICLK, SPIMOSI, SPIMISO, SPICS)
joy_y = readadc(joy_y_adc, SPICLK, SPIMOSI, SPIMISO, SPICS)
# convert the ADC values to something the motor driver can use
joy_x = (joy_x - 525)
joy_y = (joy_y - 521)
# set the steering servo's position
if joy_x <= 0:
servo_pos = float(joy_x + 1300)
else:
servo_pos = float(joy_x + 1300 + joy_x * 0.55)
servo.set_servo(20, round(servo_pos, -1))
# send commands out to the motors
if joy_y > 0:
PWM.add_channel_pulse(1, 26, 0, 0)
PWM.add_channel_pulse(2, 19, 0, int(round(joy_y * 3.8, -1)))
if joy_y <= 0:
PWM.add_channel_pulse(2, 19, 0, 0)
PWM.add_channel_pulse(1, 26, 0, int(round(joy_y * -3.8, -1)))
except KeyboardInterrupt:
servo.stop_servo(20)
RPIO.cleanup()
sys.exit()
