219 lines
7.7 KiB
Perl
Executable File
219 lines
7.7 KiB
Perl
Executable File
#!/usr/bin/perl
|
|
|
|
use strict;
|
|
|
|
# from http://www.eetimes.com/design/embedded/4006438/Generate-stepper-motor-speed-profiles-in-real-time
|
|
# f = F_CPU
|
|
# a = 1 / steps_per_mm (ie mm per step)
|
|
# w = speed (mm/sec)
|
|
# w' = accel (mm/sec/sec)
|
|
# c = timer ticks (integer)
|
|
# n = acceleration value (integer)
|
|
# C0 = f * sqrt(2 a / w' )
|
|
# = F_CPU * sqrt(2 / accel / steps_per_mm)
|
|
# Cn = C0 * (sqrt(n + 1) - sqrt(n))
|
|
# approximation:
|
|
# Cn = Cn-1 - ((2 * Cn-1) / (4n + 1))
|
|
# detach n from step index:
|
|
# Ci = Ci-1 - ((2 * Ci-1) / (4ni + 1))
|
|
# ramp down to stop in m steps:
|
|
# ni = i - m
|
|
# inaccuracies: C1 is inaccurate
|
|
# use c1 = 0.4056 * C0
|
|
|
|
# number of steps to reach speed w with acceleration w':
|
|
# n = (w^2 / (2 * a * w'))
|
|
# = w * w * steps_per_mm / 2 / w'
|
|
|
|
# changes of acceleration
|
|
# (n1 + 0.5).w'1 = (n2 + 0.5).w'2
|
|
# n2 = ((n1 + 0.5) * w'1 / w'2) - 0.5
|
|
|
|
# when to decelerate (short move of m steps)
|
|
# n = m.w'2 / (w'1 + w'2)
|
|
|
|
|
|
my $f_cpu = 16000000;
|
|
my ($x_mm, $y_mm, $f_mm_min) = (40, 34, 1500);
|
|
my ($x_steps_per_mm, $y_steps_per_mm) = (320, 320);
|
|
|
|
my ($x_accel_mm_s_s, $y_accel_mm_s_s) = (9, 5);
|
|
my ($x_decel_mm_s_s, $y_decel_mm_s_s) = (3, 8);
|
|
|
|
# **************************************
|
|
|
|
my ($x_um_per_step, $y_um_per_step) = (1000 / $x_steps_per_mm, 1000 / $y_steps_per_mm);
|
|
|
|
my ($x_delta, $y_delta) = ($x_mm * $x_steps_per_mm, $y_mm * $y_steps_per_mm);
|
|
|
|
my $distance = sqrt(($x_delta * $x_delta * $x_um_per_step * $x_um_per_step) + ($y_delta * $y_delta * $y_um_per_step * $y_um_per_step));
|
|
|
|
my $duration = $distance * $f_cpu * 60 / 1000 / $f_mm_min;
|
|
|
|
printf "MOVE %dmmx%dmm@%dmm/min: %d um (%d mm), %d ticks (%dms), %gmm/min (%gmm/s)\n", $x_mm, $y_mm, $f_mm_min, $distance, $distance / 1000, $duration, $duration / $f_cpu * 1000, $distance / 1000 / $duration * $f_cpu * 60, $distance / 1000 / $duration * $f_cpu;
|
|
|
|
my ($x_speed, $y_speed) = ($x_delta * $x_um_per_step / $duration * $f_cpu / 1000, $y_delta * $y_um_per_step / $duration * $f_cpu / 1000);
|
|
|
|
printf "X: %gmm/s, Y: %gmm/s\n", $x_speed, $y_speed;
|
|
|
|
# **************************************
|
|
# n steps to accelerate to w at w' = w * w * steps_per_mm / 2 / w'
|
|
my $x_steps_to_accel = $x_speed * $x_speed * $x_steps_per_mm / 2 / $x_accel_mm_s_s;
|
|
my $y_steps_to_accel = $y_speed * $y_speed * $y_steps_per_mm / 2 / $y_accel_mm_s_s;
|
|
|
|
printf "Xns: %d (%dum), Yns: %d (%dum)\n", $x_steps_to_accel, $x_steps_to_accel * $x_um_per_step, $y_steps_to_accel, $y_steps_to_accel * $y_um_per_step;
|
|
|
|
# now we work out which axis reaches plateau last
|
|
if ($x_steps_to_accel / $x_steps_per_mm > $y_steps_to_accel / $y_steps_per_mm) {
|
|
# x reaches last- slow down Y
|
|
# when X reaches plateau, where is Y?
|
|
# x_steps / x_distance = y_steps / y_distance
|
|
# y_steps = x_steps / x_distance * y_distance
|
|
my $y_plateau_steps = $x_steps_to_accel / $x_delta * $y_delta;
|
|
$y_accel_mm_s_s = $y_speed * $y_speed * $y_steps_per_mm / 2 / $y_plateau_steps;
|
|
}
|
|
else {
|
|
# y reaches last- slow down X
|
|
# when Y reaches plateau, where is X?
|
|
# y_steps / y_distance = x_steps / x_distance
|
|
# x_steps = y_steps / y_distance * x_distance
|
|
my $x_plateau_steps = $y_steps_to_accel / $y_delta * $x_delta;
|
|
$x_accel_mm_s_s = $x_speed * $x_speed * $x_steps_per_mm / 2 / $x_plateau_steps;
|
|
}
|
|
|
|
$x_steps_to_accel = $x_speed * $x_speed * $x_steps_per_mm / 2 / $x_accel_mm_s_s;
|
|
$y_steps_to_accel = $y_speed * $y_speed * $y_steps_per_mm / 2 / $y_accel_mm_s_s;
|
|
|
|
printf "new Xns: %d, Yns: %d\n", $x_steps_to_accel, $y_steps_to_accel;
|
|
printf "Xaccel: %g, Yaccel: %g\n", $x_accel_mm_s_s, $y_accel_mm_s_s;
|
|
|
|
# now we work out which axis has to decelerate first
|
|
# n steps to decelerate from w at w' = w * w * steps_per_mm / 2 / w'
|
|
my $x_steps_to_decel = $x_speed * $x_speed * $x_steps_per_mm / 2 / $x_decel_mm_s_s;
|
|
my $y_steps_to_decel = $y_speed * $y_speed * $y_steps_per_mm / 2 / $y_decel_mm_s_s;
|
|
|
|
printf "Xds: %d, Yds: %d\n", $x_steps_to_decel, $y_steps_to_decel;
|
|
|
|
# now we work out which axis reaches plateau last
|
|
if ($x_steps_to_decel / $x_steps_per_mm > $y_steps_to_decel / $y_steps_per_mm) {
|
|
# x reaches last- slow down Y
|
|
# when X reaches plateau, where is Y?
|
|
# x_steps / x_distance = y_steps / y_distance
|
|
# y_steps = x_steps / x_distance * y_distance
|
|
my $y_plateau_steps = $x_steps_to_decel / $x_delta * $y_delta;
|
|
$y_decel_mm_s_s = $y_speed * $y_speed * $y_steps_per_mm / 2 / $y_plateau_steps;
|
|
}
|
|
else {
|
|
# y reaches last- slow down X
|
|
# when Y reaches plateau, where is X?
|
|
# y_steps / y_distance = x_steps / x_distance
|
|
# x_steps = y_steps / y_distance * x_distance
|
|
my $x_plateau_steps = $y_steps_to_decel / $y_delta * $x_delta;
|
|
$x_decel_mm_s_s = $x_speed * $x_speed * $x_steps_per_mm / 2 / $x_plateau_steps;
|
|
}
|
|
|
|
my $x_steps_to_decel = $x_speed * $x_speed * $x_steps_per_mm / 2 / $x_decel_mm_s_s;
|
|
my $y_steps_to_decel = $y_speed * $y_speed * $y_steps_per_mm / 2 / $y_decel_mm_s_s;
|
|
|
|
printf "new Xds: %d, Yds: %d\n", $x_steps_to_decel, $y_steps_to_decel;
|
|
|
|
if (($x_steps_to_accel + $x_steps_to_decel) > $x_delta) {
|
|
# we will never reach full speed, however this doesn't affect our accel trimming so we can do this last
|
|
# n = (m.w'2) / (w'1 + w'2)
|
|
$x_steps_to_decel = int($x_delta * $x_decel_mm_s_s / ($x_accel_mm_s_s + $x_decel_mm_s_s));
|
|
}
|
|
if (($y_steps_to_accel + $y_steps_to_decel) > $y_delta) {
|
|
# we will never reach full speed, however this doesn't affect our accel trimming so we can do this last
|
|
# n = (m.w'2) / (w'1 + w'2)
|
|
$y_steps_to_decel = int($y_delta * $y_decel_mm_s_s / ($y_accel_mm_s_s + $y_decel_mm_s_s));
|
|
}
|
|
|
|
printf "new Xds: %d, Yds: %d\n", $x_steps_to_decel, $y_steps_to_decel;
|
|
|
|
# now we work out initial delays (C0)
|
|
|
|
# = F_CPU * sqrt(2 / accel / steps_per_mm)
|
|
my $x_c = int($f_cpu * sqrt(2 / $x_accel_mm_s_s / $x_steps_per_mm));
|
|
my $y_c = int($f_cpu * sqrt(2 / $y_accel_mm_s_s / $y_steps_per_mm));
|
|
|
|
# now we work out speed limits so we know when to stop accelerating
|
|
|
|
# mm/sec -> ticks per step
|
|
# mm/sec * steps/mm = steps/sec
|
|
# 1 / (mm/sec * steps/sec) = secs/step
|
|
# f_cpu / (mm/sec * steps/sec) = ticks/step
|
|
my $x_min_c = int($f_cpu / ($x_speed * $x_steps_per_mm));
|
|
my $y_min_c = int($f_cpu / ($y_speed * $y_steps_per_mm));
|
|
|
|
printf "XminC: %dt/s, YminC: %dt/s\n", $x_min_c, $y_min_c;
|
|
|
|
# now we set up counters
|
|
|
|
my $x_n = 1;
|
|
my $y_n = 1;
|
|
|
|
printf "Xc0: %d (%gus), Yc0: %d (%gus)\n", $x_c, $x_c / $f_cpu * 1000000, $y_c, $y_c / $f_cpu * 1000000;
|
|
|
|
my $elapsed_ticks = ($x_c < $y_c)?$x_c:$y_c;
|
|
my ($x_cd, $y_cd) = ($x_c, $y_c);
|
|
|
|
my $total_ticks = 0;
|
|
|
|
printf stderr "%d %.3f %.3f\n", $total_ticks, $x_delta / $x_steps_per_mm, $y_delta / $y_steps_per_mm;
|
|
|
|
while ($x_delta > 0 || $y_delta > 0) {
|
|
$x_cd -= $elapsed_ticks;
|
|
$y_cd -= $elapsed_ticks;
|
|
if ($x_cd <= 0 && $x_delta > 0) {
|
|
$x_delta--;
|
|
if ($x_delta == int($x_steps_to_decel)) {
|
|
# start decelerating
|
|
$x_n = -$x_delta;
|
|
printf "[X DECEL]";
|
|
}
|
|
printf "[X: %ds:%gmm, %dc, %dn] ", $x_delta, $x_delta / $x_steps_per_mm, $x_c, $x_n;
|
|
if ($x_n == 1) {
|
|
$x_c = int(0.4056 * $x_c * 256) / 256;
|
|
}
|
|
else {
|
|
$x_c = int(($x_c - ((2 * $x_c) / ((4 * $x_n) + 1))) * 256) / 256;
|
|
}
|
|
$x_cd = $x_c;
|
|
$x_n++;
|
|
$x_c = $x_min_c if $x_c < $x_min_c;
|
|
}
|
|
if ($y_cd <= 0 && $y_delta > 0) {
|
|
$y_delta--;
|
|
if ($y_delta == int($y_steps_to_decel)) {
|
|
$y_n = -$y_delta;
|
|
printf "[Y DECEL]";
|
|
}
|
|
printf "[Y: %ds:%gmm, %dc, %dn] ", $y_delta, $y_delta / $y_steps_per_mm, $y_c, $y_n;
|
|
if ($y_n == 1) {
|
|
$y_c = int(0.4056 * $y_c * 256) / 256;
|
|
}
|
|
else {
|
|
$y_c = int(($y_c - ((2 * $y_c) / ((4 * $y_n) + 1))) * 256) / 256;
|
|
}
|
|
$y_cd = $y_c;
|
|
$y_n++;
|
|
$y_c = $y_min_c if $y_c < $y_min_c;
|
|
}
|
|
|
|
printf stderr "%d %.3f %.3f\n", $total_ticks, $x_delta / $x_steps_per_mm, $y_delta / $y_steps_per_mm;
|
|
|
|
$elapsed_ticks = 2**31;
|
|
$elapsed_ticks = $x_cd
|
|
if $x_delta > 0 && $elapsed_ticks > $x_cd;
|
|
$elapsed_ticks = $y_cd
|
|
if $y_delta > 0 && $elapsed_ticks > $y_cd;
|
|
|
|
if ($elapsed_ticks < 2**31) {
|
|
$total_ticks += $elapsed_ticks;
|
|
printf "wait %d ticks\n", $elapsed_ticks;
|
|
}
|
|
else {
|
|
print "finished\n";
|
|
}
|
|
}
|