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 __init__(self):
PWM.cleanup()
self.servo = PWM.Servo()
self.servo.set_servo(17, 1500)
self.servo_lr_angle = 0
self.servo.set_servo(22, 1500)
self.servo_ud_angle = 0
def move(dest):
"""Move servo on pin 17 to a given angle"""
servo = PWM.Servo()
# Set servo on GPIO17 to 1200s (1.2ms)
point = (end - start) * int(dest) / 180
servo.set_servo(17, start + point - (point % 100))
time.sleep(0.5)
def __init__(self):
# Initialize lock servo and button.
self.servo = PWM.Servo()
RPIO.setup(config.BUTTON_PIN, RPIO.IN)
# Set initial box state.
self.button_state = RPIO.input(config.BUTTON_PIN)
self.is_locked = None
def __init__(self, Chan, Upper, Lower): self.chan = Chan self.upper = Upper self.lower = Lower self.servo = PWM.Servo() PWM.set_loglevel(1) self.servo.set_servo(self.chan, self.upper)
def __init__(self,
name,
pin,
kv=1000,
WMin=0,
WMax=100,
debug=True,
simulation=False):
self.dragCoeff = 1.5
self.liftConst = 1
self.omega = 0
self.name = name
self.powered = True
self.simulation = simulation
self.__pin = pin
self.__kv = kv
self.__WMin = WMin if WMin >= 0 else 0
self.__WMax = WMax if WMin <= 100 else 100
# self.setDebug(debug)
self.__W = self.__WMin
self.__Wh = 10
try:
from RPIO import PWM
self.__IO = PWM.Servo()
except ImportError:
print("simulation ****")
self.simulation = True
def rotate(seconds):
servo = PWM.Servo()
servo.set_servo(17, 1000)
time.sleep(seconds)
servo.stop_servo(17)
return 0
def __init__(self, gpio, name):
self.__gpio = gpio
self.name = name
self.__servo = PWM.Servo(self.__dma_channel, self.__subcycle_time_us,
self.__pulse_incr_us)
self.__width = 0
self.set_pulse_width(0)
def set_servo(servo_id, position):
"""
Sets a given GPIO.
@param servo_id ID of servo to set
@param position Timing value for servo position, will be rounded to nearest 10us
"""
max_pos = app.config[servo_id + '_MAX']
min_pos = app.config[servo_id + '_MIN']
# Ignore positions out of the valid range
if position > max_pos:
logging.getLogger(__name__).error('Servo %s, position %d is greater than max %d' % (servo_id, position, max_pos))
return
if position < min_pos:
logging.getLogger(__name__).error('Servo %s, position %d is less than min %d' % (servo_id, position, min_pos))
return
time_val = int(round(position, -1))
gpio = app.config[servo_id + '_GPIO']
# Create a new servo object if it does not exist
if servo_id not in SERVOS:
SERVOS[servo_id] = PWM.Servo()
servo = SERVOS[servo_id]
logging.getLogger(__name__).debug('Setting servo %s on GPIO %d: value %d' % (servo_id, gpio, time_val))
servo.set_servo(gpio, time_val)
# Record the position of the sensor
SERVO_POSITIONS[servo_id] = time_val
def __init__(self,
name,
pin,
kv=1400,
WMin=0,
WMax=100,
debug=True,
simulation=True):
print('Configured Pin: ', pin)
self.name = name
self.powered = False
self.simulation = simulation
self.__pin = pin
self.__kv = kv
self.setWLimits(WMin, WMax)
self.setDebug(debug)
self.__W = self.__WMin
self.__Wh = 10
try:
from RPIO import PWM
self.__IO = PWM.Servo()
except ImportError:
self.simulation = True
def _update_servo_width(operation, value=45):
"""TODO: Docstring for _update_servo_width.
:operation: USW_PLS|USW_SUB|USW_OBS
:value: USW_OBS will be use.
:returns: current width; return 0 if operation error.
"""
from RPIO import PWM
wkey = 'wx_servo'
servo = PWM.Servo()
mc = memcache.Client(['192.168.1.12:11211'], debug=0)
width = mc.get(wkey)
if width is None:
mc.set(wkey, 45)
width = 45
if operation == USW_PLS:
width += 30
elif operation == USW_SUB:
width -= 30
elif operation == USW_OBS:
width = value
else:
return 0
if width > 205:
width = 205
elif width < 45:
width = 45
mc.replace(wkey, width)
servo.set_servo(17, width * 10)
return width
def __init__(self,
mId,
pin,
kv=1000,
RPMMin=1,
RPMMax=100,
debug=True,
simulation=True):
self.__mId = mId
self.__pin = pin
self.__kv = kv
if RPMMin < 0:
RPMMin = 0
self.__RPMMin = RPMMin
if RPMMax > 100:
RPMMax = 100
self.__RPMMax = RPMMax
self.debug = debug
self.simulation = simulation
self.__RPM = 0
self.__RPMEquil = 50
if self.simulation:
return
from RPIO import PWM
self.__IO = PWM.Servo()
def get_servo():
try:
from RPIO import PWM
return PWM.Servo()
except SystemError:
print("Not runnig on RPi.")
return ConsoleServo()
def __init__(self):
global stop
self.servo = PWM.Servo()
# default values
self.freq = stop
self.port = 18
def kickServo():
#hmm RPIO seems to f**k up occasionally and start sending crappy
#commands to the servo. I'm not sure what causes it except it
#may be related to USB camera use? not verified. in any case
#it sucks; the servo just jitters and once the problem occurs
#it appears to persist until the script is shut down cleanly
#(DMA shutdown) and reloaded. Maybe this bullshit will fix it.
#Nope... it didn't.
global servo
print "restarting servo..."
PWM.cleanup()
servo = PWM.Servo()
print "restarted."
yield
print "restarting servo again..."
PWM.cleanup()
servo = PWM.Servo()
print "restarted."
def __init__(self, pins, pwm_max=2000, pwm_min=1000, debug=False):
self.pwm_max = pwm_max
self.pwm_min = pwm_min
self.pwm_range = pwm_max - pwm_min
self.pins = pins
self.is_setup = False
self.servo = PWM.Servo(pulse_incr_us=1)
self.debug = debug
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 _init_servos(self):
"""
Initilise the servos and set them to theirretracted positions.
"""
for note, servo_info in self._servos.items():
gpio = servo_info['gpio']
servo = pwm.Servo()
self._servos[note]['servo_object'] = servo
servo.set_servo(gpio, servo_info['retract_pos'])
def run(self):
global speeds, motorpins
servo = PWM.Servo() # 主要代码
currentspeeds = [0, 0, 0, 0]
while True:
for i in range(4):
if currentspeeds[i] != speeds[i]:
servo.set_servo(motorpins[i], speeds[i]) # 主要代码
currentspeeds[i] = speeds[i]
time.sleep(0.1)
def angle_pub():
STP = 37
DIR = 35
MS1 = 33
MS2 = 31
EN = 29
GPIO.setmode(GPIO.BOARD)
GPIO.setup(STP, GPIO.OUT)
GPIO.setup(DIR, GPIO.OUT)
GPIO.setup(MS1, GPIO.OUT)
GPIO.setup(MS2, GPIO.OUT)
GPIO.setup(EN, GPIO.OUT)
GPIO.output(DIR, GPIO.LOW)
counter = 6000
GPIO.output(MS1, GPIO.LOW)
GPIO.output(MS2, GPIO.LOW)
step = 200
sleep = 0.0025
pitch = 0
yaw = 0
resolution = 1.8
## servo
pulse = 690
servo = PWM.Servo()
pub = rospy.Publisher('angle', angle, queue_size=50)
rospy.init_node('angle_pub', anonymous=True)
rate = rospy.Rate(50)
angles = angle()
angles.angle_h = 0
angles.angle_v = 0
while not rospy.is_shutdown():
GPIO.output(STP, GPIO.HIGH)
time.sleep(0.001)
GPIO.output(STP, GPIO.LOW)
time.sleep(0.001)
pub.publish(angles)
yaw = yaw + 1
angles.angle_h = yaw * resolution
angles.angle_v = pitch
if yaw == step:
yaw = 0
servo.set_servo(17, pulse)
pulse = pulse + 5
pitch = pitch + 1
# rospy.loginfo("angle_published")
rate.sleep()
if pitch >= 90:
rospy.signal_shutdown("finished")
GPIO.cleanup()
def initGPIO():
GPIO.cleanup()
GPIO.setmode(GPIO.BCM)
for outpin in OUTPINS_BCM:
print "setting up pin " + str(outpin)
GPIO.setup(outpin, GPIO.OUT)
for inpin in INPINS_BCM:
print "setting up pin " + str(inpin)
GPIO.setup(inpin, GPIO.IN)
servo = PWM.Servo()
return servo
def start(self):
"Run the procedure to init the PWM"
if not self.simulation:
try:
from RPIO import PWM
self.__IO = PWM.Servo()
self.powered = True
#TODO Decide How to manage the WMax < 100
#to keep anyhow the throttle range 0-100
except ImportError:
self.simulation = True
self.powered = False
def __init__(self):
## Setup GPIO for reading the line sensor and LED's
GPIO.setmode(GPIO.BCM)
GPIO.setup(line_sensor, GPIO.IN)
GPIO.setup(led_one, GPIO.OUT)
GPIO.setup(led_two, GPIO.OUT)
GPIO.setup(led_three, GPIO.OUT)
## Default LED's to off
GPIO.output(led_one, False)
GPIO.output(led_two, False)
GPIO.output(led_three, False)
## Set up PWM for servos
servo = PWM.Servo()
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 blink_lights(config):
width = 100
if not servo:
servo = PWM.Servo()
while True:
ans = config['comms']
if ans == 1:
width = 2000
elif ans == -1:
width = 0
else:
width = (width + 100) % 2000
servo.set_servo(config['led'], width)
time.sleep(1.0 / 20)
def __init__(self, pitch_BCM, roll_BCM):
self.servo = PWM.Servo()
self.pitch_BCM = pitch_BCM
self.roll_BCM = roll_BCM
self.period = 20
self.pitch_pwm = 10000
self.roll_pwm = 10000
self.servo.set_servo(pitch_BCM, self.pitch_pwm)
self.servo.set_servo(roll_BCM, self.roll_pwm)
def set_angle(angle):
servos = PWM.Servo()
diff_angle = diff(angle)
if (diff_angle <> 0):
servos.set_servo(18, angle2time(angle))
global lastangle
lastangle = angle
sleeptime = float((0.5 / 180) * diff_angle)
if (sleeptime < 0.01):
sleeptime = 0.01
print "sleeptime is " + str(sleeptime)
print "diff_angle is " + str(diff_angle)
time.sleep(0.5)
servos.stop_servo(18)
def start(self):
if not self.simulation:
try:
from RPIO import PWM
self.__IO = PWM.Servo()
self.powered = True
print "Motor ", self.name, " Started"
self.setW(self.__WMin)
except ImportError:
print "MOTOR ERROR ", self.name
self.simulation = True
self.powered = False
if self.simulation:
print "SIMULATION MODE FOR ", self.name
def init_servo(pin_req=14,
req_debug_info=0,
req_zero_pos=2400,
left_val_req=2400,
right_val_req=1300):
global debug_info, zero_pos, rico, left_val, right_val, zero_pos, servo, pin
pin = pin_req
debug_info = req_debug_info
left_val = left_val_req
right_val = right_val_req
zero_pos = req_zero_pos
rico = (right_val - left_val) / 180
servo = PWM.Servo()
servo.set_servo(pin, int(zero_pos))
def __init__(self, name, pin, WMin=0, WMax=100, simulation=True):
self.name = name
self.powered = False
self.simulation = simulation
self.__pin = pin
self.setWLimits(WMin, WMax)
self.__W = self.__WMin
self.__Wh = 10
try:
from RPIO import PWM
self.__IO = PWM.Servo()
except ImportError:
self.simulation = True
