117 lines
3.3 KiB
Plaintext
117 lines
3.3 KiB
Plaintext
# load realtime modules
|
|
# kinematics
|
|
loadrt [KINS]KINEMATICS
|
|
loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
|
|
loadrt hal_parport cfg="0x378"
|
|
loadrt encoder num_chan=2
|
|
loadrt pid num_chan=2
|
|
loadrt pwmgen output_type=1,1
|
|
loadrt ddt count=4
|
|
|
|
# define the order of execution for RT code
|
|
addf parport.0.read base-thread
|
|
addf encoder.update-counters base-thread
|
|
addf pwmgen.make-pulses base-thread
|
|
addf parport.0.write base-thread
|
|
|
|
addf encoder.capture-position servo-thread
|
|
addf motion-command-handler servo-thread
|
|
addf motion-controller servo-thread
|
|
addf pid.0.do-pid-calcs servo-thread
|
|
addf pid.1.do-pid-calcs servo-thread
|
|
addf pwmgen.update servo-thread
|
|
addf ddt.0 servo-thread
|
|
addf ddt.1 servo-thread
|
|
addf ddt.2 servo-thread
|
|
addf ddt.3 servo-thread
|
|
|
|
# hook stuff together
|
|
|
|
setp encoder.0.position-scale [JOINT_0]INPUT_SCALE
|
|
setp encoder.1.position-scale [JOINT_1]INPUT_SCALE
|
|
|
|
net enc0A encoder.0.phase-A <= parport.0.pin-10-in
|
|
net enc0B encoder.0.phase-B <= parport.0.pin-11-in
|
|
net enc1A encoder.1.phase-A <= parport.0.pin-12-in
|
|
net enc1B encoder.1.phase-B <= parport.0.pin-13-in
|
|
|
|
net Xpos-fb <= encoder.0.position
|
|
net Ypos-fb <= encoder.1.position
|
|
|
|
|
|
net Xpos-fb => pid.0.feedback
|
|
net Ypos-fb => pid.1.feedback
|
|
net Xpos-fb => joint.0.motor-pos-fb
|
|
net Ypos-fb => joint.1.motor-pos-fb
|
|
|
|
net Xvel-cmd <= pid.0.output
|
|
net Yvel-cmd <= pid.1.output
|
|
|
|
net Xvel-cmd => pwmgen.0.value
|
|
net Yvel-cmd => pwmgen.1.value
|
|
|
|
net Xpwm pwmgen.0.pwm
|
|
net Ypwm pwmgen.1.pwm
|
|
|
|
net Xdir <= pwmgen.0.dir
|
|
net Ydir <= pwmgen.1.dir
|
|
|
|
net Xdir => parport.0.pin-02-out
|
|
net Xdir => parport.0.pin-04-out
|
|
setp parport.0.pin-04-out-invert TRUE
|
|
net Xpwm => parport.0.pin-03-out
|
|
|
|
net Ydir => parport.0.pin-05-out
|
|
net Ydir => parport.0.pin-07-out
|
|
setp parport.0.pin-07-out-invert TRUE
|
|
net Ypwm => parport.0.pin-06-out
|
|
|
|
setp pid.0.maxoutput 1.0
|
|
setp pid.1.maxoutput 1.0
|
|
|
|
# the values below come from the ini
|
|
setp pid.0.Pgain [JOINT_0]P
|
|
setp pid.0.Igain [JOINT_0]I
|
|
setp pid.0.Dgain [JOINT_0]D
|
|
setp pid.0.bias [JOINT_0]BIAS
|
|
setp pid.0.FF0 [JOINT_0]FF0
|
|
setp pid.0.FF1 [JOINT_0]FF1
|
|
setp pid.0.FF2 [JOINT_0]FF2
|
|
# deadband should be just over 1 count
|
|
setp pid.0.deadband [JOINT_0]DEADBAND
|
|
|
|
setp pid.1.Pgain [JOINT_1]P
|
|
setp pid.1.Igain [JOINT_1]I
|
|
setp pid.1.Dgain [JOINT_1]D
|
|
setp pid.1.bias [JOINT_1]BIAS
|
|
setp pid.1.FF0 [JOINT_1]FF0
|
|
setp pid.1.FF1 [JOINT_1]FF1
|
|
setp pid.1.FF2 [JOINT_1]FF2
|
|
# deadband should be just over 1 count
|
|
setp pid.1.deadband [JOINT_1]DEADBAND
|
|
|
|
net Xpos-cmd <= joint.0.motor-pos-cmd
|
|
net Ypos-cmd <= joint.1.motor-pos-cmd
|
|
net Xpos-cmd => pid.0.command
|
|
net Ypos-cmd => pid.1.command
|
|
net Xenable <= joint.0.amp-enable-out
|
|
net Yenable <= joint.1.amp-enable-out
|
|
net Xenable => pid.0.enable
|
|
net Xenable => pwmgen.0.enable
|
|
net Yenable => pid.1.enable
|
|
net Yenable => pwmgen.1.enable
|
|
|
|
net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in
|
|
|
|
net Xpos-cmd => ddt.0.in
|
|
net Xvel <= ddt.0.out
|
|
net Xvel => ddt.1.in
|
|
net Xacc <= ddt.1.out
|
|
net Ypos-cmd => ddt.2.in
|
|
net Yvel <= ddt.2.out
|
|
net Yvel => ddt.3.in
|
|
net Yacc <= ddt.3.out
|
|
|
|
net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared
|
|
net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed
|