def load_hal_file(filename, ini=None):
sys.stdout.write("loading " + filename + '... ')
sys.stdout.flush()
_, ext = os.path.splitext(filename)
if ext == '.py':
from machinekit import rtapi
if not rtapi.__rtapicmd:
rtapi.init_RTAPI()
if ini is not None:
from machinekit import config
config.load_ini(ini)
globals_ = {}
with open(filename, 'r') as f:
data = compile(f.read(), filename, 'exec')
exec(data, globals_)
else:
command = 'halcmd'
if ini is not None:
command += ' -i ' + ini
command += ' -f ' + filename
subprocess.check_call(command, shell=True)
sys.stdout.write('done\n')
def start(self):
# Find the hal_hw_interface comp's directory in
# LD_LIBRARY_PATH and put it into $COMP_DIR
for path in os.environ.get("LD_LIBRARY_PATH", "").split(":"):
rospy.loginfo(f"Checking for hal_hw_interface.so in {path}")
if os.path.exists(os.path.join(path, "hal_hw_interface.so")):
comp_dir = path
break
else:
comp_dir = ""
os.environ["COMP_DIR"] = comp_dir
rospy.loginfo("hal_mgr: COMP_DIR set to '%s'" % comp_dir)
# Get parameters
if not rospy.has_param(self.NAME):
self.shutdown("No parameters set for '%s'" % self.NAME, 1)
try:
hal_mgr_config = rospy.get_param(self.NAME)
except KeyError:
self.shutdown("No keys defined at %s" % self.NAME, 1)
return
if "hal_files" not in hal_mgr_config:
self.shutdown("%s has no 'hal_files' key" % self.NAME, 1)
if "hal_file_dir" not in hal_mgr_config:
self.shutdown("%s has no 'hal_file_dir' key" % self.NAME, 1)
# Set up HAL
launcher.cleanup_session() # kill any running Machinekit instances
launcher.start_realtime()
rospy.loginfo("hal_mgr: Started realtime")
# Load rtapi module and set up signal handlers
if not getattr(rtapi, "__rtapicmd"):
rtapi.init_RTAPI()
def shutdown_graceful(signum, frame):
self.shutdown("Gracefully shutting down after interrupt signal")
signal.signal(signal.SIGINT, shutdown_graceful)
signal.signal(signal.SIGTERM, shutdown_graceful)
# Load HAL configuration
for fname in hal_mgr_config["hal_files"]:
fpath = os.path.join(hal_mgr_config["hal_file_dir"], fname)
if not os.path.exists(fpath):
self.shutdown(
"No file '%s' in directory '%s'" %
(fname, hal_mgr_config["hal_file_dir"]),
res=1,
)
rospy.loginfo("hal_mgr: Loading hal file '%s'" % fname)
launcher.load_hal_file(fpath)
rospy.loginfo("hal_mgr: Loading hal file '%s' complete" % fpath)
# Spin until ROS shutdown event
rospy.loginfo("ROS node and HAL started successfully")
self._pub.publish(True)
def change_config():
# connect to HAL
try:
rt.init_RTAPI()
except RuntimeError as e:
pass
#hal.stop_threads()
time.sleep(0.1)
# check for an existing signal "mod_success"
try:
hal.Signal("mod_success")
except RuntimeError as e:
if e.message == "signal 'mod_success' does not exist":
# good to go, no previous attempt
insert_jplanners()
def load_hal_file(filename, ini=None):
sys.stdout.write("loading " + filename + '... ')
sys.stdout.flush()
_, ext = os.path.splitext(filename)
if ext == '.py':
from machinekit import rtapi
if not rtapi.__rtapicmd:
rtapi.init_RTAPI()
if ini is not None:
from machinekit import config
config.load_ini(ini)
execfile(filename)
else:
command = 'halcmd'
if ini is not None:
command += ' -i ' + ini
command += ' -f ' + filename
subprocess.check_call(command, shell=True)
sys.stdout.write('done\n')
def hal_config():
from machinekit import launcher
launcher.cleanup_session()
comp_path = os.path.join(
os.path.dirname(os.path.realpath(__file__)),
'../components/absolute_joint.icomp',
)
launcher.install_comp(comp_path)
launcher.start_realtime()
rt.init_RTAPI()
config = HalConfig(thread=THREAD)
rt.newthread(config.thread.name, config.thread.period_ns, fp=True)
hal.start_threads()
yield config
hal.stop_threads()
launcher.end_session()
def main():
movel("home", v=0.08, a=0.08)
try:
rt.init_RTAPI()
notify("HAL initialized")
sleep(1.0)
except RuntimeError as e:
print(e.message)
#if e.message == "signal 'mod_success' does not exist"
#pass
try:
hal.Signal("mod_success")
except RuntimeError as e:
if e.message == "signal 'mod_success' does not exist":
change_configuration.change_config()
notify("HAL configuration changed")
sleep(1.0)
if hal.Signal('mod_success').get() == True:
l = mastering_app.Leveller(name="Masterer")
l.init_attributes()
l.fine_calibration = True
l.calib_fast_iter = 2
l.calib_slow_iter = 4
l.max_vel = 0.4
l.max_acc = 0.8
notify("Please press OK to start the mastering program.", warning=True)
l.info_callback = notify
l.start()
notify(
"THIS IS NOT AN ERROR. The mastering of the robot has finished. The robot has moved to the pose that should hold all zeros as joint angles. Press ABORT to exit this program, power off the robot, restart, and press ZERO ALL JOINTS from the SETTINGS tab before powering on again.",
error=True)
else:
notify(
"It looks like this configuration is not suitable for mastering the robot. Please look for details in the applicable instructions.",
error=True)
# HAL file for BeagleBone + TCT paralell port cape with 5 steppers and 3D printer board
import os
from machinekit import rtapi as rt
from machinekit import hal
from machinekit import config as c
from fdm.config import velocity_extrusion as ve
from fdm.config import base
from fdm.config import storage
from fdm.config import motion
import cramps as hardware
# initialize the RTAPI command client
rt.init_RTAPI()
# loads the ini file passed by linuxcnc
c.load_ini(os.environ['INI_FILE_NAME'])
motion.setup_motion()
hardware.init_hardware()
storage.init_storage('storage.ini')
# reading functions
hardware.hardware_read()
hal.addf('motion-command-handler', 'servo-thread')
hal.addf('motion-controller', 'servo-thread')
numFans = c.find('FDM', 'NUM_FANS')
numExtruders = c.find('FDM', 'NUM_EXTRUDERS')
numLights = c.find('FDM', 'NUM_LIGHTS')
hasHbp = c.find('FDM', 'HAS_HBP')
# HAL file for Trinamic SPI configuration
import os
from machinekit import hal
from machinekit import rtapi as rt
from machinekit import config as c
#from fdm.config import motion
from fdm_local.config import motion
from fdm.config import base
import cramps as hardware
hal.epsilon[1] = 0.1
# initialize the RTAPI command client
rt.init_RTAPI()
# loads the ini file passed by linuxcnc
c.load_ini(os.environ['INI_FILE_NAME'])
motion.setup_motion()
# reading functions
hardware.hardware_read()
hal.addf('motion-command-handler', 'servo-thread')
# Axis-of-motion Specific Configs (not the GUI)
# X [0] Axis
base.setup_stepper(section='AXIS_0',
axisIndex=0,
stepgenIndex=0,
stepgenType='hm2_5i25.0.stepgen')
def init_hardware():
check_version()
rtapi.init_RTAPI()
config.load_ini(os.environ['INI_FILE_NAME'])
error_sigs = []
watchdog_sigs = []
global AXIS_TOTAL
AXIS_TOTAL = int(config.find('TRAJ', 'AXES', '3'))
if AXIS_TOTAL < 3:
raise RuntimeError("AXES must be >= 3")
global EXTRUDER_TOTAL
EXTRUDER_TOTAL = int(config.find('FDM', 'EXTRUDERS', '1'))
if EXTRUDER_TOTAL < 1:
raise RuntimeError("EXTRUDERS must be >= 1")
rtapi.loadrt('tp')
rtapi.loadrt('trivkins')
rtapi.loadrt(config.find('EMCMOT', 'EMCMOT'),
servo_period_nsec=config.find('EMCMOT', 'SERVO_PERIOD'),
num_joints=str(AXIS_TOTAL),
num_aio=51,
num_dio=21)
hal.addf('motion-command-handler', SERVO_THREAD)
hal.addf('motion-controller', SERVO_THREAD)
estop_reset = hal.newsig('estop-reset', hal.HAL_BIT)
(main_enable, main_enable_inv) = setup_enable_chain()
if BOARD_REV == 'B3A':
replicape = hardware.ReplicapeB3A()
if BOARD_REV == 'A4A':
replicape = hardware.ReplicapeA4A()
watchdog_sigs.extend(replicape.get_watchdog_sigs())
replicape.link_enable(main_enable, main_enable_inv)
(joint_commands, joint_feedbacks) = setup_joints()
for i in xrange(AXIS_TOTAL):
setup_axis(replicape, joint_commands[i], joint_feedbacks[i], i)
setup_limit_switches(replicape)
extruder_sel_sig = hal.newsig('extruder-sel', hal.HAL_S32)
setup_extruders(replicape, extruder_sel_sig)
for i in xrange(EXTRUDER_TOTAL):
t = ExtruderTemperature(replicape, i, main_enable, estop_reset)
error_sigs.append(t.get_error_sig())
watchdog_sigs.append(t.get_temp_watchdog_sig())
if config.find('HBP', 'THERMISTOR'):
t = HbpTemperature(replicape, main_enable, estop_reset)
error_sigs.append(t.get_error_sig())
watchdog_sigs.append(t.get_temp_watchdog_sig())
setup_system_fan(replicape)
setup_estop(error_sigs, watchdog_sigs, estop_reset, SERVO_THREAD)
connect_tool_changer()
def init_hardware():
config.load_ini(os.environ['INI_FILE_NAME'])
check_version()
# we need to make the gpio pins used by the pru visible to userspace (??) in /sys/class/gpio
os.system('sudo ' + HARDWARE_PATH + 'set_pru_gpio.sh')
rtapi.init_RTAPI()
error_sigs = []
watchdog_sigs = []
global AXIS_TOTAL
AXIS_TOTAL = int(config.find('TRAJ', 'AXES', '3'))
if AXIS_TOTAL < 3:
raise RuntimeError("AXES must be >= 3")
global EXTRUDER_TOTAL
EXTRUDER_TOTAL = int(config.find('FDM', 'EXTRUDERS', '1'))
if EXTRUDER_TOTAL < 1:
raise RuntimeError("EXTRUDERS must be >= 1")
global NUM_FANS
NUM_FANS = int(config.find('FDM','NUM_FANS', 4))
system_fan = int(config.find('FDM','SYSTEM_FAN', 0))
if (NUM_FANS < system_fan):
raise RuntimeError("NUM_FANS must be >= 1 if SYSTEM_FAN > 0")
if (NUM_FANS > 4) or (NUM_FANS < 0 ):
raise RuntimeError("NUM_FANS must be > 0 and < 5")
rtapi.loadrt('tp')
if config.find('MACHINE','DELTA_R') is not None:
kinematics = 'lineardeltakins'
rtapi.loadrt(kinematics)
hal.Pin('lineardeltakins.L').set(config.find('MACHINE', 'CF_ROD'))
hal.Pin('lineardeltakins.R').set(config.find('MACHINE', 'DELTA_R'))
else:
kinematics = 'trivkins'
rtapi.loadrt(kinematics)
rtapi.loadrt(config.find('EMCMOT', 'EMCMOT'),
servo_period_nsec=config.find('EMCMOT', 'SERVO_PERIOD'),
num_joints=str(AXIS_TOTAL),
num_aio=51,
num_dio=21,
kins = kinematics)
hal.addf('motion-command-handler', SERVO_THREAD)
hal.addf('motion-controller', SERVO_THREAD)
estop_reset = hal.newsig('estop-reset', hal.HAL_BIT)
(main_enable, main_enable_inv) = setup_enable_chain()
if BOARD_REV == 'B3A':
replicape = hardware.ReplicapeB3A()
if BOARD_REV == 'A4A':
replicape = hardware.ReplicapeA4A()
watchdog_sigs.extend(replicape.get_watchdog_sigs())
replicape.link_enable(main_enable, main_enable_inv)
(joint_commands, joint_feedbacks) = setup_joints()
for i in xrange(AXIS_TOTAL):
setup_axis(replicape, joint_commands[i], joint_feedbacks[i], i)
setup_limit_switches(replicape)
extruder_sel_sig = hal.newsig('extruder-sel', hal.HAL_S32)
setup_extruders(replicape, extruder_sel_sig)
for i in xrange(EXTRUDER_TOTAL):
t = ExtruderTemperature(replicape, i, main_enable, estop_reset)
error_sigs.append(t.get_error_sig())
watchdog_sigs.append(t.get_temp_watchdog_sig())
if config.find('HBP', 'THERMISTOR'):
t = HbpTemperature(replicape, main_enable, estop_reset)
error_sigs.append(t.get_error_sig())
watchdog_sigs.append(t.get_temp_watchdog_sig())
# setup_system_fan(replicape)
setup_fans(replicape)
setup_estop(error_sigs, watchdog_sigs, estop_reset, SERVO_THREAD)
connect_tool_changer()
def init_hardware():
config.load_ini(os.environ['INI_FILE_NAME'])
check_version()
# we need to make the gpio pins used by the pru visible to userspace (??) in /sys/class/gpio
os.system('sudo ' + HARDWARE_PATH + 'set_pru_gpio.sh')
rtapi.init_RTAPI()
error_sigs = []
watchdog_sigs = []
global AXIS_TOTAL
AXIS_TOTAL = int(config.find('TRAJ', 'AXES', '3'))
if AXIS_TOTAL < 3:
raise RuntimeError("AXES must be >= 3")
global EXTRUDER_TOTAL
EXTRUDER_TOTAL = int(config.find('FDM', 'EXTRUDERS', '1'))
if EXTRUDER_TOTAL < 1:
raise RuntimeError("EXTRUDERS must be >= 1")
global NUM_FANS
NUM_FANS = int(config.find('FDM','NUM_FANS', 4))
system_fan = int(config.find('FDM','SYSTEM_FAN', 0))
if (NUM_FANS < system_fan):
raise RuntimeError("NUM_FANS must be >= 1 if SYSTEM_FAN > 0")
if (NUM_FANS > 4) or (NUM_FANS < 0 ):
raise RuntimeError("NUM_FANS must be > 0 and < 5")
rtapi.loadrt('tp')
if config.find('MACHINE','DELTA_R') is not None:
kinematics = 'lineardeltakins'
rtapi.loadrt(kinematics)
hal.Pin('lineardeltakins.L').set(config.find('MACHINE', 'CF_ROD'))
hal.Pin('lineardeltakins.R').set(config.find('MACHINE', 'DELTA_R'))
else:
kinematics = 'trivkins'
rtapi.loadrt(kinematics)
rtapi.loadrt(config.find('EMCMOT', 'EMCMOT'),
servo_period_nsec=config.find('EMCMOT', 'SERVO_PERIOD'),
num_joints=str(AXIS_TOTAL),
num_aio=51,
num_dio=21,
kins = kinematics)
hal.addf('motion-command-handler', SERVO_THREAD)
hal.addf('motion-controller', SERVO_THREAD)
estop_reset = hal.newsig('estop-reset', hal.HAL_BIT)
(main_enable, main_enable_inv) = setup_enable_chain()
if BOARD_REV == 'B3A':
replicape = hardware.ReplicapeB3A()
if BOARD_REV == 'A4A':
replicape = hardware.ReplicapeA4A()
watchdog_sigs.extend(replicape.get_watchdog_sigs())
replicape.link_enable(main_enable, main_enable_inv)
(joint_commands, joint_feedbacks) = setup_joints()
for i in xrange(AXIS_TOTAL):
setup_axis(replicape, joint_commands[i], joint_feedbacks[i], i)
setup_limit_switches(replicape)
extruder_sel_sig = hal.newsig('extruder-sel', hal.HAL_S32)
setup_extruders(replicape, extruder_sel_sig)
for i in xrange(EXTRUDER_TOTAL):
t = ExtruderTemperature(replicape, i, main_enable, estop_reset)
error_sigs.append(t.get_error_sig())
watchdog_sigs.append(t.get_temp_watchdog_sig())
if config.find('HBP', 'THERMISTOR'):
t = HbpTemperature(replicape, main_enable, estop_reset)
error_sigs.append(t.get_error_sig())
watchdog_sigs.append(t.get_temp_watchdog_sig())
# setup_system_fan(replicape)
setup_fans(replicape)
setup_estop(error_sigs, watchdog_sigs, estop_reset, SERVO_THREAD)
connect_tool_changer()
class Leveller(object):
name = attr.ib()
fsm = attr.ib(Fysom())
poses = attr.ib({
1: [.0, .0, .0, .0, .0, .0],
2: [.0, .0, -pi / 2, .0, pi / 2, .0],
3: [.0, .0, -pi / 2, -pi / 2, pi / 2, .0],
4: [.0, .0, -pi / 4, -pi / 2, .0, (pi / 2 - pi / 6)],
5: [.0, .0, -pi / 4, .0, .0, (-pi / 6)],
6: [.0, pi / 4, .0, .0, -pi / 2, (-pi / 6)],
7: [-pi, -pi / 4, .0, .0, -pi / 2, (-pi / 6)],
8: [-pi, -pi / 4, -pi, pi, -pi / 2, (-pi / 6)],
9: [.0, pi / 4, .0, .0, -pi / 4, -pi],
})
joints = attr.ib({})
calibration_moves = attr.ib({
1: [1],
# search tilt at 1 and record joint angle
# move to 7 and then 8
# search tilt at 8 and record joint angle
# calculate offset of difference
2: [6, 7, 8],
3: [3],
4: [1],
# search tilt at 4 and record joint angle
# move to 5
# search tilt at 5 and record joint angle
# 5 is parallel with 3
5: [4, 5],
6: [2],
})
# sequence determines the order in which joints get calibrated
# the calibration moves define the poses in chich this is done
sequence = attr.ib([6, 3, 4, 5, 2])
# current joint to be calibrated
curr_joint = attr.ib(default=0)
# with its current calibration moves
moves = attr.ib(default=[])
# remember the current move
moves_index = attr.ib(default=0)
fine_calibration = attr.ib(default=False)
rt = attr.ib(rt.init_RTAPI())
hal = attr.ib(hal)
max_vel = attr.ib(default=0.1)
max_acc = attr.ib(default=0.1)
calib_fast_iter = attr.ib(default=3)
calib_fast_vel = attr.ib(default=0.1)
calib_fast_acc = attr.ib(default=1.6)
calib_fast_amp = attr.ib(default=0.4)
calib_slow_iter = attr.ib(default=10)
calib_slow_vel = attr.ib(default=0.005)
calib_slow_acc = attr.ib(default=0.1)
calib_slow_amp = attr.ib(default=0.1)
speed_factor = attr.ib(default=1.)
fully_automatic_levelling = attr.ib(default=True)
info_callback = attr.ib(default='put_callback_here')
def dummy_print_callback(self, txt):
print(txt)
def start(self):
self.fsm.t_activate_joint()
def move_to_pose(self, nr=0):
if nr in self.poses:
longest_distance = self.find_longest_distance(nr)
print("longest distance = %s rad" % longest_distance)
self.calc_joint_move(longest_distance)
for i in range(1, 7):
j = self.joints[i]
print("{} -> {}".format(j.jp_pin("max-vel"), j.max_vel))
print("{} -> {}".format(j.jp_pin("max-acc"), j.max_acc))
print("{} -> {}".format(j.jp_pin("pos-cmd"), j.pos_cmd))
self.hal.Pin(j.jp_pin("max-vel")).set(j.max_vel)
self.hal.Pin(j.jp_pin("max-acc")).set(j.max_acc)
self.hal.Pin(j.jp_pin("pos-cmd")).set(j.pos_cmd)
def find_longest_distance(self, nr=0):
abs_dist = 0
longest_distance = 0
joint_nr = 0
for i in range(1, 7):
curr_pos = self.hal.Signal('joint%s_ros_pos_fb' % i).get()
j = self.joints[i]
j.curr_pos = curr_pos
j.pos_cmd = self.poses[nr][i - 1] + j.offset
abs_dist = abs(j.pos_cmd - j.curr_pos)
if abs_dist > longest_distance:
longest_distance = abs_dist
return longest_distance
def calc_joint_move(self, l_dist=0):
if l_dist > 0:
for i in range(1, 7):
j = self.joints[i]
dist = abs(j.pos_cmd - j.curr_pos)
print("distance joint {} is {} rad".format(i, dist))
j.max_acc = self.max_acc * (dist / l_dist) * self.speed_factor
j.max_vel = self.max_vel * (dist / l_dist) * self.speed_factor
# prevent very slow moves nearing zero velocity
if j.max_acc < j.min_acc: j.max_acc = j.min_acc
if j.max_vel < j.min_vel: j.max_vel = j.min_vel
def initialize(self):
for i in range(1, 7):
j = self.joints[i]
j.jp_name = "jp{}.0".format(i)
j.sampler_name = "sampler{}".format(i)
j.ring_name = "sampler{}.ring".format(i)
self.recorder = Recorder(name="sample_recorder", hal=self.hal)
self.info_callback = self.dummy_print_callback
def disable_all_jplan(self):
for i in range(1, 7):
j = self.joints[i - 1]
self.hal.Pin(j.pin('enable')).set(0)
def enable_all_jplan(self):
for i in range(1, 7):
j = self.joints[i - 1]
self.hal.Pin(j.pin('enable')).set(1)
def oscillate_joint(self,
nr=0,
nominal=0.,
amplitude=0.5,
nr_measurements=5,
speed=.1,
accel=3.0,
offset=.0):
if nr in self.joints:
direction = 1
center = nominal + offset
target = center + (amplitude * direction)
# let's set delta as 5% of the amplitude
delta = amplitude * 0.05
measurement = 0
j = self.joints[nr]
self.hal.Pin(j.jp_pin("max-vel")).set(speed)
self.hal.Pin(j.jp_pin("max-acc")).set(accel)
self.hal.Pin(j.jp_pin("pos-cmd")).set(target)
# set correct settings of the ring of the joint
self.recorder.reset()
self.recorder.set_sampler(sampler_name=j.sampler_name,
ring_name=j.ring_name)
self.wait_on_finish_moving()
self.recorder.start()
while (measurement < nr_measurements):
# process the records in the ring
self.recorder.process_samples()
# get the current position
curr_pos = self.hal.Signal('joint%s_ros_pos_fb' % (nr)).get()
if direction > 0:
if curr_pos > (target - delta):
direction *= -1
target = center + (amplitude * direction)
measurement += 1
self.info_callback("Measurement: %s" % measurement)
self.hal.Pin(j.jp_pin("pos-cmd")).set(target)
if direction < 0:
if curr_pos < (target + delta):
direction *= -1
target = center + (amplitude * direction)
self.hal.Pin(j.jp_pin("pos-cmd")).set(target)
time.sleep(0.1)
self.recorder.stop()
self.hal.Pin(j.jp_pin("pos-cmd")).set(center)
j.calculate_offset(sample_values=self.recorder.get_samples(),
nominal=nominal)
def calibrate_1(self):
j = self.joints[self.curr_joint]
#self.info_callback("calibration of %s" % j.name)
def calibrate_2(self):
j = self.joints[self.curr_joint]
self.info_callback("calibration of %s" % j.name)
# roughly seek around the joint 2 nominal angle
nominal_pos = self.poses[self.moves[self.moves_index]][self.curr_joint
- 1]
self.oscillate_joint(nr=self.curr_joint,
nr_measurements=self.calib_fast_iter,
nominal=nominal_pos,
accel=self.calib_fast_acc,
amplitude=self.calib_fast_amp,
speed=self.calib_fast_vel)
self.info_callback('rough 1st angle of joint 2 at signal is %s' %
(nominal_pos + j.offset))
self.move_to_pose(self.moves[self.moves_index])
self.wait_on_finish_moving()
if self.fine_calibration == True:
self.oscillate_joint(nr=self.curr_joint,
nr_measurements=self.calib_slow_iter,
nominal=nominal_pos,
offset=j.offset,
accel=self.calib_slow_acc,
amplitude=self.calib_slow_amp,
speed=self.calib_slow_vel)
self.info_callback(
'accurate 1st angle of joint 2 at signal is %s' %
(nominal_pos + j.offset))
self.hal.Pin(j.jp_pin("pos-cmd")).set(nominal_pos + j.offset)
self.wait_on_finish_moving()
# first part is done, now save this offset value and change poses
# to the 180 degree twisted setup and repeat
offset_1 = nominal_pos + j.offset
j.offset = 0
self.moves_index += 1
self.move_to_pose(self.moves[self.moves_index])
self.wait_on_finish_moving()
self.moves_index += 1
self.move_to_pose(self.moves[self.moves_index])
self.wait_on_finish_moving()
# take new measurments for the 2nd offset
nominal_pos = self.poses[self.moves[self.moves_index]][self.curr_joint
- 1]
self.oscillate_joint(nr=self.curr_joint,
nr_measurements=self.calib_fast_iter,
nominal=nominal_pos,
accel=self.calib_fast_acc,
amplitude=self.calib_fast_amp,
speed=self.calib_fast_vel)
self.info_callback('rough 2nd angle of joint 2 at signal is %s' %
(nominal_pos + j.offset))
self.move_to_pose(self.moves[self.moves_index])
self.wait_on_finish_moving()
if self.fine_calibration == True:
self.oscillate_joint(nr=self.curr_joint,
nr_measurements=self.calib_slow_iter,
nominal=nominal_pos,
offset=j.offset,
accel=self.calib_slow_acc,
amplitude=self.calib_slow_amp,
speed=self.calib_slow_vel)
self.info_callback(
'accurate 2nd angle of joint 2 at signal is %s' %
(nominal_pos + j.offset))
# move to the calculated offset value
self.hal.Pin(j.jp_pin("pos-cmd")).set(nominal_pos + j.offset)
self.wait_on_finish_moving()
offset_2 = nominal_pos + j.offset
# the average offset is the real offset of joint 2
j.offset = (offset_1 + offset_2) / 2
self.info_callback('offset of joint 2 is %s' % j.offset)
def calibrate_3(self):
j = self.joints[self.curr_joint]
self.info_callback("calibration of %s" % j.name)
# first rough calculation,
nominal_pos = self.poses[self.moves[self.moves_index]][self.curr_joint
- 1]
self.oscillate_joint(nr=self.curr_joint,
nr_measurements=self.calib_fast_iter,
nominal=nominal_pos,
accel=self.calib_fast_acc,
amplitude=self.calib_fast_amp,
speed=self.calib_fast_vel)
self.info_callback('rough offset of joint 3 is %s' % j.offset)
self.move_to_pose(self.moves[self.moves_index])
self.wait_on_finish_moving()
if self.fine_calibration == True:
self.oscillate_joint(nr=self.curr_joint,
nr_measurements=self.calib_slow_iter,
nominal=nominal_pos,
offset=j.offset,
accel=self.calib_slow_acc,
amplitude=self.calib_slow_amp,
speed=self.calib_slow_vel)
# move to the calculated offset value
self.hal.Pin(j.jp_pin("pos-cmd")).set(nominal_pos + j.offset)
self.wait_on_finish_moving()
self.info_callback('accurate offset of joint 3 is %s' % j.offset)
def calibrate_4(self):
j = self.joints[self.curr_joint]
self.info_callback("calibration of %s" % j.name)
# first rough calculation,
nominal_pos = self.poses[self.moves[self.moves_index]][self.curr_joint
- 1]
self.oscillate_joint(nr=self.curr_joint,
nr_measurements=self.calib_fast_iter,
nominal=nominal_pos,
accel=self.calib_fast_acc,
amplitude=self.calib_fast_amp,
speed=self.calib_fast_vel)
self.info_callback('rough offset of joint 4 is %s' % j.offset)
self.move_to_pose(self.moves[self.moves_index])
self.wait_on_finish_moving()
if self.fine_calibration == True:
self.oscillate_joint(nr=self.curr_joint,
nr_measurements=self.calib_slow_iter,
nominal=nominal_pos,
offset=j.offset,
accel=self.calib_slow_acc,
amplitude=self.calib_slow_amp,
speed=self.calib_slow_vel)
# move to the calculated offset value
self.hal.Pin(j.jp_pin("pos-cmd")).set(nominal_pos + j.offset)
self.info_callback('accurate offset of joint 4 is %s' %
(nominal_pos + j.offset))
self.wait_on_finish_moving()
def calibrate_5(self):
j3 = self.joints[3]
j = self.joints[self.curr_joint]
self.info_callback("calibration of %s" % j.name)
# first remember old home value and find angle for joint 3
joint_3_home = j3.offset
# roughly seek around the joint 3 nominal angle
nominal_pos = self.poses[self.moves[self.moves_index]][2]
self.oscillate_joint(nr=3,
nr_measurements=self.calib_fast_iter,
nominal=nominal_pos,
accel=self.calib_fast_acc,
amplitude=self.calib_fast_amp,
speed=self.calib_fast_vel)
self.info_callback('rough angle of joint 3 at signal is %s' %
(nominal_pos + j3.offset))
self.hal.Pin(j3.jp_pin("pos-cmd")).set(nominal_pos + j3.offset)
self.wait_on_finish_moving()
if self.fine_calibration == True:
self.oscillate_joint(nr=3,
nr_measurements=self.calib_slow_iter,
nominal=nominal_pos,
offset=j3.offset,
accel=self.calib_slow_acc,
amplitude=self.calib_slow_amp,
speed=self.calib_slow_vel)
self.info_callback('accurate angle of joint 3 at signal is %s' %
(nominal_pos + j3.offset))
# the angle of jont 3 at which the IMU signals is now known,
# now rotate joint 4 and joint 6 and find the offset of joint 5
# since joint 5 will be in line with joint 3 and 4, that offset
# is the correct deviation from "zero"
self.moves_index += 1
self.move_to_pose(self.moves[self.moves_index])
self.wait_on_finish_moving()
nominal_pos = self.poses[self.moves[self.moves_index]][self.curr_joint
- 1]
self.oscillate_joint(nr=self.curr_joint,
nr_measurements=self.calib_fast_iter,
nominal=nominal_pos,
accel=self.calib_fast_acc,
amplitude=self.calib_fast_amp,
speed=self.calib_fast_vel)
self.info_callback('rough offset of joint 5 is %s' %
(nominal_pos + j.offset))
self.hal.Pin(j.jp_pin("pos-cmd")).set(nominal_pos + j.offset)
self.wait_on_finish_moving()
if self.fine_calibration == True:
self.oscillate_joint(nr=self.curr_joint,
nr_measurements=self.calib_slow_iter,
nominal=nominal_pos,
offset=j.offset,
accel=self.calib_slow_acc,
amplitude=self.calib_slow_amp,
speed=self.calib_slow_vel)
# move to the calculated offset value
self.hal.Pin(j.jp_pin("pos-cmd")).set(nominal_pos + j.offset)
self.info_callback('accurate offset of joint 5 is %s' % j.offset)
self.wait_on_finish_moving()
# put old home value of joint 3 back again
self.joints[3].offset = joint_3_home
def calibrate_6(self):
j = self.joints[self.curr_joint]
self.info_callback("calibration of %s" % j.name)
# first rough calculation,
nominal_pos = self.poses[self.moves[self.moves_index]][self.curr_joint
- 1]
self.oscillate_joint(nr=self.curr_joint,
nr_measurements=self.calib_fast_iter,
nominal=nominal_pos,
accel=self.calib_fast_acc,
amplitude=self.calib_fast_amp,
speed=self.calib_fast_vel)
self.info_callback('rough offset of joint 6 is %s' % j.offset)
self.move_to_pose(self.moves[self.moves_index])
self.wait_on_finish_moving()
# then do accurate calculation
if self.fine_calibration == True:
self.oscillate_joint(nr=self.curr_joint,
nr_measurements=self.calib_slow_iter,
nominal=nominal_pos,
offset=j.offset,
accel=self.calib_slow_acc,
amplitude=self.calib_slow_amp,
speed=self.calib_slow_vel)
# move to the calculated offset value
self.hal.Pin(j.jp_pin("pos-cmd")).set(nominal_pos + j.offset)
self.info_callback('accurate offset of joint 6 is %s' % j.offset)
self.wait_on_finish_moving()
def wait_on_finish_moving(self):
timeout = 2000
t = 0
prev_pos_arr = [.0, .0, .0, .0, .0, .0]
curr_pos_arr = [
self.hal.Pin('jp1.0.curr-pos').get(),
self.hal.Pin('jp2.0.curr-pos').get(),
self.hal.Pin('jp3.0.curr-pos').get(),
self.hal.Pin('jp4.0.curr-pos').get(),
self.hal.Pin('jp5.0.curr-pos').get(),
self.hal.Pin('jp6.0.curr-pos').get()
]
motion = (prev_pos_arr != curr_pos_arr)
#while((self.hal.Pin('jplanners_active.out').get() == True) and (t < timeout)):
while (motion and (t < timeout)):
time.sleep(0.05)
t += 1
prev_pos_arr = curr_pos_arr
curr_pos_arr = [
self.hal.Pin('jp1.0.curr-pos').get(),
self.hal.Pin('jp2.0.curr-pos').get(),
self.hal.Pin('jp3.0.curr-pos').get(),
self.hal.Pin('jp4.0.curr-pos').get(),
self.hal.Pin('jp5.0.curr-pos').get(),
self.hal.Pin('jp6.0.curr-pos').get()
]
#print prev_pos_arr
#print curr_pos_arr
motion = (prev_pos_arr != curr_pos_arr)
#print motion
if (t >= timeout):
print("timeout")
def oninitial(self, e):
self.initialize()
self.fsm.t_setup()
def onsetup(self, e):
# set indexes to pick the first joint number in sequence
self.sequence_index = 0
# set index to first list entry
self.moves_index = 0
self.fsm.t_idle()
def onactivate_joint(self, e):
# set the first joint to calibrate
self.curr_joint = self.sequence[self.sequence_index]
# select the moves list to use
self.moves = self.calibration_moves[self.curr_joint]
self.fsm.t_move_to_first_pose()
def onmove_to_first_pose(self, e):
self.move_to_pose(self.moves[self.moves_index])
self.wait_on_finish_moving()
self.fsm.t_calibrate_joint()
def oncalibrate_joint(self, e):
self.joints[self.curr_joint].calibration()
self.fsm.t_joint_calibrated()
def onjoint_calibrated(self, e):
self.joints[self.curr_joint].calibrated = True
self.fsm.t_set_next_joint()
def onset_next_joint(self, e):
# set index to first list entry
self.moves_index = 0
if self.sequence_index < (len(self.sequence) - 1):
# set indexes to pick the first joint number in sequence
self.sequence_index += 1
if self.fully_automatic_levelling == True:
self.fsm.t_activate_joint()
else:
# just start over with an entire new set if we want.
self.sequence_index = 0
self.fsm.t_move_zero()
def onmove_to_zero(self, e):
self.move_to_pose(7)
self.wait_on_finish_moving()
self.move_to_pose(1)
self.wait_on_finish_moving()
def sync_jp_with_current_pos(self, i=0):
if (i != 0):
j = self.joints[i]
curr_pos = self.hal.Signal('joint%s_ros_pos_fb' % i).get()
self.hal.Pin(j.jp_pin("pos-cmd")).set(curr_pos)
self.wait_on_finish_moving()
print("joint %s HAL synced" % i)
def init_attributes(self):
self.joints = {
1: Joint(name='joint_1', calibration=self.calibrate_1),
2: Joint(name='joint_2', calibration=self.calibrate_2),
3: Joint(name='joint_3', calibration=self.calibrate_3),
4: Joint(name='joint_4', calibration=self.calibrate_4),
5: Joint(name='joint_5', calibration=self.calibrate_5),
6: Joint(name='joint_6', calibration=self.calibrate_6),
}
self.fsm = Fysom({
'initial': {
'state': 'initial',
'event': 'init',
'defer': True
},
'events': [
{
'name': 't_setup',
'src': ['initial', 'fault'],
'dst': 'setup',
},
{
'name': 't_idle',
'src': 'setup',
'dst': 'idle'
},
{
'name': 't_activate_joint',
'src': ['idle', 'set_next_joint', 'joint_calibrated'],
'dst': 'activate_joint'
},
{
'name': 't_move_to_first_pose',
'src': ['activate_joint'],
'dst': 'move_to_first_pose'
},
{
'name': 't_calibrate_joint',
'src': 'move_to_first_pose',
'dst': 'calibrate_joint'
},
{
'name': 't_joint_calibrated',
'src': 'calibrate_joint',
'dst': 'joint_calibrated'
},
{
'name': 't_set_next_joint',
'src': 'joint_calibrated',
'dst': 'set_next_joint'
},
{
'name': 't_move_zero',
'src': 'set_next_joint',
'dst': 'move_to_zero'
},
{
'name': 't_end',
'src': 'move_to_zero',
'dst': 'finished'
},
{
'name':
't_error',
'src': [
'initial', 'setup', 'idle', 'activate_joint',
'move_to_first_pose', 'start_next_move',
'calibrate_joint', 'joint_calibrated',
'set_next_joint', 'move_to_zero'
],
'dst':
'fault',
},
{
'name':
't_abort',
'src': [
'initial', 'setup', 'idle', 'activate_joint',
'move_to_first_pose', 'calibrate_joint',
'joint_calibrated', 'set_next_joint', 'move_to_zero'
],
'dst':
'abort',
},
],
'callbacks': {
'oninitial': self.oninitial,
'onsetup': self.onsetup,
'onactivate_joint': self.onactivate_joint,
'onmove_to_first_pose': self.onmove_to_first_pose,
'oncalibrate_joint': self.oncalibrate_joint,
'onjoint_calibrated': self.onjoint_calibrated,
'onset_next_joint': self.onset_next_joint,
'onmove_to_zero': self.onmove_to_zero
}
})
self.fsm.init()
def init_hardware():
check_version()
rtapi.init_RTAPI()
config.load_ini(os.environ['INI_FILE_NAME'])
error_sigs = []
watchdog_sigs = []
global AXIS_TOTAL
AXIS_TOTAL = int(config.find('TRAJ', 'AXES', '3'))
if AXIS_TOTAL < 3:
raise RuntimeError("AXES must be >= 3")
global EXTRUDER_TOTAL
EXTRUDER_TOTAL = int(config.find('FDM', 'EXTRUDERS', '1'))
if EXTRUDER_TOTAL < 1:
raise RuntimeError("EXTRUDERS must be >= 1")
rtapi.loadrt('tp')
rtapi.loadrt('trivkins')
rtapi.loadrt(config.find('EMCMOT', 'EMCMOT'),
servo_period_nsec=config.find('EMCMOT', 'SERVO_PERIOD'),
num_joints=str(AXIS_TOTAL),
num_aio=51,
num_dio=21)
hal.addf('motion-command-handler', SERVO_THREAD)
hal.addf('motion-controller', SERVO_THREAD)
estop_reset = hal.newsig('estop-reset', hal.HAL_BIT)
(main_enable, main_enable_inv) = setup_enable_chain()
if BOARD_REV == 'B3A':
replicape = hardware.ReplicapeB3A()
if BOARD_REV == 'A4A':
replicape = hardware.ReplicapeA4A()
watchdog_sigs.extend(replicape.get_watchdog_sigs())
replicape.link_enable(main_enable, main_enable_inv)
(joint_commands, joint_feedbacks) = setup_joints()
for i in xrange(AXIS_TOTAL):
setup_axis(replicape, joint_commands[i], joint_feedbacks[i], i)
setup_limit_switches(replicape)
extruder_sel_sig = hal.newsig('extruder-sel', hal.HAL_S32)
setup_extruders(replicape, extruder_sel_sig)
for i in xrange(EXTRUDER_TOTAL):
t = ExtruderTemperature(replicape, i, main_enable, estop_reset)
error_sigs.append(t.get_error_sig())
watchdog_sigs.append(t.get_temp_watchdog_sig())
if config.find('HBP', 'THERMISTOR'):
t = HbpTemperature(replicape, main_enable, estop_reset)
error_sigs.append(t.get_error_sig())
watchdog_sigs.append(t.get_temp_watchdog_sig())
setup_system_fan(replicape)
setup_estop(error_sigs, watchdog_sigs, estop_reset, SERVO_THREAD)
connect_tool_changer()
