Mirror
/
klipper-dgus
mirror of https://github.com/Desuuuu/klipper.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

delta: Minor change to get_position_from_stable() 

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2018-07-14 11:35:13 -04:00
parent 244d0aa65b
commit ce0f4fe038
2 changed files with 22 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
klippy/extras/delta_calibrate.py
View File

@ -4,7 +4,7 @@
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import math, logging
import probe, mathutil, kinematics.delta
import probe, mathutil
class DeltaCalibrate:
def __init__(self, config):
@ -40,20 +40,18 @@ class DeltaCalibrate:
logging.info("Initial delta_calibrate parameters: %s", params)
adj_params = ('endstop_a', 'endstop_b', 'endstop_c', 'radius',
'angle_a', 'angle_b')
get_position_from_stable = kinematics.delta.get_position_from_stable
def delta_errorfunc(params):
total_error = 0.
for spos in positions:
x, y, z = get_position_from_stable(spos, params)
for x, y, z in kin.get_positions_from_stable(positions, params):
total_error += (z - z_offset)**2
return total_error
new_params = mathutil.coordinate_descent(
adj_params, params, delta_errorfunc)
logging.info("Calculated delta_calibrate parameters: %s", new_params)
for spos in positions:
logging.info("orig: %s new: %s",
get_position_from_stable(spos, params),
get_position_from_stable(spos, new_params))
old_positions = kin.get_positions_from_stable(positions, params)
new_positions = kin.get_positions_from_stable(positions, new_params)
for oldpos, newpos in zip(old_positions, new_positions):
logging.info("orig: %s new: %s", oldpos, newpos)
self.gcode.respond_info(
"stepper_a: position_endstop: %.6f angle: %.6f\n"
"stepper_b: position_endstop: %.6f angle: %.6f\n"

28
klippy/kinematics/delta.py
View File

@ -174,18 +174,22 @@ class DeltaKinematics:
'angle_c': self.angles[2], 'radius': self.radius,
'arm_a': self.arm_lengths[0], 'arm_b': self.arm_lengths[1],
'arm_c': self.arm_lengths[2] }
def get_position_from_stable(spos, params):
angles = [params['angle_a'], params['angle_b'], params['angle_c']]
radius = params['radius']
radius2 = radius**2
towers = [(math.cos(angle) * radius, math.sin(angle) * radius)
for angle in map(math.radians, angles)]
arm2 = [a**2 for a in [params['arm_a'], params['arm_b'], params['arm_c']]]
endstops = [params['endstop_a'], params['endstop_b'], params['endstop_c']]
sphere_coords = [(t[0], t[1], es + math.sqrt(a2 - radius2) - p)
for t, es, a2, p in zip(towers, endstops, arm2, spos)]
return mathutil.trilateration(sphere_coords, arm2)
def get_positions_from_stable(self, stable_positions, params):
angle_names = ['angle_a', 'angle_b', 'angle_c']
angles = [math.radians(params[an]) for an in angle_names]
radius = params['radius']
radius2 = radius**2
towers = [(math.cos(a) * radius, math.sin(a) * radius) for a in angles]
arm2 = [params[an]**2 for an in ['arm_a', 'arm_b', 'arm_c']]
endstop_names = ['endstop_a', 'endstop_b', 'endstop_c']
endstops = [params[en] + math.sqrt(a2 - radius2)
for en, a2 in zip(endstop_names, arm2)]
out = []
for spos in stable_positions:
sphere_coords = [(t[0], t[1], es - sp)
for t, es, sp in zip(towers, endstops, spos)]
out.append(mathutil.trilateration(sphere_coords, arm2))
return out
def load_kinematics(toolhead, config):
return DeltaKinematics(toolhead, config)