Здесь описан один энкодер, PID, и генератор pwmgen.
Энкодер висит на пинах 11 и 10.
Выход ШИМ на пине 1.
stan.hal (для просмотра содержимого нажмите на ссылку)
loadrt trivkins
loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES
loadrt probe_parport
loadrt hal_parport cfg="0x378 out "
setp parport.0.reset-time 5000
addf parport.0.write base-thread
addf parport.0.reset base-thread
# описываем энкодер
loadrt encoder num_chan=1
addf encoder.update-counters base-thread
addf encoder.capture-position servo-thread
setp encoder.0.position-scale 20.000000
net Xpos-phase-a encoder.0.phase-A
net Xpos-phase-b encoder.0.phase-B
net Xpos-index encoder.0.phase-Z
net Xpos-phase-a <= parport.0.pin-10-in
net Xpos-index <= parport.0.pin-11-in
# описываем ШИМ через pwmgen и PID
loadrt pwmgen output_type=0
addf pwmgen.make-pulses base-thread
addf pwmgen.update servo-thread
loadrt at_pid num_chan=1 debug=1
addf pid.0.do-pid-calcs servo-thread
net Xvel-cmd <= pid.0.out => pwmgen.0.value
net Xpos-fb <= encoder.0.position => pid.0.feedback
net Xpos-fb axis.0.motor-pos-fb
net Xpos-cmd <= axis.0.motor-pos-cmd => pid.0.command
net Xpos-pwm <= pwmgen.0.pwm => parport.0.pin-01-out
net Xenable <= axis.0.amp-enable-out => pid.0.enable
net Xenable => pwm.0.enable
net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in
# задаем значения параметров для PID
setp pid.0.Pgain [AXIS_0]P
setp pid.0.Igain [AXIS_0]I
setp pid.0.Dgain [AXIS_0]D
setp pid.0.bias [AXIS_0]BIAS
setp pid.0.FF0 [AXIS_0]FF0
setp pid.0.FF1 [AXIS_0]FF1
setp pid.0.FF2 [AXIS_0]FF2
setp pid.0.deadband [AXIS_0]DEADBAND
addf motion-command-handler servo-thread
addf motion-controller servo-thread
net estop-out <= iocontrol.0.user-enable-out
net estop-out => iocontrol.0.emc-enable-in
loadusr -W hal_manualtoolchange
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
loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES
loadrt probe_parport
loadrt hal_parport cfg="0x378 out "
setp parport.0.reset-time 5000
addf parport.0.write base-thread
addf parport.0.reset base-thread
# описываем энкодер
loadrt encoder num_chan=1
addf encoder.update-counters base-thread
addf encoder.capture-position servo-thread
setp encoder.0.position-scale 20.000000
net Xpos-phase-a encoder.0.phase-A
net Xpos-phase-b encoder.0.phase-B
net Xpos-index encoder.0.phase-Z
net Xpos-phase-a <= parport.0.pin-10-in
net Xpos-index <= parport.0.pin-11-in
# описываем ШИМ через pwmgen и PID
loadrt pwmgen output_type=0
addf pwmgen.make-pulses base-thread
addf pwmgen.update servo-thread
loadrt at_pid num_chan=1 debug=1
addf pid.0.do-pid-calcs servo-thread
net Xvel-cmd <= pid.0.out => pwmgen.0.value
net Xpos-fb <= encoder.0.position => pid.0.feedback
net Xpos-fb axis.0.motor-pos-fb
net Xpos-cmd <= axis.0.motor-pos-cmd => pid.0.command
net Xpos-pwm <= pwmgen.0.pwm => parport.0.pin-01-out
net Xenable <= axis.0.amp-enable-out => pid.0.enable
net Xenable => pwm.0.enable
net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in
# задаем значения параметров для PID
setp pid.0.Pgain [AXIS_0]P
setp pid.0.Igain [AXIS_0]I
setp pid.0.Dgain [AXIS_0]D
setp pid.0.bias [AXIS_0]BIAS
setp pid.0.FF0 [AXIS_0]FF0
setp pid.0.FF1 [AXIS_0]FF1
setp pid.0.FF2 [AXIS_0]FF2
setp pid.0.deadband [AXIS_0]DEADBAND
addf motion-command-handler servo-thread
addf motion-controller servo-thread
net estop-out <= iocontrol.0.user-enable-out
net estop-out => iocontrol.0.emc-enable-in
loadusr -W hal_manualtoolchange
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
