Skip to content

Commit

Permalink
initial commit
Browse files Browse the repository at this point in the history
  • Loading branch information
Ross MacLeod committed Sep 23, 2020
0 parents commit b654d9a
Show file tree
Hide file tree
Showing 14 changed files with 718 additions and 0 deletions.
2 changes: 2 additions & 0 deletions .gitignore
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
*.halsave
*.bak
68 changes: 68 additions & 0 deletions PRO4896_7I76ED.pref
Original file line number Diff line number Diff line change
@@ -0,0 +1,68 @@
[DEFAULT]
dro_digits = 3
dro_size = 28
abs_color = #0000FF
rel_color = #000000
dtg_color = #FFFF00
homed_color = #00FF00
unhomed_color = #FF0000
enable_dro = False
scale_jog_vel = 126.0
scale_spindle_override = 1
scale_feed_override = 1
scale_rapid_override = 1
spindle_bar_min = 0.0
spindle_bar_max = 6000.0
turtle_jog_factor = 20
hide_turtle_jog_button = False
unlock_code = 123
toggle_readout = True
spindle_start_rpm = 300.0
blockheight = 0.0
open_file =
screen1 = window
x_pos = 40
y_pos = 30
width = 979
height = 750
use_toolmeasurement = False
gtk_theme = Follow System Theme
grid_size = 1.0
view = p
mouse_btn_mode = 4
hide_cursor = False
system_name_tool = Tool
system_name_g5x = G5x
system_name_rot = Rot
system_name_g92 = G92
system_name_g54 = G54
system_name_g55 = G55
system_name_g56 = G56
system_name_g57 = G57
system_name_g58 = G58
system_name_g59 = G59
system_name_g59.1 = G59.1
system_name_g59.2 = G59.2
system_name_g59.3 = G59.3
jump_to_dir = /home/robo
show_keyboard_on_offset = False
show_keyboard_on_tooledit = False
show_keyboard_on_edit = False
show_keyboard_on_mdi = False
x_pos_popup = 45.0
y_pos_popup = 55
width_popup = 250.0
max_messages = 10
message_font = sans 10
use_frames = True
reload_tool = True
blockdel = False
show_offsets = False
show_dtg = False
view_tool_path = True
view_dimension = True
run_from_line = no_run
unlock_way = use
show_preview_on_offset = False
use_keyboard_shortcuts = False

27 changes: 27 additions & 0 deletions autosave.halscope
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
THREAD servo-thread
MAXCHAN 4
HMULT 1
HZOOM 1
HPOS 5.000000e-01
CHAN 1
PIN vc-p2s.zero-x
VSCALE 0
VPOS 0.500000
VOFF 0.000000e+00
CHAN 2
PIN vc-p2s.zero-y
VSCALE 0
VPOS 0.500000
VOFF 0.000000e+00
CHAN 3
PIN vc-p2s.zero-z
VSCALE 0
VPOS 0.500000
VOFF 0.000000e+00
CHAN 4
SIG x-pos-cmd
VSCALE 0
VPOS 0.500000
VOFF 0.000000e+00
TMODE 0
RMODE 0
39 changes: 39 additions & 0 deletions macros/probe_z.ngc
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
o<probe_z> sub
#<fast_speed> = 4000
#<initial_speed> = 500
#<slow_speed> = 100
#<probe_drop> = 50
#<plate_z_offset> = 25.3

