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. # This file may be distributed under the terms of the GNU GPLv3 license.
import math, logging import math, logging
import probe, mathutil, kinematics.delta import probe, mathutil
class DeltaCalibrate: class DeltaCalibrate:
def __init__(self, config): def __init__(self, config):
@ -40,20 +40,18 @@ class DeltaCalibrate:
logging.info("Initial delta_calibrate parameters: %s", params) logging.info("Initial delta_calibrate parameters: %s", params)
adj_params = ('endstop_a', 'endstop_b', 'endstop_c', 'radius', adj_params = ('endstop_a', 'endstop_b', 'endstop_c', 'radius',
'angle_a', 'angle_b') 'angle_a', 'angle_b')
get_position_from_stable = kinematics.delta.get_position_from_stable
def delta_errorfunc(params): def delta_errorfunc(params):
total_error = 0. total_error = 0.
for spos in positions: for x, y, z in kin.get_positions_from_stable(positions, params):
x, y, z = get_position_from_stable(spos, params)
total_error += (z - z_offset)**2 total_error += (z - z_offset)**2
return total_error return total_error
new_params = mathutil.coordinate_descent( new_params = mathutil.coordinate_descent(
adj_params, params, delta_errorfunc) adj_params, params, delta_errorfunc)
logging.info("Calculated delta_calibrate parameters: %s", new_params) logging.info("Calculated delta_calibrate parameters: %s", new_params)
for spos in positions: old_positions = kin.get_positions_from_stable(positions, params)
logging.info("orig: %s new: %s", new_positions = kin.get_positions_from_stable(positions, new_params)
get_position_from_stable(spos, params), for oldpos, newpos in zip(old_positions, new_positions):
get_position_from_stable(spos, new_params)) logging.info("orig: %s new: %s", oldpos, newpos)
self.gcode.respond_info( self.gcode.respond_info(
"stepper_a: position_endstop: %.6f angle: %.6f\n" "stepper_a: position_endstop: %.6f angle: %.6f\n"
"stepper_b: 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, 'angle_c': self.angles[2], 'radius': self.radius,
'arm_a': self.arm_lengths[0], 'arm_b': self.arm_lengths[1], 'arm_a': self.arm_lengths[0], 'arm_b': self.arm_lengths[1],
'arm_c': self.arm_lengths[2] } 'arm_c': self.arm_lengths[2] }
def get_positions_from_stable(self, stable_positions, params):
def get_position_from_stable(spos, params): angle_names = ['angle_a', 'angle_b', 'angle_c']
angles = [params['angle_a'], params['angle_b'], params['angle_c']] angles = [math.radians(params[an]) for an in angle_names]
radius = params['radius'] radius = params['radius']
radius2 = radius**2 radius2 = radius**2
towers = [(math.cos(angle) * radius, math.sin(angle) * radius) towers = [(math.cos(a) * radius, math.sin(a) * radius) for a in angles]
for angle in map(math.radians, angles)] arm2 = [params[an]**2 for an in ['arm_a', 'arm_b', 'arm_c']]
arm2 = [a**2 for a in [params['arm_a'], params['arm_b'], params['arm_c']]] endstop_names = ['endstop_a', 'endstop_b', 'endstop_c']
endstops = [params['endstop_a'], params['endstop_b'], params['endstop_c']] endstops = [params[en] + math.sqrt(a2 - radius2)
sphere_coords = [(t[0], t[1], es + math.sqrt(a2 - radius2) - p) for en, a2 in zip(endstop_names, arm2)]
for t, es, a2, p in zip(towers, endstops, arm2, spos)] out = []
return mathutil.trilateration(sphere_coords, arm2) 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): def load_kinematics(toolhead, config):
return DeltaKinematics(toolhead, config) return DeltaKinematics(toolhead, config)