156 lines
4.5 KiB
Python
Executable File
156 lines
4.5 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
#
|
|
# Copyright 2022 Petter Reinholdtsen
|
|
#
|
|
# This program is free software; you can redistribute it and/or modify
|
|
# it under the terms of the GNU General Public License as published by
|
|
# the Free Software Foundation; either version 2 of the License, or
|
|
# (at your option) any later version.
|
|
#
|
|
# This program is distributed in the hope that it will be useful, but
|
|
# WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
# General Public License for more details.
|
|
#
|
|
# You should have received a copy of the GNU General Public License
|
|
# along with this program; if not, write to the Free Software
|
|
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
|
|
# 02110-1301 USA.
|
|
|
|
"""
|
|
|
|
LinuxCNC automated PID auto-tuner
|
|
=================================
|
|
|
|
Automatically conduct a automatic PID tuning using the pid
|
|
LinuxCNC component.
|
|
|
|
This script first home the axis, then move it to the middle of its range.
|
|
Also extend the f-error value used by the PID controller. Next, disable
|
|
the PID controller (tune-mode=1), start the tuning process (tune-start=1)
|
|
and wait for it to finish before enabling the PID controller again.
|
|
|
|
This script assume the pid component is configured like this:
|
|
|
|
loadrt pid names=pid.x,pid.y,pid.z
|
|
|
|
It also assume the axis ranges are correct to be able to position the axes
|
|
in their middle positions.
|
|
"""
|
|
|
|
import time
|
|
|
|
import linuxcnc
|
|
import linuxcnc_util
|
|
import hal
|
|
|
|
axis = 'x'
|
|
axisid = 0
|
|
|
|
# Some values that need to be replaced during tuning
|
|
replace = {
|
|
# f"pid.{axis}.tune-cycles": "0x40", # Increase number of back and forth movement (default 50)
|
|
f"pid.{axis}.tune-effort": "0.2", # Set effort, should start low
|
|
f"ini.{axisid}.ferror": "100", # Extend to not trigger following error
|
|
f"ini.{axisid}.min_ferror": "100", # Extend to not trigger following error
|
|
}
|
|
|
|
def set_mode(mode):
|
|
s.poll()
|
|
if linuxcnc.MODE_MDI != s.task_mode:
|
|
c.mode(linuxcnc.MODE_MDI)
|
|
c.wait_complete()
|
|
s.poll()
|
|
def is_homed():
|
|
isHomed=True
|
|
for i,h in enumerate(s.homed):
|
|
if i >= s.joints: break
|
|
isHomed = isHomed and h
|
|
return isHomed
|
|
|
|
c = linuxcnc.command()
|
|
s = linuxcnc.stat();
|
|
e = linuxcnc.error_channel()
|
|
l = linuxcnc_util.LinuxCNC(command=c, status=s, error=e)
|
|
|
|
s.poll()
|
|
|
|
# Bootstrap hal library
|
|
hc = hal.component("hal-watcher")
|
|
|
|
error = []
|
|
|
|
# Make sure machine is turned on and out of estop
|
|
if linuxcnc.STATE_ESTOP == s.task_state or linuxcnc.STATE_OFF == s.task_state:
|
|
error.append("error: You need to start the machine and make sure ESTOP is not active")
|
|
|
|
effortpin = f"pid.{axis}.tune-effort"
|
|
try:
|
|
v = hal.get_value(effortpin)
|
|
except RuntimeError as e:
|
|
error.append(f"error: failed to find at_pin specific pin {effortpin} in HAL")
|
|
error.append(f"error: Did you remember to replace 'loadrt pid' with 'loadrt at_pid' for axis {axis}?")
|
|
|
|
if error:
|
|
print("\n".join(error))
|
|
exit(1)
|
|
|
|
# Start homing, wait for it to complete
|
|
set_mode(linuxcnc.MODE_MANUAL)
|
|
if not is_homed():
|
|
print("Requesting axis homing.")
|
|
c.teleop_enable(0)
|
|
c.wait_complete()
|
|
c.home(-1)
|
|
c.wait_complete()
|
|
l.wait_for_home([1, 1, 1, 0, 0, 0, 0, 0, 0])
|
|
else:
|
|
print("Already homed, skipping axis homing.")
|
|
|
|
# Locate mid range and move to the middle, switch to MDI mode while moving
|
|
|
|
midpos = (
|
|
s.joint[axisid]["max_position_limit"]
|
|
+ s.joint[axisid]["min_position_limit"]
|
|
) / 2
|
|
print(f"Moving {axis} to position {midpos}")
|
|
set_mode(linuxcnc.MODE_MDI)
|
|
c.mdi(f"G0 {axis}{midpos}")
|
|
c.wait_complete()
|
|
set_mode(linuxcnc.MODE_MANUAL)
|
|
|
|
|
|
print("info: Will start PID auto tuning in 5 seconds")
|
|
time.sleep(5)
|
|
|
|
# Replace some old values
|
|
oldvals = {}
|
|
for pin in replace.keys():
|
|
print(f"Keeping {pin}")
|
|
oldvals[pin] = hal.get_value(pin)
|
|
hal.set_p(pin, replace[pin])
|
|
|
|
# Run PID autotuning
|
|
print("Starting PID tuning")
|
|
hal.set_p(f"pid.{axis}.tune-mode", "1")
|
|
hal.set_p(f"pid.{axis}.tune-start", "1")
|
|
|
|
startpin = f"pid.{axis}.tune-start"
|
|
tunerunning = hal.get_value(startpin)
|
|
while hal.get_value(startpin) == tunerunning:
|
|
time.sleep(0.1)
|
|
hal.set_p(f"pid.{axis}.tune-mode", "0")
|
|
print(f"PID tuning of axis {axis} completed.")
|
|
|
|
# Give axis time to return to where it want to be before restoring old
|
|
# error limits. Reissuing G0 from above to be able to use
|
|
# wait_complete() to detect when it is done.
|
|
set_mode(linuxcnc.MODE_MDI)
|
|
c.mdi(f"G0 {axis}{midpos}")
|
|
c.wait_complete()
|
|
set_mode(linuxcnc.MODE_MANUAL)
|
|
|
|
# Reinsert old values
|
|
for pin in replace.keys():
|
|
hal.set_p(pin, "%s" % oldvals[pin])
|