o<safety_check> if [#<_hal[motion.probe-input]> NE 0]
(msg, probe already touching!)
o<probe_z> return
o<safety_check> endif

#<start_z> = [#<_z>]

g21
g91
g92.1
g49

g38.3 f[#<initial_speed>] z[-#<probe_drop>]

o<fail_to_touch> if [#5070 EQ 0]
g90 g1 f[#<fast_speed>] z[#<start_z>]
(msg, probe never touched)
o<probe_z> return
o<fail_to_touch> endif

g1 z1
g38.2 f[#<slow_speed>] z-2

g90

g1 f[#<fast_speed>] z[#<start_z>]

g10 l20 p0 z[[#<start_z>-#5063]+#<plate_z_offset>]

o<probe_z> endsub
m2

6 changes: 6 additions & 0 deletions macros/safe_to_g54_zero.ngc
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
o<safe_to_g54_zero> sub
g53 g0 z0
g54 g0 x0 y0
o<safe_to_g54_zero> endsub
m2

6 changes: 6 additions & 0 deletions macros/to_back_left.ngc
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
o<to_back_left> sub
g53 g0 z0
g53 g0 x0 y2500
o<to_back_left> endsub
m2

6 changes: 6 additions & 0 deletions macros/to_front_right.ngc
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
o<to_front_right> sub
g53 g0 z0
g53 g0 x1250 y0
o<to_front_right> endsub
m2

10 changes: 10 additions & 0 deletions macros/to_g53_zero.ngc
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
o<to_g53_zero> sub
g21
g90
g92.1
g49

g53 g0 z0
g53 g0 x0 y0
o<to_g53_zero> endsub
m2
135 changes: 135 additions & 0 deletions pro4896_7i76ed.hal
Original file line number Diff line number Diff line change
@@ -0,0 +1,135 @@
loadrt [KINS]KINEMATICS
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
loadrt hostmot2
loadrt hm2_eth config="sserial_port_0=22222222" board_ip=192.168.1.121
loadrt estop_latch
loadrt logic count=1 personality=0x203

addf hm2_7i76e.0.read-request servo-thread
addf hm2_7i76e.0.read servo-thread
addf logic.0 servo-thread
addf estop-latch.0 servo-thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
addf hm2_7i76e.0.write servo-thread

################################################################################
# Hardware input signals

net x-neg-lim <= hm2_7i76e.0.7i76.0.0.input-06-not
net y1-neg-lim <= hm2_7i76e.0.7i76.0.0.input-01-not
net y2-neg-lim <= hm2_7i76e.0.7i76.0.0.input-00-not
net x-pos-lim <= hm2_7i76e.0.7i76.0.0.input-08-not
net y1-pos-lim <= hm2_7i76e.0.7i76.0.0.input-07-not
net z-pos-lim <= hm2_7i76e.0.7i76.0.0.input-02-not
net estop-button <= hm2_7i76e.0.7i76.0.0.input-03
net touch-probe <= hm2_7i76e.0.7i76.0.0.input-05-not
net aux <= hm2_7i76e.0.7i76.0.0.input-04-not
net spin-fault <= hm2_7i76e.0.7i76.0.0.input-09

################################################################################
# E stop / faulting

net estop-clear iocontrol.0.user-request-enable => estop-latch.0.reset
net estop-ok estop-latch.0.ok-out => iocontrol.0.emc-enable-in
net estop-trigger logic.0.or => estop-latch.0.fault-in
net estop-button => logic.0.in-00
net estop-watchdog hm2_7i76e.0.watchdog.has_bit => logic.0.in-01
net spin-fault => logic.0.in-02

################################################################################
# Tool change

net tool-prep-loop iocontrol.0.tool-prepare => iocontrol.0.tool-prepared
net tool-change-loop iocontrol.0.tool-change => iocontrol.0.tool-changed

net touch-probe => motion.probe-input


################################################################################
# Joint 0 (X)

setp hm2_7i76e.0.stepgen.00.steplen 3000
setp hm2_7i76e.0.stepgen.00.stepspace 3000
setp hm2_7i76e.0.stepgen.00.position-scale -80.204
setp hm2_7i76e.0.stepgen.00.maxaccel 0
setp hm2_7i76e.0.stepgen.00.dirsetup 5500
setp hm2_7i76e.0.stepgen.00.dirhold 5500

newsig x-enable bit
sets x-enable FALSE
net x-enable joint.0.amp-enable-out => hm2_7i76e.0.stepgen.00.enable
net x-pos-cmd joint.0.motor-pos-cmd => hm2_7i76e.0.stepgen.00.position-cmd
net x-pos-fb joint.0.motor-pos-fb <= hm2_7i76e.0.stepgen.00.position-fb
net x-neg-lim => joint.0.home-sw-in
net x-neg-lim => joint.0.neg-lim-sw-in
net x-pos-lim => joint.0.pos-lim-sw-in

################################################################################
# Joint 1 (Y)

setp hm2_7i76e.0.stepgen.01.steplen 3010
setp hm2_7i76e.0.stepgen.01.stepspace 3010
setp hm2_7i76e.0.stepgen.01.position-scale 80.204
setp hm2_7i76e.0.stepgen.01.maxaccel 0
setp hm2_7i76e.0.stepgen.01.dirsetup 5500
setp hm2_7i76e.0.stepgen.01.dirhold 5500

newsig y1-enable bit
sets y1-enable FALSE
net y1-enable joint.1.amp-enable-out => hm2_7i76e.0.stepgen.01.enable
net y1-pos-cmd joint.1.motor-pos-cmd => hm2_7i76e.0.stepgen.01.position-cmd
net y1-pos-fb joint.1.motor-pos-fb <= hm2_7i76e.0.stepgen.01.position-fb
net y1-neg-lim => joint.1.home-sw-in
net y1-neg-lim => joint.1.neg-lim-sw-in
net y1-pos-lim => joint.1.pos-lim-sw-in

################################################################################
# Joint 2 (Z)

setp hm2_7i76e.0.stepgen.02.steplen 3010
setp hm2_7i76e.0.stepgen.02.stepspace 3010
setp hm2_7i76e.0.stepgen.02.position-scale 200
setp hm2_7i76e.0.stepgen.02.maxaccel 0
setp hm2_7i76e.0.stepgen.02.dirsetup 5500
setp hm2_7i76e.0.stepgen.02.dirhold 5500

newsig z-enable bit
sets z-enable FALSE
net z-enable joint.2.amp-enable-out => hm2_7i76e.0.stepgen.02.enable
net z-pos-cmd joint.2.motor-pos-cmd => hm2_7i76e.0.stepgen.02.position-cmd
net z-pos-fb joint.2.motor-pos-fb <= hm2_7i76e.0.stepgen.02.position-fb
net z-pos-lim => joint.2.home-sw-in
net z-pos-lim => joint.2.pos-lim-sw-in

################################################################################
# Joint 3 (Y')

setp hm2_7i76e.0.stepgen.03.steplen 3010
setp hm2_7i76e.0.stepgen.03.stepspace 3010
setp hm2_7i76e.0.stepgen.03.position-scale -80.204
setp hm2_7i76e.0.stepgen.03.maxaccel 0
setp hm2_7i76e.0.stepgen.03.dirsetup 5500
setp hm2_7i76e.0.stepgen.03.dirhold 5500

newsig y2-enable bit
sets y2-enable FALSE
net y2-enable joint.3.amp-enable-out => hm2_7i76e.0.stepgen.03.enable
net y2-pos-cmd joint.3.motor-pos-cmd => hm2_7i76e.0.stepgen.03.position-cmd
net y2-pos-fb joint.3.motor-pos-fb <= hm2_7i76e.0.stepgen.03.position-fb
net y2-neg-lim => joint.3.home-sw-in
net y2-neg-lim => joint.3.neg-lim-sw-in

################################################################################
# Spindle

setp hm2_7i76e.0.7i76.0.0.spinout-scalemax 24000
setp hm2_7i76e.0.7i76.0.0.spinout-minlim 8000
setp hm2_7i76e.0.7i76.0.0.spinout-maxlim 24000

# FIXME spindle fault
net spindle-on spindle.0.on => hm2_7i76e.0.7i76.0.0.spinena
net spindle-speed spindle.0.speed-out-abs => hm2_7i76e.0.7i76.0.0.spinout

# start

Loading

0 comments on commit b654d9a

Please sign in to comment.