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))
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]
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)
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