class Runtime:
def __init__(self, robot: str):
# Thread-safe event flags
self.program_stopped = Event()
self.ignore_controller = Event()
self._current_mode = 'off'
self.controller = None
self.already_connected = False
self.controller_poll_rate = 5
self.mode_poll_rate = 0.1
self.mode_lock = RLock()
self.lcd = LCD()
# Preprocess string
robot_str = robot.strip().lower()
if robot_str == '6rus':
self.robot = SixRUS(stepper_mode=1 / 32, step_delay=0.002)
elif robot_str == 'quattro':
self.robot = Quattro(stepper_mode=1 / 32, step_delay=0.002)
elif robot_str == 'delta':
self.robot = Delta(stepper_mode=1 / 32, step_delay=0.002)
else:
raise ValueError(f"Unknown robot type: {robot}")
self.lcd.print_status(f'Started {robot}')
@property
def current_mode(self):
with self.mode_lock:
return self._current_mode
@current_mode.setter
def current_mode(self, val):
with self.mode_lock:
self._current_mode = val
def eval_controller_response(self, response):
"""
evaluates the answer from the mode_from_input-function
"""
if isinstance(response, str):
if response in ['stop', 'demo', 'manual', 'calibrate', 'off']:
pass
elif response == 'homing':
self.ignore_controller.set()
else:
raise ValueError("Unknown answer from controller")
if self.current_mode != response: # only print if the mode changes
logging.debug(
f'Switching from {self.current_mode} to {response}.')
self.lcd.print_status(f'Status: {response}')
self.current_mode = response # set robot mode to the response
return True
return False # no response given
def poll_controller_status(self):
if self.program_stopped.is_set():
return
if not controller.still_connected():
self.already_connected = False
logging.info("Please connect controller! Retrying in 5 seconds...")
self.lcd.print_connection(False)
else:
if self.already_connected:
# no new initialisation required here
logging.info('Controller still connected.')
else:
# stop listening as the controller gets initalised
self.ignore_controller.set()
# init new joystick since the controller was reconnected or connected the first time
self.controller = controller.init_controller()
self.ignore_controller.clear()
self.already_connected = True
logging.info('Controller connected.')
self.lcd.print_connection(True)
# call program again after 5 seconds
Timer(self.controller_poll_rate, self.poll_controller_status).start()
def poll_program_mode(self):
if self.program_stopped.is_set():
# Finish thread if program is terminated
return
# Handle controller inputs all the time to keep button states up to date
try:
controls = controller.get_controller_inputs(self.controller)
if not self.ignore_controller.is_set():
# evaluate the answer from controller
self.eval_controller_response(
controller.mode_from_inputs(controls))
except AttributeError:
pass
except pygame.error as e:
logging.exception(e)
finally:
# call program again after 0.1 seconds
Timer(self.mode_poll_rate, self.poll_program_mode).start()
def move(self, pose):
try:
self.robot.mov(pose)
except WorkspaceViolation:
logging.debug(f"Cannot move to pose:{pose}")
else:
self.lcd.print_pose(pose)
def move_manual(self, dt=0.005):
"""
This is the manual controlling mode, where the robot can be driven with the controller.
Exits only if the mode was changed or the program was interrupted
"""
# stop listening to controller (bc. we listen all the time in here)
self.ignore_controller.set()
time.sleep(dt)
try:
inputs = controller.get_controller_inputs(self.controller)
except AttributeError:
return
new_pose = controller.get_movement_from_cont(inputs,
self.robot.currPose)
# check if mode was changed
if self.eval_controller_response(controller.mode_from_inputs(inputs)):
self.ignore_controller.clear()
self.move(new_pose)
def move_demo(self):
"""
Selects a random demo programm and executes it
"""
modules = []
for a in dir(demo):
if isinstance(getattr(demo, a), types.FunctionType):
modules.append(getattr(demo, a))
prog = random.choice(modules) # choose a random demo
demo_pos_list = prog() # execute chosen demo programm
for pos in demo_pos_list:
try:
if pos[6] == 'lin':
coord = pos[:6] # extract only pose
self.robot.mov_lin(coord) # move linear
elif pos[6] == 'mov':
coord = pos[:6] # extract only pose
self.move(coord) # move with PTP-interplation
except IndexError:
self.move(pos) # if 'lin' or 'mov' wasent given, use mov/PTP
if not self.current_mode == 'demo': # break if the mode was changed
break
def calibrate_process(self, dt=0.005):
"""
enters mode, where the user can calibrate each motor in microstep mode.
A homing procedure has to be done afterwarts!
`dt`: how fast the controller inputs get checked in [s]
"""
mot_num = 0 # motornumber from 0 to 5
# pose after calibration has to be given to move the motors but is not necessary here
# since a homing procedure has to be done afterwards anyways
pose_after_cali = [0, 0, 0, 0, 0, 0]
allowed_to_change_again = True # if the next motor can be selected
cali_step_increment = 1
while True:
time.sleep(dt)
try:
controls = controller.get_controller_inputs(self.controller)
except AttributeError:
continue
# check if mode was changed
if self.eval_controller_response(
controller.mode_from_inputs(controls)):
break
cali_mot = [0, 0, 0, 0, 0, 0]
if allowed_to_change_again:
# change motornumber with L1 and R1
if controls['L1']:
mot_num -= 1
elif controls['R1']:
mot_num += 1
# check if selected motor number exists
if mot_num > self.robot.dof - 1:
mot_num = 0
elif mot_num < 0:
mot_num = self.robot.dof - 1
allowed_to_change_again = False
if controls['L1'] == 0 and controls[
'R1'] == 0: # both buttons have to be released to switch to next motor
allowed_to_change_again = True
if controls['UP']:
cali_mot[
mot_num] = cali_step_increment # set 1 posivitve for selected motor
elif controls['DOWN']:
cali_mot[
mot_num] = -cali_step_increment # set -1 posivitve for selected motor
elif controls['LEFT'] and self.robot.stepperMode != 0:
# Decrement calibration step size but needs to be at least 1
cali_step_increment = max(1, cali_step_increment - 1)
elif controls['RIGHT'] and self.robot.stepperMode != 0:
# Increment calibration step size but needs to be <= 1/step-mode (e.g. 1/32 -> 32 inc = 1 full step)
cali_step_increment = min(1 / self.robot.stepperMode,
cali_step_increment + 1)
self.robot.mov_steps(cali_mot, pose_after_cali)
def loop(self):
self.robot.homing('90') # home robot
self.controller = controller.init_controller()
if self.controller is None:
self.already_connected = False
# call subroutine every 5-seconds to check for controller
self.poll_controller_status()
# start listening to controller
self.ignore_controller.clear()
self.poll_program_mode()
while not self.program_stopped.is_set():
# State Machine
if self.current_mode == 'off':
self.robot.disable_steppers()
time.sleep(0.0001) # limit loop time
else:
self.robot.enable_steppers()
if self.current_mode == 'demo':
self.move_demo()
time.sleep(2)
elif self.current_mode == 'manual':
# control the robot with the controller
self.move_manual()
elif self.current_mode == 'calibrate':
# stop listening to controller (bc. we listen all the time in here)
self.ignore_controller.set()
time.sleep(0.5)
self.calibrate_process()
time.sleep(0.5)
# home robot afterwards
logging.info('Switching to homing')
self.lcd.print_status(f'Status: homing')
self.current_mode = 'homing'
elif self.current_mode == 'homing':
# stop listening to controller to prevent program change while homing
self.ignore_controller.set()
time.sleep(
1.5) # wait a bit to reduce multiple homing attempts
self.robot.homing('90') # use homing method '90'
# exit homing and switch to state that stopped calibration
logging.info('Switching to stop')
self.lcd.print_status(f'Status: stop')
self.current_mode = 'stop'
self.ignore_controller.clear()
elif self.current_mode == 'stop':
# stop robot after next movement and do nothing
time.sleep(0.0001) # limit loop time
