def left_speed(speed):
if speed <0:
PWM.clear_channel_gpio(DMA_LEFT, LEFT_FORWARD)
set_motor_speed(DMA_LEFT, LEFT_BACKWARD, -speed)
else:
PWM.clear_channel_gpio(DMA_LEFT, LEFT_BACKWARD)
set_motor_speed(DMA_LEFT, LEFT_FORWARD, speed)
def __init__(self, min_speed=50.0, flipLeft=False, flipRight=False):
PWM.set_loglevel(PWM.LOG_LEVEL_ERRORS)
# each motor will use seperate dma channel (not sure if completely necessary)
self.left_servo = PWM.Servo(dma_channel=0,
subcycle_time_us=self.SUBCYCLE_TIME)
self.right_servo = PWM.Servo(dma_channel=1,
subcycle_time_us=self.SUBCYCLE_TIME)
# RPIO.PWM module uses BCM GPIO numbering
self._gpio_lf = 24 # left forward
self._gpio_lr = 23 # left rear
self._gpio_rf = 22 # right forward
self._gpio_rr = 27 # right rear
# Holder for last commanded speed per motor
self.left_speed = 0
self.right_speed = 0
self.slope = math.floor(self.SUBCYCLE_TIME * (1 - min_speed/100.0) / 100.0)
self.offset = math.floor(self.SUBCYCLE_TIME * min_speed / 100.0)
if flipLeft:
self.flipLeftMotor()
if flipRight:
self.flipRightMotor()
def right_speed(speed):
if speed <0:
PWM.clear_channel_gpio(DMA_RIGHT, RIGHT_FORWARD)
set_motor_speed(DMA_RIGHT, RIGHT_BACKWARD, -speed)
else:
PWM.clear_channel_gpio(DMA_RIGHT, RIGHT_BACKWARD)
set_motor_speed(DMA_RIGHT, RIGHT_FORWARD, speed)
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, xin1, xin2, channels):
self.xin1 = xin1
self.xin2 = xin2
self.channel1 = channels[0]
self.channel2 = channels[1]
PWM.init_channel(self.channel1)
PWM.init_channel(self.channel2)
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 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 __init__(self, pin, minW = 0, maxW = 100): PWM.set_loglevel(PWM.LOG_LEVEL_ERRORS) self.__pin = pin self.__minW = minW self.__maxW = maxW self.__servo = PWM.Servo() self.__W = 0
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 __enter__(self):
PWM.set_loglevel(PWM.LOG_LEVEL_ERRORS)
GPIO.setup(self._gpios['solenoid'], GPIO.OUT)
GPIO.setup(self._gpios['echo_trigger'], GPIO.OUT)
GPIO.setup(self._gpios['echo'], GPIO.IN)
self._i2c_bus = SMBus()
self._i2c_bus.open(Monster.I2C_BUS_NUM)
self.close_door()
return self
def configureRPIO():
PWM.set_loglevel(PWM.LOG_LEVEL_ERRORS)
RPIO.setwarnings(False)
RPIO.setup(R, RPIO.OUT)
RPIO.setup(A, RPIO.OUT)
RPIO.setup(G, RPIO.OUT)
RPIO.output(R, False)
RPIO.output(A, False)
RPIO.output(G, False)
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 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 __init__(self):
from RPIO import PWM
# Defaults for PWM.Servo, but we need to work with them
self._subcycle_time_us = 20000
self._pulse_incr_us = 10
PWM.set_loglevel(PWM.LOG_LEVEL_ERRORS)
self._servo = PWM.Servo(
subcycle_time_us = self._subcycle_time_us,
pulse_incr_us = self._pulse_incr_us)
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 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 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 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 __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 __init__(self, gpioPin):
# RPIO PWM initalization
if PWM.is_setup() == 0:
PWM.setup(pulse_incr_us=self.pulseIncrementUs)
self.gpioPin = gpioPin
self.dutyCycle = self.initialpulsewidth
self.pwmChannel = self.getFreePwmChannel()
PWM.init_channel(self.pwmChannel, subcycle_time_us=self.subCycleTime)
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 main(): servo = PWM.Servo() time.sleep(0.2) init(servo) move(servo, 0) move(servo, 180) move(servo, 0) PWM.cleanup()
def initialize(self):
# Setup PWM if backlight is defined
if self.__backlight:
PWM.setup()
PWM.init_channel(0)
self.enableBacklight()
# Setup rs, enable and rw pins
GPIO.setup(self.__rs, GPIO.OUT)
GPIO.setup(self.__enable, GPIO.OUT)
if self.__rw:
GPIO.setup(self.__rw, GPIO.OUT)
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 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 setW(self, W):
"Checks W% is between limits than sets it"
import RPIO.PWM as PWM
PW = 0
self.__W = W
if self.__W < self.__WMin:
self.__W = self.__WMin
if self.__W > self.__WMax:
self.__W = self.__WMax
#on time[us]/granularity[us]
PW = ((PWM.get_channel_subcycle_time_us(0) * self.__W)/100)/PWM.get_pulse_incr_us()
# Set servo to xxx us
if self.powered:
self.__IO.set_servo(self.__pin, PW)
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 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 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
sys.path.append(os.pardir)
#from moter import Moter
#from pwm_rpio import PwmRPIO
from RPIO.PWM import Servo
from RPIO import PWM
import RPi.GPIO as GPIO
import time
trigger_pin = 40
GPIO.setmode(GPIO.BOARD)
GPIO.setup(trigger_pin, GPIO.OUT)
GPIO.setwarnings(False)
PWM.setup(pulse_incr_us=1)
PWM.init_channel(0, subcycle_time_us=10000)
# Add some pulses to the subcycle
PWM.add_channel_pulse(0, 20, 0, 1000)
PWM.set_loglevel(PWM.LOG_LEVEL_ERRORS)
# Stop PWM for specific GPIO on channel 0
while True:
GPIO.output(trigger_pin, 0)
#PWM.cleanup()
#PWM.set_loglevel( PWM.LOG_LEVEL_ERRORS)
#PWM.setup(pulse_incr_us=1)
#PWM.init_channel(0, subcycle_time_us=10000)
#PWM.clear_channel_gpio(0, 20)
PWM.add_channel_pulse(0, 20, 0, 1000)
PWM.seek(0, 0)
import RPIO
from RPIO import PWM
import time
PWM.setup()
PWM.init_channel(0)
pin = 4 # pin 7 on the board, GPIO4
# Test script for the relay; powers it up
# at full PWM for a second; drops the voltage
# to about 10 volts (to reduce heat generation
# in the 330 ohm resistor) for 5 seconds; and
# then switches # it off for a second.
while True:
PWM.add_channel_pulse(0, pin, start=0, width=2000 - 1)
print "Full on"
time.sleep(0.2)
print "Dropping back to 25%"
PWM.add_channel_pulse(0, pin, start=0, width=500)
time.sleep(2)
print "off"
PWM.clear_channel(0)
time.sleep(0.5)
print "And done."
PWM.cleanup()
from RPIO import PWM
import time
s = PWM.Servo()
s.set_servo(12, 1500)
input('stop')
s.stop_servo(12)
#time.sleep(3)
#s.set_servo(12, 1900)
#time.sleep(10)
#s.stop_servo(12)
def start(self):
"Run the procedure to init the PWM"
self.__IO = PWM.Servo()
self.powered = True
from RPIO import PWM
roll = PWM.Servo(0, 20000, 5)
pitch = PWM.Servo(0, 20000, 5)
throttle = PWM.Servo(0, 20000, 5)
yaw = PWM.Servo(0, 20000, 5)
# start PWM on servo specific GPIO no, this is not the pin no but it is the GPIO no
roll.set_servo(5, 1500) # pin 29
pitch.set_servo(6, 1500) # pin 31
throttle.set_servo(13, 1150) # pin 33
yaw.set_servo(19, 1505) # pin 35
try:
while True:
x = int(input("CHECK Roll: "))
roll.set_servo(5, x)
except:
yaw.stop_servo(19)
roll.stop_servo(5)
pitch.stop_servo(6)
throttle.stop_servo(13)
PWM.cleanup()
def cleanup():
PWM.cleanup()
#!/usr/bin/env python
import logging
import RPIO, time, os
from RPIO import PWM
import requests
import sys
import threading
import time
logging.basicConfig(level=logging.WARN)
PWM.set_loglevel(PWM.LOG_LEVEL_ERRORS)
RPIO.setmode(RPIO.BCM)
servo = None
def decode_color(color):
if color.endswith('anime'):
return 0
elif color == 'blue':
return 1
else:
return -1
def get_config():
return {
'examplesvc': {
'button':
17,
'led':
P_angle = math.degrees(fusionPose[1])
Y_angle = math.degrees(fusionPose[2])
MagX = compass[0]
MagY = compass[1]
heading = math.degrees(math.atan2(MagY, MagX))
heading = heading + mag_declination
sleep(poll_interval * 0.01 / 1000.0)
return R_angle, P_angle, Y_angle, heading
#%% Initialise Variables
host = '' # Initialise host
port = 5560 # Arbitrary choice. Must be a free port.
s = PWM.Servo() # Redeclare servo variable
# GPIO Pins 17, 18, 22, 27 for PWM Signal and Pin 14 for Ground
# Assign short thruster IDs FL (Front-Left), FR, BL (Back-Left) and BR
FL = 17 # Assign Front-Left thruster to pin 17 (R6C1)
FR = 18 # Assign Front-Right thruster to pin 18 (R6C2)
BL = 22 # Assign Back-Left thruster to pin 22 (R8C1)
BR = 27 # Assign Back-Right thruster to pin 27 (R7C1)
all_thrusters = [FL, FR, BL, BR] # Create array of all thruster pins
# Note that the ground pin 14 is located at R7C2
# Initialisation of PWM array with zero frequency
PWM_array = [0, 0, 0, 0] # [FL , FR , BL , BR]
# Initialisation of max_PWM_range variable
max_PWM_range = 120
def get_DMA():
a = find_DMA_available()
PWM.init_channel(a)
return a
def __init__(self):
if not PWM.is_setup():
PWM.setup() #put this in def __init__():
##==============================================================================## #Initializing Servos #servo GPIO connections Roll = 23 Pitch = 24 # Upper limit RollUL = 230 PitchUL = 230 # Lower Limit RollLL = 60 PitchLL = 60 #initial Position initroll = ((RollUL - RollLL) / 2) + RollLL initpitch = ((PitchUL - PitchLL) / 2) + PitchLL PWM.setup() PWM.init_channel(0) #init servos to center PWM.add_channel_pulse(0, Roll, 0, initroll) PWM.add_channel_pulse(0, Pitch, 0, initpitch) RollCPQ = Queue( ) # Servo zero current position, sent by subprocess and read by main process PitchCPQ = Queue( ) # Servo one current position, sent by subprocess and read by main process RollDPQ = Queue( ) # Servo zero desired position, sent by main and read by subprocess PitchDPQ = Queue( ) # Servo one desired position, sent by main and read by subprocess RollSQ = Queue() # Servo zero speed, sent by main and read by subprocess PitchSQ = Queue() # Servo one speed, sent by main and read by subprocess
#MAY THE BACKWARD STEERING BE CHANGED?? THIS MAY BECOME CONFUSING! #NOTE driving-METHOD-COMMENTS from RPIO import PWM from time import sleep servo = PWM.Servo() #define PIN-Settings (Board-Numbering, not GPIO-Numbering) # actually not needed here as we use GPIO-Numbering #PIN_Servo=11 #Servo-Signal #PIN_Motor=7 #Motor-Signal #PIN_SDA=3 #Kompass SDA #PIN_SCL=5 #Kompass SCL #PIN_TX=8 #GPS TX #PIN_RX=10 #GPS RX #define PIN-Settings (GPIO-Numbering, not Board-Numbering) GPIO_Servo = 12 GPIO_Motor = 4 #GPIO_SDA=2 # not needed in this module #GPIO_SCL=3 #GPIO_TX=14 #GPIO_RX=15 #define Servopositions for steering # values adjusted to incrementer 10us (micro-seconds) Left = 1900 Half_Left = 1700 #needed? Straight = 1500 Half_Right = 1300 #needed?
def main(argv=None):
if argv is None:
import sys
argv = sys.argv
'''RPIO.setmode(RPIO.BOARD)
RPIO.setup(p_roll, RPIO.OUT)
RPIO.setup(p_pitch, RPIO.OUT)
RPIO.setup(p_throttle, RPIO.OUT)
RPIO.setup(p_yaw, RPIO.OUT)'''
servo = PWM.Servo()
dc_min = 0.0
dc_max = 100.0
dc_step = (dc_max - dc_min) / 100.0
dc_roll = (dc_max - dc_min) / 2.0
dc_pitch = (dc_max - dc_min) / 2.0
dc_throttle = dc_min
dc_yaw = (dc_max - dc_min) / 2.0
def dc_change(dc, type):
if (type == '-'):
dc -= dc_step
elif (type == '+'):
dc += dc_step
if (dc < dc_min):
dc = dc_min
elif (dc > dc_max):
dc = dc_max
return dc
while True:
ch = read_single_keypress()
print(ch, ord(ch))
if (ch == '\x1b'): # cannot display
break
if (ch == 'w'):
dc_pitch = dc_change(dc_pitch, '-')
print("dc_pitch:", dc_pitch)
elif (ch == 's'):
dc_pitch = dc_change(dc_pitch, '+')
print("dc_pitch:", dc_pitch)
elif (ch == 'a'):
dc_roll = dc_change(dc_roll, '-')
print("dc_roll:", dc_roll)
elif (ch == 'd'):
dc_roll = dc_change(dc_roll, '+')
print("dc_roll:", dc_roll)
elif (ch == 'h'):
dc_throttle = dc_change(dc_throttle, '-')
print("dc_throttle:", dc_throttle)
elif (ch == 'j'):
dc_throttle = dc_change(dc_throttle, '+')
print("dc_throttle:", dc_throttle)
elif (ch == 'k'):
dc_yaw = dc_change(dc_yaw, '+')
print("dc_yaw:", dc_yaw)
elif (ch == 'l'):
dc_yaw = dc_change(dc_yaw, '-')
print("dc_yaw:", dc_yaw)
pwm_update(servo, p_roll, dc_roll)
pwm_update(servo, p_pitch, dc_pitch)
pwm_update(servo, p_throttle, dc_throttle)
pwm_update(servo, p_yaw, dc_yaw)
servo.stop_servo(p_roll)
servo.stop_servo(p_pitch)
servo.stop_servo(p_throttle)
servo.stop_servo(p_yaw)
except:
print('Error occured, closing socket')
PWM.cleanup()
clientsocket.close()
# Set up the server socket
serversocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# Set the port and IP address for the host. Must be the IP for the
# Raspberry Pi
PORT = 1000
HOST = '192.168.0.101'
# Bind the socket and begin listening
serversocket.bind((HOST, PORT))
serversocket.listen(10)
# Constantly look for a connection
while 1:
print('Waiting for connection!')
# Accept connections from client
(clientsocket, address) = serversocket.accept()
# Create a thread for the connection and then run it
ct = Thread(target=handle, args=(clientsocket,))
ct.run()
# Clean up PWM signals at the end of the program -- Must be done!
PWM.cleanup()
from RPIO import PWM
GPIO_PINS = [25, 8, 7, 24]
servo = PWM.Servo()
while True:
speed = raw_input("Gimmie Value For Port 25: ")
sp = int(speed)
servo.set_servo(GPIO_PINS[0], sp)
servo.set_servo(GPIO_PINS[0], 1000)
servo.stop_servo(GPIO_PINS[0])
PWM.cleanup()
def handle(clientsocket):
print('Made connection\n')
# Look for if any exceptions are thrown
try:
flying = 1
while(flying):
# Wait for first command from client
buf = clientsocket.recv(MAX_LENGTH)
navigating = 0
movDirection = 2
search = 0
serInput = ''
sensor1 = 0
sensor2 = 0
sensor3 = 0
sensor4 = 0
# If command is takeoff then initialize and takeoff
if(buf == 'takeoff'):
autopilot.initialize()
autopilot.arm()
autopilot.takeOff()
navigating = 1
# If client is lost then leave method
if buf == '': return
# Handle navigation and object avoidance
while(navigating == 1):
#print("In navigation\n")
# Check for serial input -- Not implemented yet
if(ser.inWaiting()):
serInput = ser.readline()
#if 'f' in serInput: sensor1 = 1
#else: sensor1 = 0
#if 'r' in serInput: sensor2 = 1
#else: sensor2 = 0
#if 'b' in serInput: sensor3 = 1
#else: sensor3 = 0
#if 'l' in serInput: sensor4 = 1
#else: sensor4 = 0
#if 'k' in serInput: isKill = 1
#else: isKill = 0
print(serInput)
#time.sleep(1)
# Prints to see what values are received
#sensor1 = GPIO.input(9)
#print("Sensor1 = " + str(sensor1))
#sensor2 = GPIO.input(7)
#print("Sensor2 = " + str(sensor2))
#sensor3 = GPIO.input(8)
#print("Sensor3 = " + str(sensor3))
#sensor4 = GPIO.input(11)
#print("Sensor4 = " + str(sensor4))
# Wait for next input from client -- causes program
# to hang until it receives something. Needs changed!
#buf = clientsocket.recv(MAX_LENGTH)
#if buf == '': return
#isKill = GPIO.input(15)
print(isKill)
# Initial move right
if(sensor3 == 1 and sensor1 == 0 and sensor4 == 0 and sensor2 == 0 and movDirection == 2):
#print("Initial move right")
autopilot.strafeR()
autopilot.forward()
movDirection = 2
print('Moving right')
if(sensor3 == 0 and sensor1 == 0 and sensor4 == 0 and sensor2 == 0 and movDirection == 2):
autopilot.strafeR()
autopilot.backward()
movDirection = 2
print('Moving right-ADJ')
# End initial move right
# Found right wall, start moving forward
if(sensor1 == 0 and sensor2 == 1 and (sensor3 == 0 or sensor3 == 1) and sensor4 == 0 and movDirection == 2):
#print("Found right wall, start moving forward")
autopilot.stop()
time.sleep(1)
autopilot.forward()
movDirection = 1
print('Start Moving forward')
# Following right wall
if(sensor1 == 0 and sensor2 == 1 and sensor3 == 0 and sensor4 == 0 and movDirection == 1):
#print("Following right wall")
#autopilot.stop()
autopilot.forward()
autopilot.strafeL()
#movDirection = 1
print('Moving forward')
if(sensor1 == 0 and sensor2 == 0 and sensor3 == 0 and sensor4 == 0 and movDirection == 1):
#autopilot.stop()
autopilot.forward()
autopilot.strafeR()
#movDirection = 1
print('Moving forward-ADJ')
# End following right wall
# Hit a right back corner
if(sensor1 == 1 and sensor4 == 0 and sensor2 == 1 and sensor3 == 0 and movDirection == 1):
#print("Hit a right back corner")
autopilot.stop()
#time.sleep(.3)
autopilot.strafeL()
movDirection = 4
print('Start Moving left')
# Moving left following front wall
if(sensor1 == 1 and sensor4 == 0 and sensor2 == 0 and sensor3 == 0 and movDirection == 4):
#print("Moving left following front wall")
autopilot.stop()
#time.sleep(.3)
autopilot.strafeL()
autopilot.backward()
#movDirection = 4
print('Moving left')
if(sensor1 == 0 and sensor4 == 0 and sensor2 == 0 and sensor3 == 0 and movDirection == 4):
autopilot.stop()
#time.sleep(.3)
autopilot.strafeL()
autopilot.forward()
#movDirection = 4
print('Moving left-ADJ')
# End following front wall
# Lost front wall
if(sensor1 == 0 and sensor2 == 0 and sensor3 == 0 and sensor4 == 0 and movDirection == 4):
#print("Lost front wall")
autopilot.stop()
time.sleep(.3)
audopilot.strafeL()
time.sleep(.3)
autopilot.stop()
time.sleep(.5)
autopilot.forward()
time.sleep(.5)
autopilot.stop()
time.sleep(.5)
movDirection = 2
while (sensor3 == 0):
autopilot.strafeR()
time.sleep(1)
autopilot.stop()
print('Wall vanished')
ser.flush()
# Found back left corner
if(sensor1 == 1 and sensor2 == 0 and sensor3 == 0 and sensor4 == 1 and movDirection == 4):
print("Found back left corner")
autopilot.stop()
time.sleep(1)
autopilot.turnR()
time.sleep(1.5)
autopilot.stop()
time.sleep(1)
search = 1
navigating = 0
ser.flush()
# No direction to fly
if(sensor1 == 1 and sensor2 == 1 and sensor3 == 1 and sensor4 == 1):
#print("No direction to fly")
autopilot.land()
time.sleep(5)
flying = 0
print('Can\'t fly')
# Emergency kill command
if(buf == "kill"):
autopilot.throttleCut()
#time.sleep(10)
#autopilot.landed()
flying = 0
#print('Killing operation')
if(isKill):
#autopilot.throttleCut()
autopilot.stop()
autopilot.land()
time.sleep(10)
flying = 0
return
#print('Killed manually')
# Variable for if ball is seen
seen = 0
# Start searching for ball -- Needs implemented
#while(search == 1 and buf != 'stop'):
# Capture image and look for ball
# Close the socket if ctrl+c key combination is used
except KeyboardInterrupt:
clientsocket.close()
# Close the socket in all other exceptions
except:
print('Error occured, closing socket')
PWM.cleanup()
clientsocket.close()
import asyncio
import websockets
from RPIO import PWM
servo1 = PWM.Servo()
async def updateMotor(websocket, path):
valor = 0
while (True):
#Espera por nuevos datos
potencia = await websocket.recv()
#cuando hay un cambio, lo muestra por consola
print("Potencia: " + potencia + "%")
#actualiza la potencia del motor
servo1.set_servo(26, 1000 + int(valor) * 10)
start_server = websockets.serve(updateMotor, '0', 8765)
asyncio.get_event_loop().run_until_complete(start_server)
asyncio.get_event_loop().run_forever()
from RPIO import PWM
import time
# Setup RPIO Pins
RPIO.setup(26, RPIO.OUT, initial = RPIO.LOW) #ENA
RPIO.setup(11, RPIO.OUT, initial = RPIO.LOW) #ENB
RPIO.setup(19, RPIO.OUT, initial = RPIO.LOW) #IN1
RPIO.setup(13, RPIO.OUT, initial = RPIO.LOW) #IN2
RPIO.setup(6, RPIO.OUT, initial = RPIO.LOW) #IN3
RPIO.setup(5, RPIO.OUT, initial = RPIO.LOW) #IN4
PWM.setup()
PWM.init_channel(0)
RPIO.output(19, RPIO.HIGH)
RPIO.output(13, RPIO.LOW)
RPIO.output(6, RPIO.HIGH)
RPIO.output(5, RPIO.LOW)
try:
PWM.add_channel_pulse(0, 26, 0, 10000) #start at 0 and width = 10000us i.e 50%
PWM.add_channel_pulse(0, 11, 0, 10000)
time.sleep(5)
except KeyboardInterrupt:
PWM.clear_channel(0)
# PWM.clear_channel_gpio(0, 17)
PWM.cleanup()
RPIO.cleanup()
from RPIO import PWM
import time
motor = PWM.Servo()
#motor.set_servo(7,1000)
#time.sleep(1)
while (True):
eingabe = raw_input('Wert eingebe:')
#for i in range (600,700,20):
# motor.set_servo(7,i)
# time.sleep(2)
motor.set_servo(4, int(eingabe))
time.sleep(4)
motor.stop_servo(4)
#motor.stop_servo(7)
import argparse from pythonosc import dispatcher from pythonosc import osc_server from pythonosc import osc_message_builder from pythonosc import udp_client # RPI Stuff from RPIO import PWM GPIO_PINS = [25, 24, 8, 7] PWM.set_loglevel(PWM.LOG_LEVEL_ERRORS) servo = PWM.Servo(pulse_incr_us=1) PWM_MAX = 2000 PWM_MIN = 1000 PWM_RANGE = PWM_MAX - PWM_MIN # Client Management active_client = [] # Running Variables. armed = False running = False manual_control = False is_waiting = False def pwm_max_calibrate(): servo.set_servo(GPIO_PINS[0], PWM_MAX) servo.set_servo(GPIO_PINS[1], PWM_MAX) servo.set_servo(GPIO_PINS[2], PWM_MAX) servo.set_servo(GPIO_PINS[3], PWM_MAX)
def init_pos():
PWM.add_channel_pulse(0, 4, 0, 198)
PWM.add_channel_pulse(0, 22, 0, 148)
sleep(2)
def __init__(self, pin):
self.pin = pin
self.servo = PWM.Servo()
self.status = None
frontalface = cv2.CascadeClassifier(
"lbpcascade_frontalface.xml") # frontal face pattern detection
#profileface = cv2.CascadeClassifier("lbpcascade_profileface.xml") # side face pattern detection
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
import sys
import RPi.GPIO as GPIO
from time import sleep
from RPIO import PWM
pos_file_x = "./pos_x.txt"
pos_file_y = "./pos_y.txt"
GPIO.setmode(GPIO.BCM)
GPIO.setup(22,GPIO.OUT)
GPIO.setup(4,GPIO.OUT)
PWM.setup()
PWM.init_channel(0)
# GPIO 22 = pos_x gauche-droite = 100 (150) 200
# GPIO 4 = pos_y bas-haut = 150 (200) 250
pos_x = 0
pos_y = 0
step = 15
def init_pos():
PWM.add_channel_pulse(0, 4, 0, 198)
PWM.add_channel_pulse(0, 22, 0, 148)
sleep(2)
def read_pos():
global pos_y
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)
from RPIO import PWM
import time
import imumodule.py
#Distance and Angle obtained
file=open("xyz.txt","r")
contents=file.read()
distance=contents[contents.find(":")+1:contents.find("\n")]
file.seek(contents.find(":")+1)
contents=file.read()
angle=contents[contents.find(":")+1:]
'''print(distance)
print(angle)'''
# initialize servo objects with PWM function
roll = PWM.Servo()
pitch = PWM.Servo()
throttle = PWM.Servo()
yaw = PWM.Servo()
aux = PWM.Servo()
# start PWM on servo specific GPIO no, this is not the pin no but it is the GPIO no
roll.set_servo(5,1520)# pin 29
pitch.set_servo(6,1520)# pin 31
throttle.set_servo(13,1100)# pin 33
yaw.set_servo(19,1520)# pin 35
aux.set_servo(26,1010)# pin 37, pin 39 is Ground
# assign global min and max values
th_min = 1100
th_max = 2400
#GPIO.setup(roll_pin, GPIO.OUT) #GPIO.setup(pitch_pin, GPIO.OUT) #GPIO.setup(throttle_pin, GPIO.OUT) #GPIO.setup(yaw_pin, GPIO.OUT) #GPIO.setup(aux_pin, GPIO.OUT) #roll = GPIO.PWM(roll_pin, 500) # Aileron #pitch = GPIO.PWM(pitch_pin, 500) # Elevator #throttle = GPIO.PWM(throttle_pin, 600) # Throttle #yaw = GPIO.PWM(yaw_pin, 1520) # Rudder #aux = GPIO.PWM(aux_pin, 1010) # Control Mode # Servo 객체 초기화 roll = PWM.Servo() # Aileron pitch = PWM.Servo() # Elevator throttle = PWM.Servo() # Throttle yaw = PWM.Servo() # Rudder aux = PWM.Servo() # Control Mode # 각각의 GPIO번호에 PWM 세팅(핀번호 아님) roll.set_servo(5,1520) # pin 29 pitch.set_servo(6,1520) # pin 31 throttle.set_servo(13,1100) # pin 33 yaw.set_servo(19,1520) # pin 35 aux.set_servo(26,1010) # pin 37, pin 39 is Ground # 최대최소값 지정 th_min = 1100 th_max = 2400
return (((90.0+angulo)*(50.0/9.0))+1000)
else:
return 1500
#Miramos se existe un argumento de entrada utilizamolo como angulo
angle1 = 0
angle2 = 0
if len(sys.argv)>2:
angle1 = int(sys.argv[1])
angle2 = int(sys.argv[2])
pulso1 = angleToTime(angle1)
pulso2 = angleToTime(angle2)
print 'Pulso1: '+ repr(pulso1) +', Pulso2: '+ repr(pulso2)
servo=PWM.Servo() #Iniciamos a libreria para o servo
servo.set_servo(GPIO_SERVO2,1500)
servo.set_servo(GPIO_SERVO1,1500) #Centramos o servo
time.sleep(1)
while True:
try:
servo.set_servo(GPIO_SERVO1,590)
servo.set_servo(GPIO_SERVO2,590)
time.sleep(1)
servo.set_servo(GPIO_SERVO1,2200)
servo.set_servo(GPIO_SERVO2,2200)
time.sleep(1)
except KeyboardInterrupt:
break
subcycle_time = [3000, 5000, 100000, 150000
] #moltiplicare per granularity (default: 10 microsecondi)
#(in millisec. sono: 20ms, 200ms, 5sec, 10sec)
#pulse_start = [200, 20, 50, 100] #_start=200*100 microsec=20 msec.
#pulse_width = [100, 500, 1000, 5000] #_width=100*100 microsec=10 msec.
pulse_start = [0, 0, 0, 0] #_start=0*10 microsec=0 msec.
pulse_width = [0, 0, 0, 0] #_width=0*10 microsec=0 msec.
#incr_impulso = [10, 10, 10, 10]
canale_dma = [0, 1, 2, 3] #ce ne sono 15 (0-14)
gpio_port = [18, 23, 24, 25] #porte GPIO (lato pin pari)
#frequency = [500, 50, 2, 1] #in Hz
#set della granularity (è il default durante l'inizializzazione tic di incremento in microsecondi)
PWM.setup(granularity, 0) #default: pulse_incr_us=10, delay_hw=0
for i in canale_dma:
PWM.init_channel(i, subcycle_time[i]) #canale DMA con tempo subcycle
# PWM.setup(granularity, 0) #default: pulse_incr_us=10, delay_hw=0
#aggiungo un impulso nel canale DMA all'interno di ogni subcycle per ogni GPIO
#POSSO ANCHE NON AGGIUNGERE NIENTE :-), VOLENDO.
PWM.add_channel_pulse(i, gpio_port[i], pulse_start[i], pulse_width[i])
#setup per output su GPIO
for i in gpio_port:
RPIO.setup(i, RPIO.OUT)
for i in canale_dma:
PWM.print_channel(i)
if PWM.is_channel_initialized(i):
print("canale ", i, " inizializzato")
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]))
