# controller.py : skeleton Webots API to connect the controller objects to the physical hardware. # No copyright, 2021, Garth Zeglin. This file is explicitly placed in the public domain. import time, threading, queue, math ################################################################ class Robot(object): """This object emulates the portions of the Webots API common to all physical robots.""" def __init__(self): print("Entering Robot.__init__ and initializing superclasses") super(Robot, self).__init__() # include a receiver/emitter pair by default self.emitter = Emitter() self.receiver = Receiver() # keep track of elapsed time self._time = 0.0 # lock to synchronize access for this object and its children self._lock = threading.Lock() def step(self, interval): # print(f"Robot step function sleeping for {interval} msec on {self}.") self._lock.release() time.sleep(0.001 * interval) self._lock.acquire() self._time += 0.001 * interval # print("Robot step function resuming.") return 0 def getTime(self): return self._time # only support a single receiver and emitter def getReceiver(self, name): return self.receiver def getEmitter(self, name): return self.emitter ################################################################ class SuitcaseRobot(object): """This object emulates the portions of the Webots API related to the physical suitcase robot.""" def __init__(self): print("Entering SuitcaseRobot.__init__ and initializing superclasses") super(SuitcaseRobot, self).__init__() # The hardware is assumed to use 1/4 step microstepping and 800 steps per revolution. self.radians_to_steps = 800 / (2*math.pi) self.steps_to_radians = 1.0 / self.radians_to_steps # Create objects representing the physical hardware. self._motors = [RealMotor(name) for name in ['motor1', 'motor2', 'motor3', 'motor4']] self._sensors = [RealSensor(name) for name in ['joint1', 'joint2', 'joint3', 'joint4']] # Collect all the devices into a single dictionary for retrieval by the controller. self._devices = {} for m in self._motors: self._devices[m.name] = m for s in self._sensors: self._devices[s.name] = s # print("SuitcaseRobot initialized devices: ", self._devices) def getDevice(self, name): return self._devices[name] def process_sensor_string(self, line): # Process a bytestring of text from the hardware, typically called from # a serial port thread with data from the Arduino. # This assumes the StepperWinch sketch is running. # Sample input line: txyza 1966200716 0 0 0 0 try: tokens = line.split() if len(tokens) >= 6: # ignore the timestamp and extract the stepper motor counts values = [int(s) for s in tokens[2:6]] # update the sensor values in a thread-safe way with self._lock: self._sensors[0].value = self.steps_to_radians * values[0] self._sensors[1].value = self.steps_to_radians * values[1] self._sensors[2].value = self.steps_to_radians * values[2] self._sensors[3].value = self.steps_to_radians * values[3] except ValueError: pass def get_output_string(self): # Generate the next string of output to the hardware, typically called # from a serial port thread to generate output for the Arduino. # This assumes the StepperWinch sketch is running. with self._lock: # fetch the current motor commands in a thread-safe way positions = [m.target for m in self._motors] velocities = [m.rate for m in self._motors] # Accumulate a list of position targets and a list of velocity targets. # The default mode is position, but an infinite position target # indicates the use of velocity mode instead. v_c = "" p_c = "" vel = [] pos = [] for l, p, v in zip('xyza', positions, velocities): if not math.isinf(p): p_c += l pos.append(str(int(self.radians_to_steps * p))) else: v_c += l vel.append(str(int(self.radians_to_steps * v))) # create either one or two commands as output if v_c != "": vcommand = "v " + v_c + " " + " ".join(vel) + "\n" else: vcommand = "" if p_c != "": pcommand = "a " + p_c + " " + " ".join(pos) + "\n" else: pcommand = "" return pcommand + vcommand ################################################################ # Emulate the Webots sensor API. This object just stores and retrieves a single value. class RealSensor(object): def __init__(self, name): self.value = 0 self.name = name def enable(self, rate): pass def getValue(self): return self.value ################################################################ # Emulate the Webots motor API. This object just stores the commanded state; the parent robot emulator # is responsible for communicating it to the hardware controller. class RealMotor(object): def __init__(self, name): self.rate = 0 self.target = 0 self.name = name def setVelocity(self, rate): self.rate = rate def setPosition(self, target): self.target = target ################################################################ # Emulate the Webots 'radio' communication receiver. class Receiver(object): def __init__(self): # create a thread-safe queue for received messages self.queue = queue.Queue() pass def enable(self, rate): pass def getQueueLength(self): # return the number of available packets return self.queue.qsize() def getData(self): # return the next packet data as a bytestring # this also removes it from the queue, which doesn't match the Webots behavior, but # will work as long as it isn't called twice data = self.queue.get() print(f"receiver returning {data}") return data def nextPacket(self): # remove the oldest packet from the queue # because it is removed by getData, this is a no-op pass # Emulate the Webots 'radio' communication transmitter. class Emitter(object): def __init__(self): self.receivers = [] def add_receiver(self, receiver): self.receivers.append(receiver) def send(self, data): # send a bytestring to the subscribed receivers print(f"emitter broadcasting {data}") for r in self.receivers: r.queue.put(data) ################################################################