123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112 |
- loadrt [KINS]KINEMATICS
- loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD traj_period_nsec=[EMCMOT]SERVO_PERIOD key=[EMCMOT]SHMEM_KEY num_joints=[KINS]JOINTS
- loadrt stepgen step_type=0,0,0
- loadrt charge_pump
- loadrt comp count=3
- loadusr -W hal_manualtoolchange
- # Base thread
- addf stepgen.make-pulses base-thread
- # Servo thread
- addf stepgen.capture-position servo-thread
- addf motion-command-handler servo-thread
- addf motion-controller servo-thread
- addf stepgen.update-freq servo-thread
- addf charge-pump servo-thread
- addf comp.0 servo-thread
- addf comp.1 servo-thread
- addf comp.2 servo-thread
- # Spindle control
- net spindle-cw <= spindle.0.forward
- net spindle-ccw <= spindle.0.reverse
- # Coolant control
- net coolant-mist <= iocontrol.0.coolant-mist
- net coolant-flood <= iocontrol.0.coolant-flood
- # heartbeat generator
- setp charge-pump.enable 1
- net heartbeat <= charge-pump.out-4
- # Emergency stop logic
- net estop-out-not <= iocontrol.0.user-enable-out
- net estop-in-not => iocontrol.0.emc-enable-in
- # Manual tool change
- net tool-change iocontrol.0.tool-change => hal_manualtoolchange.change
- net tool-changed iocontrol.0.tool-changed <= hal_manualtoolchange.changed
- net tool-number iocontrol.0.tool-prep-number => hal_manualtoolchange.number
- net tool-prepare-loopback iocontrol.0.tool-prepare => iocontrol.0.tool-prepared
- # X joint
- setp stepgen.0.position-scale [JOINT_0]SCALE
- setp stepgen.0.steplen [STEPCONF]STEPLEN
- setp stepgen.0.stepspace [STEPCONF]STEPSPACE
- setp stepgen.0.dirhold [STEPCONF]DIRHOLD
- setp stepgen.0.dirsetup [STEPCONF]DIRSETUP
- setp stepgen.0.maxaccel [JOINT_0]STEPGEN_MAXACCEL
- net xpos-cmd joint.0.motor-pos-cmd => stepgen.0.position-cmd
- net xpos-fb stepgen.0.position-fb => joint.0.motor-pos-fb
- net xstep <= stepgen.0.step
- net xdir <= stepgen.0.dir
- net xenable joint.0.amp-enable-out => stepgen.0.enable
- net home-x => joint.0.home-sw-in
- net limit-x => joint.0.neg-lim-sw-in
- net limit-x => joint.0.pos-lim-sw-in
- # Y joint
- setp stepgen.1.position-scale [JOINT_1]SCALE
- setp stepgen.1.steplen [STEPCONF]STEPLEN
- setp stepgen.1.stepspace [STEPCONF]STEPSPACE
- setp stepgen.1.dirhold [STEPCONF]DIRHOLD
- setp stepgen.1.dirsetup [STEPCONF]DIRSETUP
- setp stepgen.1.maxaccel [JOINT_1]STEPGEN_MAXACCEL
- net ypos-cmd joint.1.motor-pos-cmd => stepgen.1.position-cmd
- net ypos-fb stepgen.1.position-fb => joint.1.motor-pos-fb
- net ystep <= stepgen.1.step
- net ydir <= stepgen.1.dir
- net yenable joint.1.amp-enable-out => stepgen.1.enable
- net home-y => joint.1.home-sw-in
- net limit-y => joint.1.neg-lim-sw-in
- net limit-y => joint.1.pos-lim-sw-in
- # Z joint
- setp stepgen.2.position-scale [JOINT_2]SCALE
- setp stepgen.2.steplen [STEPCONF]STEPLEN
- setp stepgen.2.stepspace [STEPCONF]STEPSPACE
- setp stepgen.2.dirhold [STEPCONF]DIRHOLD
- setp stepgen.2.dirsetup [STEPCONF]DIRSETUP
- setp stepgen.2.maxaccel [JOINT_2]STEPGEN_MAXACCEL
- net zpos-cmd joint.2.motor-pos-cmd => stepgen.2.position-cmd
- net zpos-fb stepgen.2.position-fb => joint.2.motor-pos-fb
- net zstep <= stepgen.2.step
- net zdir <= stepgen.2.dir
- net zenable joint.2.amp-enable-out => stepgen.2.enable
- net home-z => joint.2.home-sw-in
- net limit-z => joint.2.neg-lim-sw-in
- net limit-z => joint.2.pos-lim-sw-in
- # Simulate home switches
- net xpos-cmd => comp.0.in0
- setp comp.0.in1 0
- setp comp.0.hyst 0.1
- net home-x <= comp.0.out
- net ypos-cmd => comp.1.in0
- setp comp.1.in1 0
- setp comp.1.hyst 0.1
- net home-y <= comp.1.out
- net zpos-cmd => comp.2.in0
- setp comp.2.in1 0
- setp comp.2.hyst 0.1
- net home-z <= comp.2.out
- # Simulate hardware limit switches
- sets limit-x 0
- sets limit-y 0
- sets limit-z 0
|