Fanuc DC servo + 7i77
I need some help here to correctly set up a worm gear driven rotary table (this is the first axis Im trying to get work, as it cannot run away...)
So I have an old FANUC DC Servo amplifier, which needs two analog signals, one for driving and one for feedback.
some problems I came around during configuration:
If I set up the feedback scale low enough, than the motor starts oscillation, even on minimal pid gain (1, other stuff is all 0). If I set the scale large enough to avoid oscillation, than the drive's runaway detection kicks in (TGAL), disabling the drive on first move. It can be overridden, but there is another problem:
the motor has a 2500 PPR encoder, and after a reaching a certain speed, the encoder signal is loosed and the drive going full speed.
I believe the 7i77 cannot keep up with the speed, and could not find out how to achieve higher speeds (the docs mentions that is possible but cannot find out how to do)
in the latest set up I could achive a correct position, oscillating in the micron range <0.01, but the ferror is too high if I set it to 0.1 than its instantly disables servo with path following error.
I read many tutorials on PID tuning, but after two days past it seem quite impossible to me to tune it correctly.
anyone did such configurations in the past? I could use some help here. Thanks
The current configuration is shown below.
#*******************
# AXIS C
#*******************
setp pid.c.Pgain [AXIS_4]P
setp pid.c.Igain [AXIS_4]I
setp pid.c.Dgain [AXIS_4]D
setp pid.c.bias [AXIS_4]BIAS
setp pid.c.FF0 [AXIS_4]FF0
setp pid.c.FF1 [AXIS_4]FF1
setp pid.c.FF2 [AXIS_4]FF2
setp pid.c.deadband [AXIS_4]DEADBAND
setp pid.c.maxoutput [AXIS_4]MAX_OUTPUT
setp pid.c.error-previous-target true
net c-index-enable <=> pid.c.index-enable
net c-enable => pid.c.enable
net c-pos-cmd => pid.c.command
net c-vel-cmd => pid.c.command-deriv
net c-pos-fb => pid.c.feedback
net c-output => pid.c.output
# ---PWM Generator signals/setup---
setp hm2_5i25.0.7i77.0.4.analogout3-scalemax [AXIS_4]OUTPUT_SCALE
setp hm2_5i25.0.7i77.0.4.analogout3-minlim [AXIS_4]OUTPUT_MIN_LIMIT
setp hm2_5i25.0.7i77.0.4.analogout3-maxlim [AXIS_4]OUTPUT_MAX_LIMIT
setp hm2_5i25.0.7i77.0.4.analogout4-scalemax [AXIS_4]FEEDBACK_SCALE
setp hm2_5i25.0.7i77.0.4.analogout4-minlim [AXIS_4]FEEDBACK_MIN_LIMIT
setp hm2_5i25.0.7i77.0.4.analogout4-maxlim [AXIS_4]FEEDBACK_MAX_LIMIT
net c-output => hm2_5i25.0.7i77.0.4.analogout3
net c-pos-cmd axis.4.motor-pos-cmd
net c-enable axis.4.amp-enable-out
# ---Encoder feedback signals/setup---
setp hm2_5i25.0.encoder.07.counter-mode 0
setp hm2_5i25.0.encoder.07.filter 1
setp hm2_5i25.0.encoder.07.index-invert 0
setp hm2_5i25.0.encoder.07.index-mask 0
setp hm2_5i25.0.encoder.07.index-mask-invert 0
setp hm2_5i25.0.encoder.07.scale [AXIS_4]ENCODER_SCALE
net c-pos-fb <= hm2_5i25.0.encoder.07.position
net c-vel-fb <= hm2_5i25.0.encoder.07.velocity => hm2_5i25.0.7i77.0.4.analogout4
net c-pos-fb => axis.4.motor-pos-fb
net c-index-enable axis.4.index-enable <=> hm2_5i25.0.encoder.07.index-enable
net c-pos-rawcounts <= hm2_5i25.0.encoder.07.rawcounts
# ---setup home / limit switch signals---
net home-c => axis.4.home-sw-in
net min-c => axis.4.neg-lim-sw-in
net max-c => axis.4.pos-lim-sw-in
#********************
# Axis C
#********************
[AXIS_4]
TYPE = ANGULAR
HOME = 0.0
FERROR = 0.5
MIN_FERROR = 0.05
MAX_VELOCITY = 360.0
MAX_ACCELERATION = 1200.0
P = 50.0
I = 0.0
D = 0.0
FF0 = 0.0
FF1 = 0.0
FF2 = 0.0
BIAS = 0.0
DEADBAND = 0.0
MAX_OUTPUT = 0.0
ENCODER_SCALE = 3000.0
OUTPUT_SCALE = -500.0
OUTPUT_MIN_LIMIT = -10.0
OUTPUT_MAX_LIMIT = 10.0
FEEDBACK_SCALE = 1000.0
FEEDBACK_MIN_LIMIT = -10.0
FEEDBACK_MAX_LIMIT = 10.0
MIN_LIMIT = -9999.0
MAX_LIMIT = 9999.0
HOME_OFFSET = 0.000000
HOME_SEARCH_VEL = -2.000000
HOME_LATCH_VEL = -0.500000
HOME_FINAL_VEL = 0.000000
HOME_USE_INDEX = YES
HOME_SEQUENCE = 3
Please Log in or Create an account to join the conversation.
OUTPUT_SCALE = -500.0
OUTPUT_MIN_LIMIT = -10.0
OUTPUT_MAX_LIMIT = 10.0
FEEDBACK_SCALE = 1000.0
FEEDBACK_MIN_LIMIT = -10.0
FEEDBACK_MAX_LIMIT = 10.0
These settings will bound the maximum analog output voltage of the 7I77 to +-200 mv (output) and +-100 mv (feedback)
so cannot possibly work. I would just use the defaults unless you have a very good reason to change them, for example
these settings will not bound the maximum outputs
OUTPUT_SCALE = -10.0
OUTPUT_MIN_LIMIT = -10.0
OUTPUT_MAX_LIMIT = 10.0
FEEDBACK_SCALE = 10.0
FEEDBACK_MIN_LIMIT = -10.0
or if you do change the analog out scale:
OUTPUT_SCALE = -500.0
OUTPUT_MIN_LIMIT = -500.0
OUTPUT_MAX_LIMIT = 500.0
FEEDBACK_SCALE = 1000.0
FEEDBACK_MIN_LIMIT = -1000.0
FEEDBACK_MAX_LIMIT = 1000.0
Since you have dual feedback with the 7I77/encoder simulating a tachometer you should make sure the velocity
loop is stable and calibrated (you get the expected velocity from the velocity command) before attempting to tune the
position PID loop
The 7I77 can read encoder inputs up to about 1 MHz with the filter on, this is about 6000 RPM with a 2500 line encoder
so its unlikely you are running into this limit
Please Log in or Create an account to join the conversation.
I have checked the encoder and replaced by another one, which does not produce this kind of error, and counting at full speed too. Even the direction changed, dont really know how it might happen, but needed to change the output scale to get right directions.
Next changed the
OUTPUT_SCALE = 1500
OUTPUT_MIN_LIMIT = -1500.0
OUTPUT_MAX_LIMIT = 1500.0
because I read somewhere that this motor (Fanuc DC servo motor 10M) can do 1500RPM so 1500 is reasonable output.
Completly disabled feedback, by commenting out the relevant line in hal config, and overridden the TGAL error by jumper.
I did manual PID tuning, with the at_pid module because I got an immediate runaway on autotuning start, dont know why.
The results I get are a good start for the fine tuning.
P = 350.0
I = 20.0
D = 2.8
all others are 0
with
FERROR and MIN_FERROR set to 0.1(mm) I got no following errors, however I can see in the scope the there is a slight overshoot at stopping.
I tried to get rid of this, but without success, need to read more or get some advices
Please Log in or Create an account to join the conversation.
- tommylight
- Away
- Moderator
- Posts: 19202
- Thank you received: 6436
Please Log in or Create an account to join the conversation.
Another question:
This rotary table has a hidro-pneumatic braking system which can lock the table in position. What would be the best way to control a (two state) valve to engage the brake?
So when in position and no commanded move, the brake should be engaged.
But there are other things that worth consideration: pid control should be paused or set up a way it wont overdrive the motor while the brake is engaged.
Which parameter would You monitor to disengage the brake? Commanded velocity? Ferror? or in what combination ?
Thanks
Please Log in or Create an account to join the conversation.
- tommylight
- Away
- Moderator
- Posts: 19202
- Thank you received: 6436
There is a comp in linuxcnc for exactly that type of locked rotary, but i can not recal how it is named. Check the list of comps.
Please Log in or Create an account to join the conversation.
1)
LOCKING_INDEXER = 1
in the axis config which would set pins axis.N.unlock & axis.N.is-unlocked, but this prevents coordinated moves, or moving other axes when in unlocked state.
linuxcnc.org/docs/html/config/ini-config.html
Thats not what I want, bacause the machine is capable of simultaneous 5 axis machining by the mechanics, so I just want to lock the table when it is not used. Or If I not using the LOCKING_INDEXER just simpli rely on pins than it will work? I will try it next week.
2)
the other one is the motion.spindle-locked pin, which I think is only for spindles and M19.
linuxcnc.org/docs/html/man/man9/motion.9.html
But these are standard motion components is this what You mentioned or there is a separate hal comp Im not aware of?
I read Your guide, already, but I may need to read it again a few times
Thanks
Please Log in or Create an account to join the conversation.
- tommylight
- Away
- Moderator
- Posts: 19202
- Thank you received: 6436
As for the tutorial, if you think something is not explained properly, please let me know so i can correct it. As i mentioned there, i am not good at explaining things, that was an exercise for me.
Please Log in or Create an account to join the conversation.
Im suspecting that the velocity feedback loop causing my problems, as it is working like a double pid loop (a pid of pid) and interfere with my settings.
Has anyone set up a FANUC DC velocity control unit ever? what would be the correct procedure to set it up?
Thanks
Please Log in or Create an account to join the conversation.
- Todd Zuercher
- Offline
- Platinum Member
- Posts: 5007
- Thank you received: 1441
Are your Fanuc drives getting a tack feedback? Most older Fanuc controls/DC amp combos I've worked with have the control generate a tacho signal from the encoder to send to the amp. Without tach feedback I believe you will be running in torque mode, and tuning strategies will be different and probably more difficult.
Please Log in or Create an account to join the conversation.