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 engine(angle):
#PWM.set_duty_cycle("P9_14", (angle/36)+5)
if angle < 0:
angle = 0
elif angle > 180:
angle = 180
PWM.set_duty_cycle("P9_14", (angle+60)/18)
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 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 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 testH():
PWM.set_duty_cycle(EnA, 100)
PWM.set_duty_cycle(EnB, 100)
while(1):
GPIO.output(I1A, GPIO.HIGH)
GPIO.output(I2A, GPIO.LOW)
GPIO.output(I1B, GPIO.HIGH)
GPIO.output(I2B, GPIO.LOW)
time.sleep(1)
#Rotate
GPIO.output(I1B, GPIO.LOW)
GPIO.output(I2B, GPIO.LOW)
time.sleep(1)
#Invert
GPIO.output(I1A, GPIO.LOW)
GPIO.output(I2A, GPIO.HIGH)
GPIO.output(I1B, GPIO.LOW)
GPIO.output(I2B, GPIO.HIGH)
time.sleep(1)
#Rotate
GPIO.output(I1A, GPIO.LOW)
GPIO.output(I2A, GPIO.LOW)
time.sleep(1)
def speed(self, duty=55, offset=0): if offset>15: x=15 elif offset<-15: x=-15 else: x=offset print 'duty ',duty if duty>75 or duty<-20: print 'bad pwm' return else: print ' ' #smaller offset for high speeds, assumed top offset of 15 i=x/15#i between -1 and 1 mult=-.19*abs(duty)+25 #mult=-.199*abs(duty)+25 x=mult*i print 'effective offset ', x #check if negative l_speed=duty+x r_speed=duty-x if l_speed<0:#negative speed, reverse GPIO.output(self.left_wheel_dir, GPIO.LOW) else: GPIO.output(self.left_wheel_dir, GPIO.HIGH) if r_speed<0:#negative speed, reverse GPIO.output(self.right_wheel_dir, GPIO.LOW) else: GPIO.output(self.right_wheel_dir, GPIO.HIGH) PWM.set_duty_cycle(self.left_wheel, abs(l_speed)) PWM.set_duty_cycle(self.right_wheel, abs(r_speed))
def listener():
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("turn_rate", Float32, callback)
rospy.spin()
print ("test")
PWM.stop(pwm_pin)
PWM.cleanup()
def OnAdjust(self,event):
#val = self.sld.GetValue()
#self.SetSize((val*2,val))
valuex1 = acc.readU8(0x32)
valuex2 = acc.readU8(0x33)
valuex = (valuex2 << 8) + valuex1
if valuex > (1<<15):
valuex = int(bin(valuex),2)-(1<<16)
#print (bin(value),2)
print "val %i, val1: %i, val2: %i\n" % (valuex, valuex1, valuex2)
PWM.set_duty_cycle("P9_14",((valuex+512.0)/1024.0)*100.0)
valuey1 = acc.readU8(0x34)
valuey2 = acc.readU8(0x35)
valuey = (valuey2 << 8) + valuey1
if valuey > (1<<15):
valuey = int(bin(valuey),2)-(1<<16)
valuez1 = acc.readU8(0x36)
valuez2 = acc.readU8(0x37)
valuez = (valuez2 << 8) + valuez1
if valuez > (1<<15):
valuez = int(bin(valuez),2)-(1<<16)
self.sldx.SetValue(valuex+512)
self.sldy.SetValue(valuey+512)
self.sldz.SetValue(valuez+512)
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 center(self):
"""
center: Center the servomotor.
"""
PWM.set_duty_cycle(self.pin, self.duty_min+(self.duty_max-self.duty_min)/2)
return
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 _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 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 leftRear(self, motorspeed):
# motorspeed -100-100,
if motorspeed < 0:
Channel_A = 1
Channel_B = 0
elif motorspeed > 0:
Channel_A = 0
Channel_B = 1
elif motorspeed == 0:
Channel_A = 0
Channel_B = 0
# convert speed to a positive number
motorspeed = abs(motorspeed)
# makes sure motorspeed doesn't go out of range
if motorspeed > 100:
motorspeed = 100
# pwm for motor 2
PWM.set_duty_cycle("P9_16", motorspeed)
# left Side
# writes to motor 2
GPIO.output("P9_13", Channel_A)
GPIO.output("P9_15", Channel_B)
def rightFront(self, motorspeed):
# motorspeed -100-100,
if motorspeed < 0:
Channel_A = 0
Channel_B = 1
elif motorspeed > 0:
Channel_A = 1
Channel_B = 0
elif motorspeed == 0:
Channel_A = 0
Channel_B = 0
# convert speed to a positive number
motorspeed = abs(motorspeed)
# makes sure motorspeed doesn't go out of range
if motorspeed > 100:
motorspeed = 100
# pwm for motor 3
PWM.set_duty_cycle("P8_13", motorspeed)
# right side
# writes to motor 3
GPIO.output("P8_14", Channel_A)
GPIO.output("P8_16", Channel_B)
def set_servo_position(self):
"""
Set a servo position
:return:
"""
duty_min = 3
duty_max = 14.5
duty_span = duty_max - duty_min
self.last_problem = '7-0\n'
pin = self.validate_pin(self.pwm_pins)
if pin == 99:
self.last_problem = '7-1\n'
return
index = self.pwm_pins.index(pin)
pin_entry = self.pwm_pin_states[index]
if not pin_entry['enabled']:
self.last_problem = '7-1\n'
return
position = int(self.payload['position'])
angle_f = float(position)
duty = 100 - ((angle_f / 180) * duty_span + duty_min)
PWM.set_duty_cycle(pin, duty)
def do_blink():
print 'running LEDbar'
r_speeds = redis.StrictRedis(host='localhost', port=6379, db=0)
# get last 60 results (ie LRANGE times 0 59)
# find max and min
# scale most recent result to max and min
# tell LEDbar to be that color
recent = r_speeds.lrange('times',0,59)
pingAv = []
ulAv = []
dlAv = []
for event in recent:
# redis stores dict as a string, this is working ok to re-dict
event = ast.literal_eval(event)
for entry in event:
if entry=='UL':
ulAv.append(float(event[entry]))
if entry=='DL':
dlAv.append(float(event[entry]))
if entry=='ping':
pingAv.append(float(event[entry]))
current = ast.literal_eval(recent[0])
print current
ulOutput = mapVals(float(current['UL']), min(ulAv), max(ulAv),0.0,100.0)
dlOutput = mapVals(float(current['DL']), min(dlAv),max(dlAv),0,100.0)
pingOutput = 10.0*mapVals(float(current['ping']), min(pingAv),max(pingAv),0,100.0) # times 10 just for pingtime to be noticeable
print ulOutput, dlOutput, pingOutput
pwm.set_duty_cycle(redPin, dlOutput+0.0)
pwm.set_duty_cycle(greenPin, 100.0-dlOutput)
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 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 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 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 analog_write(self):
"""
Set a PWM configured pin to the requested value
:return: None
"""
# clear out any residual problem strings
self.last_problem = '4-0\n'
pin = self.validate_pin(self.pwm_pins)
if pin == 99:
self.last_problem = '4-1\n'
return
# get pin information
index = self.pwm_pins.index(pin)
pin_entry = self.pwm_pin_states[index]
if not pin_entry['enabled']:
self.last_problem = '4-2\n'
return
value = float(self.payload['value'])
if not (0 <= value <= 100):
self.last_problem = '4-3\n'
PWM.set_duty_cycle(pin, value)
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 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_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 disable():
global ENABLED, RUNNING
if not ENABLED: return
PWM.stop(LEFT_SERVO_TX)
PWM.stop(RIGHT_SERVO_TX)
ENABLED = False
RUNNING = False
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 speed(self, duty=55, offset=0): print 'duty ',duty if duty>83 or duty<-20: print 'bad pwm' return else: print ' ' #check if negative l_speed=duty+offset r_speed=duty-offset if l_speed<0:#negative speed, reverse GPIO.output(self.left_wheel_dir, GPIO.LOW) if l_speed<-20: l_speed=-20 elif l_speed>85:#check if speed too high l_speed=85 GPIO.output(self.left_wheel_dir, GPIO.HIGH) else: GPIO.output(self.left_wheel_dir, GPIO.HIGH) if r_speed<0:#negative speed, reverse GPIO.output(self.right_wheel_dir, GPIO.LOW) if r_speed<-20: r_speed=-20 elif r_speed>85:#check if speed too high r_speed=85 GPIO.output(self.right_wheel_dir, GPIO.HIGH) else: GPIO.output(self.right_wheel_dir, GPIO.HIGH) PWM.set_duty_cycle(self.left_wheel, abs(l_speed)) PWM.set_duty_cycle(self.right_wheel, abs(r_speed))
def zelda_Song_of_Time():
"""Plays the Song of Time from The Legend of Zelda."""
play_note(NOTE_A4, 0.5)
play_note(NOTE_D4, 1.0)
play_note(NOTE_F4, 0.5)
play_note(NOTE_A4, 0.5)
play_note(NOTE_D4, 1.0)
play_note(NOTE_F4, 0.5)
play_note(NOTE_A4, 0.25)
play_note(NOTE_C5, 0.25)
play_note(NOTE_B4, 0.5)
play_note(NOTE_G4, 0.5)
play_note(NOTE_F4, 0.25)
play_note(NOTE_G4, 0.25)
play_note(NOTE_A4, 0.5)
play_note(NOTE_D4, 0.5)
play_note(NOTE_C4, 0.25)
play_note(NOTE_E4, 0.25)
play_note(NOTE_D4, 1.5)
PWM.stop(piezo_pin)
PWM.cleanup()
def CasPID():
rootLogger.info("Arriving in CasPID State. ")
PID = [0.008, 0.020, .01]
CasCtr = ctrlib.PidController_WindUp(PID, TSAMPLING, max_output=1.)
while llc_ref.state == 'CASPID':
if IMU and is_poti():
read_imu()
calc_angle(self)
#referenz über pattern
pattern_ref(patternname='pattern_0.csv', alpha=True)
for name in CHANNELset:
aref = llc_ref.alpha[name]
pref = CasCtr.output(aref, llc_rec.aIMU[name])
pwm = pwm = calibration.cut_off(pref * 100, 100)
PWM.set_duty_cycle(OUT[name], pwm)
llc_rec.u[name] = pwm
time.sleep(self.sampling_time)
return llc_ref.state
class Servol:
servolr = 'P9_16'
servoll = 'P9_14'
PWM.start(servolr, 100, 50)
PWM.start(servoll, 100, 50)
def set_pwm(self, pwml, pwmr):
if pwml > 100:
pwml = 100
elif pwml < -100:
pwml = -100
if pwmr > 100:
pwmr = 100
elif pwmr < -100:
pwmr = -100
PWM.set_duty_cycle(self.servolr, 100 - abs(pwmr))
PWM.set_duty_cycle(self.servoll, 100 - abs(pwml))
def write_voltage(v):
# direction
if v > 0: # should turn motor cw from rest
pin1 = GPIO.LOW
pin2 = GPIO.HIGH
else:
pin1 = GPIO.HIGH
pin2 = GPIO.LOW
# duty cycle: percentage of full-scale
# note: pwm can be set w/ non-ints.
duty = abs(1.0 * v / MAX_VOLTAGE) * 100.0
# limits
if duty > 100: duty = 100
if duty < 0: duty = 0
GPIO.output(MY_PWM_DIR_PIN_1, pin1)
GPIO.output(MY_PWM_DIR_PIN_2, pin2)
PWM.set_duty_cycle(MY_PWM_PIN, duty)
def stayStragightUntilOutOfBox():
base_speed = 15
GPIO.output(Left_Motor_Direction,
GPIO.HIGH) #set left motor to go forward.
GPIO.output(Right_Motor_Direction, GPIO.HIGH) #right motor forward
sensorVals = getSensorVals(ser)
while sensorVals < 14 * [
black_line
]: #keep going until one of the sensors reads a black line.
PWM.set_duty_cycle(Left_Motor_Pin, base_speed)
PWM.set_duty_cycle(Right_Motor_Pin, base_speed)
PWM.set_duty_cycle(Left_Motor_Pin, 0)
PWM.set_duty_cycle(Right_Motor_Pin, 0)
return
def run(self, speed):
"""
Makes motor to run (or stop).
Parameters:
|speed| is a float value in the range [-100.0, 100.0].
Zero means "stop motor"
100.0 means "run forward at full speed"
-100.0 means "run backward at full speed"
"""
self.speed = min(max(speed, -self.max_speed), self.max_speed)
if self.speed > 0:
GPIO.output(self._dir1_pin, GPIO.LOW)
GPIO.output(self._dir2_pin, GPIO.HIGH)
PWM.set_duty_cycle(self._pwm_pin, abs(self.speed))
elif self.speed < 0:
GPIO.output(self._dir1_pin, GPIO.HIGH)
GPIO.output(self._dir2_pin, GPIO.LOW)
PWM.set_duty_cycle(self._pwm_pin, abs(self.speed))
else:
GPIO.output(self._dir1_pin, GPIO.LOW)
GPIO.output(self._dir2_pin, GPIO.LOW)
PWM.set_duty_cycle(self._pwm_pin, 0)
async def handle_alarm():
while True:
# Wait for one of the two events
h = asyncio.ensure_future(smoke_event.wait())
b = asyncio.ensure_future(button_event.wait())
done, pending = await asyncio.wait({h, b},
return_when=asyncio.FIRST_COMPLETED)
# Cancel the other waiting task
for task in pending:
task.cancel()
# If there is smoke, set the alarm
smoke_event.clear()
async with smoke_lock:
smoke = is_smoke
if smoke:
print("Fire!")
PWM.start("P8_13", 25, 1000)
else:
# Otherwise, check to see if the button has been pressed,
# and if so clear the alarm
if button_event.is_set():
print("Reset!")
PWM.stop("P8_13")
PWM.cleanup()
button_event.clear()
def fade(colorA, colorB, ignore_color): PWM.set_duty_cycle(ignore_color, 100) for i in range(0, 100): PWM.set_duty_cycle(colorA, i) PWM.set_duty_cycle(colorB, 100 - i) time.sleep(0.05)
def shutItDown():
#turn pump off
PWM.start(SERVO_PIN, MINDUT, SERVO_FREQUENCY, 0)
PWM.stop(SERVO_PIN)
GPIO.output(OUT_PIN, GPIO.LOW)
GPIO.cleanup()
PWM.cleanup()
def __init__(self):
""" get everything going """
# (motor, duty, frequency, polarity)
print 'Signals Started'
PWM.start(Motor.motor1, Motor.duty_stop, 60.0)
PWM.start(Motor.motor2, Motor.duty_stop, 60.0)
PWM.start(Motor.motor3, Motor.duty_stop, 60.0)
def __init__(self):
signal.signal(signal.SIGINT, self.exit_signal)
self.zmq_context = zmq.Context()
self.setup_subscriber()
#setup flatbuffers
self.fb_builder = flatbuffers.Builder(1024)
self.motor_command_msg = None
self.motor_1_command = 0.0
self.motor_2_command = 0.0
self.motor_3_command = 0.0
#setup motors
self.pwm_freq = 8000.0 #8000 Hz max for BlueRobotics ESC
self.pwm_stop = 1500 #motor esc stops at 1500 microseconds
PWM.start("P9_21", self.pwm_stop / (1e6 / self.pwm_freq),
self.pwm_freq) #motor 1 beaglebone PWM pin
PWM.start("P9_22", self.pwm_stop / (1e6 / self.pwm_freq),
self.pwm_freq) #motor 2 beaglebone PWM pin
PWM.start("P9_16", self.pwm_stop / (1e6 / self.pwm_freq),
self.pwm_freq) #motor 3 beaglebone PWM pin
#check BlueRobotics website for current draw - https://bluerobotics.com/store/thrusters/t100-t200-thrusters/t200-thruster/
#limit ourselves to ~6.5A
self.pwm_min = 1275 #max reverse microseconds pwm value
self.pwm_max = 1725 #max forward microseconds pwm value
self.pwm_deadband = 40 #deadband around 1500 microseconds of 40 microseconds
self.command_min = -80.0
self.command_max = 80.0
self.command_deadband = 2.0
time.sleep(10.0)
def shutDown(self):
#shutdown PWM cleanly
PWM.stop(pan_pin)
PWM.stop(tilt_pin)
PWM.cleanup()
#shutdown cherrypy cleanly
cherrypy.engine.exit()
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 set_color_rgb(self, rgbcolor): PWM.set_duty_cycle(self.r_pin,rgbcolor[0]/2.55) PWM.set_duty_cycle(self.g_pin,rgbcolor[1]/2.55) PWM.set_duty_cycle(self.b_pin,rgbcolor[2]/2.55) #print(rgbcolor) #Storing color self.color_rgb = rgbcolor
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 set_enabled(self, enabled=True):
# call super
super().set_enabled(enabled)
# raise enable pin?
if enabled and self.enable_pin:
#print('> Raising enable_pin')
GPIO.output(self.enable_pin, 1)
if not enabled:
# wait
time.sleep(0.1)
# and write 0 to motor
PWM.set_duty_cycle(self.pwm_pin, 0)
# lower enable pin?
if self.enable_pin:
#print('> Lowering enable_pin')
GPIO.output(self.enable_pin, 0)
def initialize():
global pub
rospy.on_shutdown(shutItDown)
PWM.start(SERVO_PIN, MINDUT, SERVO_FREQUENCY, 0)
rospy.init_node('pump_gpio_control', anonymous=True)
pub = rospy.Publisher('ef_status', EFStatus, queue_size=1000, latch=True)
#GPIO.setup(OUT_PIN, GPIO.OUT)
#GPIO.setup(IN_PIN, GPIO.IN)
#initialize suckage
#GPIO.output(OUT_PIN, GPIO.HIGH)
#reset servo just in case
#angle = 0.0
#move in to grab
#while ((not GPIO.input(IN_PIN)) and (angle < ANGLE_LOWERED)):
# dutycyc = MINDUT + (MAXDUT-MINDUT)*(angle/SERVOMAX)
# PWM.start(SERVO_PIN, dutycyc, SERVO_FREQUENCY, 0)
# angle += 5
# rospy.sleep(0.25)
rospy.Subscriber('ef_command', EFCommand, commandlistener)
rospy.spin()
def pwmLED(led, name, level):
if isNumber(level):
levelValue = int(level)
else:
levelValue = -1
if (levelValue >= 0) and (levelValue <= 100):
PWM.set_duty_cycle(led, float(levelValue))
if levelValue > 0:
st = "The " + name + " LED's brightness has been set to " + str(
levelValue) + "%."
elif levelValue == 0:
st = "The " + name + " LED has been turned off."
else:
st = "You have requested an invalid brightness level for the " + name + " LED!!\n\nPlease use numbers from 0 to 100."
templateData = {"title": "Set the " + name + " LED brightness", "str": st}
temp = render_template("brightness.html", **templateData)
return temp
def run(self):
"""Class run method"""
global recording
global Timbre
while (1):
# While the playback switch is set to 1, don't playback
while (GPIO.input(playback_switch) == 1):
pass
song = recording
# While playback switch is 0, read through the song file and play
while (GPIO.input(playback_switch) == 0):
for i in range(0, len(song)):
print('ready')
PWM.start("P2_3", Timbre, song[i][0])
time.sleep(song[i][2] - song[i][1])
PWM.start("P2_3", 0, 100)
try:
time.sleep(song[i + 1][1] - song[i][2])
except:
pass
return
def commandlistener(cmdmessage):
GPIO.setup(OUT_PIN, GPIO.OUT)
GPIO.setup(IN_PIN, GPIO.IN)
rospy.sleep(2)
msg = EFStatus()
msg.status = msg.EF_BUSY
pub.publish(msg)
if (cmdmessage.command == cmdmessage.CMD_GRAB):
#initialize suckage
GPIO.output(OUT_PIN, GPIO.HIGH)
#reset servo just in case
angle = 0.0
#move in to grab
while ((not GPIO.input(IN_PIN)) and (angle < ANGLE_LOWERED)):
dutycyc = MINDUT + (MAXDUT - MINDUT) * (angle / SERVOMAX)
PWM.start(SERVO_PIN, dutycyc, SERVO_FREQUENCY, 0)
angle += 5
rospy.sleep(0.25)
if GPIO.input(IN_PIN):
#we have contacted something
#carefully retract
while ((angle > ANGLE_RAISED)):
dutycyc = MINDUT + (MAXDUT - MINDUT) * (angle / SERVOMAX)
PWM.start(SERVO_PIN, dutycyc, SERVO_FREQUENCY, 0)
angle -= 5
rospy.sleep(0.5)
if not (GPIO.input(IN_PIN)):
#we dropped it or never got it in the first place
while ((angle > ANGLE_RAISED)):
dutycyc = MINDUT + (MAXDUT - MINDUT) * (angle / SERVOMAX)
PWM.start(SERVO_PIN, dutycyc, SERVO_FREQUENCY, 0)
angle -= 5
rospy.sleep(0.5)
#for now, retract as usual
#we made it this far, this probably still have it
msg = EFStatus()
msg.status = msg.EF_READY
pub.publish(msg)
else:
#kinda like in reverse
GPIO.output(OUT_PIN, GPIO.LOW) #end suckage
#reset angle
angle = 0.0
dutycyc = MINDUT + (MAXDUT - MINDUT) * (angle / SERVOMAX)
PWM.start(SERVO_PIN, dutycyc, SERVO_FREQUENCY, 0)
msg = EFStatus()
msg.status = msg.EF_READY
pub.publish(msg)
def enable_motor(self, motor="M1", direction="CW", duty_cycle=75):
if direction == "CW":
try:
GPIO.output(getattr(self, motor + "_IN1"), GPIO.HIGH)
GPIO.output(getattr(self, motor + "_IN2"), GPIO.LOW)
PWM.start(getattr(self, motor + "_EN"), duty_cycle)
except:
print "Unexpected error:"
raise
sys.exit()
else:
try:
GPIO.output(getattr(self, motor + "_IN1"), GPIO.LOW)
GPIO.output(getattr(self, motor + "_IN2"), GPIO.HIGH)
PWM.start(getattr(self, motor + "_EN"), duty_cycle)
except:
print "Unexpected error:"
raise
sys.exit()
print "motor ", motor, " enabled"
self.motor_enabled[motor] = True
def callback(self, msg):
x = msg.transform.translation.x
y = msg.transform.translation.y
z = msg.transform.translation.z
x = (x - STAIR_TOP) / STAIR_LENGTH
if x < DESIRED_LOCATION:
x = 0
dutycycle = self.step(x)
if dutycycle < 0:
dutycycle = 0
# Change dutycycle from percentage to 0 ~ 100
dutycycle = dutycycle * 100
# Display position info on log screen
rospy.loginfo('x: {}, PWM: {}'.format(x, dutycycle))
PWM.set_duty_cycle(speedPin, dutycycle)
def _init_(self):
#Initialize the PWM generators
#print ('Initialized')
#(Motor, Duty Frequency, Polarity)
#Check the ESC setup for frequency
PWM.start(Motor.motor1, Motor.duty_stop, 60)
PWM.start(Motor.motor2, Motor.duty_stop, 60)
PWM.start(Motor.motor3, Motor.duty_stop, 60)
def __init__(self,pin):
"""
When started, the library puts the servo at -90°
"""
self.__pin=pin
PWM.start(self.__pin, 0) #PWM set at 0% so the servo goes to -90° (50% = 0° and 100%= 90°)
PWM.stop(self.__pin)#Close the PWM connection
PWM.cleanup()
def cleanup(cls):
PWM.stop(PWMA)
PWM.stop(PWMB)
PWM.cleanup()
GPIO.cleanup()
GPIO.output(STBY, GPIO.LOW)
def SetAngle(self,input):
"""
Recieve an input from 0 to 100 and sets the position following this input
"""
PWM.set_duty_cycle(self.__pin, input)
PWM.stop(self.__pin)
PWM.cleanup()
def __init__(self):
#initilize all PWM Beaglebone Outputs
duty_min = 4.0
duty_max = 10.0
#initilize ESC and Motors
PWM.start(self.motor1, duty_min, 55.0, 1)
PWM.start(self.motor2, duty_min, 55.0, 1)
PWM.start(self.motor3, duty_min, 55.0, 1)
PWM.start(self.motor4, duty_min, 55.0, 1)
#time.sleep(.5)
#PWM.set_duty_cycle(self.motor1, duty_min)
#PWM.set_duty_cycle(self.motor2, duty_min)
#PWM.set_duty_cycle(self.motor3, duty_min)
#PWM.set_duty_cycle(self.motor4, duty_min)
print "motors set"
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 write_outputs_ao(io_num=None, val=None, pri=None):
gpio = analog_out(io_num)
io_num = io_num.upper()
if gpio == -1:
return jsonify({
'1_state': "unknownType",
'2_ioNum': io_num,
'3_gpio': gpio,
'4_val': val,
"5_msg": analogOutTypes
}), http_error
elif is_float(val):
val = float(val)
if 0 <= val <= 100:
PWM.set_duty_cycle(gpio, val) # !! GPIO CALL !!!
return jsonify({
'1_state': "writeOk",
'2_ioNum': io_num,
'3_gpio': gpio,
'4_val': val,
'5_msg': 'wrote value to the GPIO'
}), http_success
else:
return jsonify({
'1_state': "unknownRange",
'2_ioNum': io_num,
'3_gpio': gpio,
'4_val': val,
'5_msg': 'val must be >=0 and <=100'
}), http_success
else:
return jsonify({
'1_state': "unknownValue",
'2_ioNum': io_num,
'3_gpio': gpio,
'4_val': val,
'5_msg': 'value must be a float'
}), http_error
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()
