def __init__(self):
# Start-up in 'paused' mode and
# handle button presses to exit paused mode
red.on()
green.off()
#from rcpy.button import modeAI, pauseAI
button.ButtonEvent.__init__(self, pause, button.ButtonEvent.PRESSED)
def toggle(self):
self.state = not self.state
if self.state:
red.on()
green.off()
else:
green.on()
red.off()
def action(self, event):
global paused
paused = not paused
if paused:
red.on()
green.off()
else:
green.on()
red.off()
def PushLights():
# Get rcpy running
rcpy.set_state(rcpy.RUNNING)
# Welcome message
print("=" * 65)
print("Welcome to PressyLights!")
print(
"Press <PAUSE> to turn on the red light, or <MODE> to turn on the green light."
)
print("Press both <PAUSE> and <MODE> to exit.")
print("-" * 65)
# Main loop
while not (mode.is_pressed() and pause.is_pressed()):
if pause.is_pressed():
red.on()
green.off()
elif mode.is_pressed():
red.off()
green.on()
# Exit message
print("Bye Beaglebone!")
print("=" * 65)
red.off()
green.off()
def main():
# import blocks and controller
from pyctrl.rc.mip import Controller
from pyctrl.block.container import Container, Input, Output
from pyctrl.block.system import System, Subtract, Differentiator, Sum, Gain
from pyctrl.block.nl import ControlledCombination, Product
from pyctrl.block import Fade, Printer
from pyctrl.system.ss import DTSS
from pyctrl.block.logic import CompareAbsWithHysterisis, SetFilter, State
from rcpy.gpio import GRN_LED, PAUSE_BTN
from rcpy.led import red
# export json?
export_json = True
# create mip
mip = Controller()
# phi is the average of the encoders
mip.add_signal('phi')
mip.add_filter('phi', Sum(gain=0.5), ['encoder1', 'encoder2'], ['phi'])
# phi dot
mip.add_signal('phi_dot')
mip.add_filter('phi_dot', Differentiator(), ['clock', 'phi'], ['phi_dot'])
# phi dot and steer reference
mip.add_signals('phi_dot_reference', 'phi_dot_reference_fade')
mip.add_signals('steer_reference', 'steer_reference_fade')
# add fade in filter
mip.add_filter('fade', Fade(target=[0, 0.5], period=5),
['clock', 'phi_dot_reference', 'steer_reference'],
['phi_dot_reference_fade', 'steer_reference_fade'])
# state-space matrices
A = np.array([[0.913134, 0.0363383], [-0.0692862, 0.994003]])
B = np.array([[0.00284353, -0.000539063], [0.00162443, -0.00128745]])
C = np.array([[-383.009, 303.07]])
D = np.array([[-1.22015, 0]])
B = 2 * np.pi * (100 / 7.4) * np.hstack((-B, B[:, 1:]))
D = 2 * np.pi * (100 / 7.4) * np.hstack((-D, D[:, 1:]))
ssctrl = DTSS(A, B, C, D)
# state-space controller
mip.add_signals('pwm')
mip.add_filter('controller', System(model=ssctrl),
['theta_dot', 'phi_dot', 'phi_dot_reference_fade'], ['pwm'])
# enable pwm only if about small_angle
mip.add_signals('small_angle', 'small_angle_pwm')
mip.add_filter('small_angle_pwm', Product(), ['small_angle', 'pwm'],
['small_angle_pwm'])
# steering biasing
mip.add_filter(
'steer', ControlledCombination(),
['steer_reference_fade', 'small_angle_pwm', 'small_angle_pwm'],
['pwm1', 'pwm2'])
# set references
mip.set_signal('phi_dot_reference', 0)
mip.set_signal('steer_reference', 0.5)
# add supervisor actions on a timer
# actions are inside a container so that they are executed all at once
mip.add_timer('supervisor',
Container(), ['theta'], ['small_angle', 'is_running'],
period=0.5,
repeat=True)
mip.add_signals('timer/supervisor/theta', 'timer/supervisor/small_angle')
mip.add_source('timer/supervisor/theta', Input(), ['theta'])
mip.add_sink('timer/supervisor/small_angle', Output(), ['small_angle'])
mip.add_sink('timer/supervisor/is_running', Output(), ['is_running'])
# add small angle sensor
mip.add_filter(
'timer/supervisor/is_angle_small',
CompareAbsWithHysterisis(threshold=0.11,
hysterisis=0.09,
offset=-0.07,
state=(State.LOW, )), ['theta'],
['small_angle'])
# reset controller and fade
mip.add_sink(
'timer/supervisor/reset_controller',
SetFilter(label=['/controller', '/fade'], on_rise={'reset': True}),
['small_angle'])
# add green led
mip.add_sink('timer/supervisor/green_led', ('pyctrl.rc.led', 'LED'),
['small_angle'],
kwargs={'pin': GRN_LED},
enable=True)
# add pause button on a timer
mip.add_source('timer/supervisor/pause_button',
('pyctrl.rc.button', 'Button'), ['is_running'],
kwargs={
'pin': PAUSE_BTN,
'invert': True
},
enable=True)
# print controller
print(mip.info('all'))
# export json?
if export_json:
from pyctrl.flask import JSONEncoder
# export controller as json
json = JSONEncoder(sort_keys=True, indent=4).encode(mip)
with open('rc_mip_balance.json', 'w') as f:
f.write(json)
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
print("""
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* M I P B A L A N C E *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
""")
print("""
Hold your MIP upright to start balancing
Use your keyboard to control the mip:
* UP and DOWN arrows move forward and back
* LEFT and RIGHT arrows steer
* / stops forward motion
* . stops steering
* SPACE resets forward motion and steering
""")
# reset everything
mip.set_source('clock', reset=True)
mip.set_source('encoder1', reset=True)
mip.set_source('encoder2', reset=True)
mip.set_filter('controller', reset=True)
mip.set_source('inclinometer', reset=True)
# turn on red led
red.on()
# start the controller
mip.start()
print("Press Ctrl-C or press the <PAUSE> button to exit")
# fire thread to update velocities
thread = threading.Thread(target=get_arrows, args=(mip, fd))
thread.daemon = False
thread.start()
# and wait until controller dies
mip.join()
# print message
print("\nDone with balancing")
except KeyboardInterrupt:
print("\nBalancing aborted")
finally:
# turn off red led
red.off()
# make sure it exits
mip.set_state(pyctrl.EXITING)
print("Press any key to exit")
thread.join()
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
def main():
# import blocks and controller
from pyctrl.rc.mip import Controller
from pyctrl.block.container import Container, Input, Output
from pyctrl.block.system import System, Subtract, Differentiator, Sum, Gain
from pyctrl.block.nl import ControlledCombination, Product
from pyctrl.block import Fade, Printer
from pyctrl.system.ss import DTSS
from pyctrl.block.logic import CompareAbsWithHysterisis, SetFilter, State
from rcpy.gpio import GRN_LED, PAUSE_BTN
from rcpy.led import red
# export json?
export_json = True
# create mip
mip = Controller()
# phi is the average of the encoders
mip.add_signal('phi')
mip.add_filter('phi', Sum(gain=0.5), ['encoder1', 'encoder2'], ['phi'])
# phi dot
mip.add_signal('phi_dot')
mip.add_filter('phi_dot', Differentiator(), ['clock', 'phi'], ['phi_dot'])
# phi dot and steer reference
mip.add_signals('pwm', 'pwm_fade')
mip.add_signals('steer_reference', 'steer_reference_fade')
# add fade in filter
mip.add_filter('fade', Fade(target=[0, 0.5],
period=5), ['clock', 'pwm', 'steer_reference'],
['pwm_fade', 'steer_reference_fade'])
# steering biasing
mip.add_filter('steer', ControlledCombination(),
['steer_reference_fade', 'pwm_fade', 'pwm_fade'],
['pwm1', 'pwm2'])
# set references
mip.set_signal('steer_reference', 0.5)
# print controller
print(mip.info('all'))
# export json?
if export_json:
from pyctrl.flask import JSONEncoder
# export controller as json
json = JSONEncoder(sort_keys=True, indent=4).encode(mip)
with open('rc_mip_drive.json', 'w') as f:
f.write(json)
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
print("""
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* M I P D R I V E *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
""")
print("""
Use your keyboard to drive the mip as a car:
* UP and DOWN arrows move forward and back
* LEFT and RIGHT arrows steer
* / stops forward motion
* . stops steering
* SPACE resets forward motion and steering
""")
# reset everything
mip.set_source('clock', reset=True)
mip.set_source('encoder1', reset=True)
mip.set_source('encoder2', reset=True)
mip.set_source('inclinometer', reset=True)
# turn on red led
red.on()
# start the controller
mip.start()
print("Press Ctrl-C or press the <PAUSE> button to exit")
# fire thread to update velocities
thread = threading.Thread(target=get_arrows, args=(mip, fd))
thread.daemon = False
thread.start()
# and wait until controller dies
mip.join()
# print message
print("\nDone with driving")
except KeyboardInterrupt:
print("\nDriving aborted")
finally:
# turn off red led
red.off()
# make sure it exits
mip.set_state(pyctrl.EXITING)
print("Press any key to exit")
thread.join()
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
def __init__(self):
# Start-up in 'paused' mode and handle button presses to exit paused mode
red.on()
green.off()
#button.ButtonEvent.__init__(self, pause, button.ButtonEvent.PRESSED)
button.ButtonEvent.__init__(self, pauseAI, button.ButtonEvent.PRESSED)
# Redirect input/output to a socket
SOCK_OUT = 3001
SOCK_IN = 3002
sock_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock_out.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
sock_out.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
sock_out.connect(("255.255.255.255", SOCK_OUT))
sys.stdout = sock_out.makefile('w', buffering=None)
#sock_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
#sock_in.connect(("127.0.0.1", SOCK_IN))
#sys.stdin = sock_in.makefile('r', buffering=None)
# Start-up in 'paused' mode and handle button presses to exit paused mode
paused = True
red.on()
green.off()
class PauseButtonEvent(button.ButtonEvent):
def action(self, event):
global paused
paused = not paused
if paused:
red.on()
green.off()
else:
green.on()
red.off()
