Пример #1
0
import time
import sys
sys.path.append('..')
from getch import getch

from RealWorldKit import *
# from ControlsKit import logger
from ControlsKit.import_planner import importPlanner
from ControlsKit.filters import LowPassFilter
from Utilities.pubsub import *
import time
import threading

# Check command-line arguments to find the planner module
retval = importPlanner()
# FIXME: This is to allow backwards compatibility to older versions of importPlanner
if type(retval) == tuple:
    update, controller = retval
else:
    update = retval
    controller = None

leg1 = ControlBus(device='/dev/ttyUSB1')
yaw_valve = ValveNode(leg1       , node_id=1 , name="yaw_valve"     , bore=0.0254 , rod=0.0189        , lpm=22.712 , e_deadband=89 , r_deadband=75)
pitch_valve = ValveNode(leg1     , node_id=2 , name="pitch_valve"   , bore=0.0381 , rod=0.0254        , lpm=22.712 , e_deadband=77 , r_deadband=70)
knee_valve = ValveNode(leg1      , node_id=3 , name="knee_valve"    , bore=0.0254 , rod=0.0159        , lpm=22.712 , e_deadband=87 , r_deadband=69)
yaw_encoder = EncoderNode(leg1   , node_id=4 , name="yaw_encoder"   , gain=1      , offset=35 * pi / 180 , bias=(1200 , 1200))
pitch_encoder = EncoderNode(leg1 , node_id=5 , name="pitch_encoder" , gain=-1     , offset=45 * pi / 180 , bias=(1200 , 1200))
knee_encoder = EncoderNode(leg1  , node_id=6 , name="knee_encoder"  , gain=1      , offset=225 * pi / 180 , bias=(1200 , 1200))
shock_encoder = EncoderNode(leg1 , node_id=7 , name="shock_encoder" , gain=1      , offset=0          , bias=(1200 , 1200))
Пример #2
0
import sys
sys.path.append('..')
sys.path.append('planners')

from SimulationKit import Simulator
from SimulationKit.Robots import SpiderWHydraulics
from SimulationKit.helpers import *
from ControlsKit.import_planner import importPlanner
from UI import InputServer, logger

# Check command-line arguments to find the planner module
update = importPlanner()


d = {'offset':(0,0,1.0)}
s = Simulator(dt=1e-3,plane=1,pave=0,graphical=1,robot=SpiderWHydraulics,robot_kwargs=d, start_paused = True)
input_server = InputServer()
input_server.startListening()


try:
    last_command = None
    while True:
        s.step()
        if not s.getPaused():
            time = s.getSimTime()+.0001 # FIXME: first time time_delta is called, it returns zero, which means pid commands infinity
            # FIXME: Known bug, getAcceleration returns (0,0,0)
            next_command = input_server.getLastCommand()
            command = next_command if next_command != last_command else None
            last_command = next_command
#            print command[15]
Пример #3
0
from SimulationKit.helpers import *
from ControlsKit.import_planner import importPlanner
from Utilities.pubsub import Publisher


def keyboardInterruptHandler(sig, frame):
    print 'Caught keyboard interrupt!'
    print "Goodbye!"
    exit(0)


import signal
signal.signal(signal.SIGINT, keyboardInterruptHandler)

# Check command-line arguments to find the planner module
retval = importPlanner()
# FIXME: This is to allow backwards compatibility to older versions of importPlanner
if type(retval) == tuple:
    if len(retval) == 3:
        update, controller, graceful_exit = retval
    elif len(retval) == 2:
        update, controller = retval
else:
    update = retval
    controller = None

commands = [0, 0, 0]

publisher = Publisher(5055)
if controller != None:
    publisher.addToCatalog('yaw_ang_target', controller.getDesiredYawDeg)
Пример #4
0
import sys
sys.path.append('..')
sys.path.append('planners')

from SimulationKit import Simulator
from SimulationKit.Robots import SpiderWHydraulics
from SimulationKit.helpers import *
from ControlsKit.import_planner import importPlanner
from UI import InputServer, logger

# Check command-line arguments to find the planner module
update = importPlanner()

d = {'offset': (0, 0, 1.0)}
s = Simulator(dt=1e-3,
              plane=1,
              pave=0,
              graphical=1,
              robot=SpiderWHydraulics,
              robot_kwargs=d,
              start_paused=True)
input_server = InputServer()
input_server.startListening()

try:
    last_command = None
    while True:
        s.step()
        if not s.getPaused():
            time = s.getSimTime(
            ) + .0001  # FIXME: first time time_delta is called, it returns zero, which means pid commands infinity