From a28526dddbce79a9cb95be236abd0291f2fc99b6 Mon Sep 17 00:00:00 2001 From: Michael Moon Date: Tue, 12 Apr 2011 19:40:40 +1000 Subject: [PATCH] algorithm proof of concept in perl, partial implementation in c --- research/alg.c | 379 ++++++++++++++++++++++++++++++++++++++++++++++ research/alg.make | 4 + research/alg.pl | 218 ++++++++++++++++++++++++++ research/alg.plot | 15 ++ 4 files changed, 616 insertions(+) create mode 100644 research/alg.c create mode 100755 research/alg.make create mode 100755 research/alg.pl create mode 100644 research/alg.plot diff --git a/research/alg.c b/research/alg.c new file mode 100644 index 0000000..105f92c --- /dev/null +++ b/research/alg.c @@ -0,0 +1,379 @@ +#include +#include +#include +#include + +#define F_CPU 16000000UL + +#define X_STEPS_PER_MM 320.0 +#define Y_STEPS_PER_MM 320.0 +#define Z_STEPS_PER_MM 200.0 +#define E_STEPS_PER_MM 287.0 + +#define X_UM_PER_STEP (1000.0 / X_STEPS_PER_MM) +#define Y_UM_PER_STEP (1000.0 / Y_STEPS_PER_MM) +#define Z_UM_PER_STEP (1000.0 / Z_STEPS_PER_MM) +#define E_UM_PER_STEP (1000.0 / E_STEPS_PER_MM) + +#define X_ACCEL_MM_S_S 9.0 +#define Y_ACCEL_MM_S_S 5.0 +#define Z_ACCEL_MM_S_S 1.0 +#define E_ACCEL_MM_S_S 15.0 + +#define X_DECEL_MM_S_S 3.0 +#define Y_DECEL_MM_S_S 8.0 +#define Z_DECEL_MM_S_S 1.0 +#define E_DECEL_MM_S_S 18.0 + + +// courtesy of http://www.flipcode.com/archives/Fast_Approximate_Distance_Functions.shtml +/*! linear approximation 2d distance formula +\param dx distance in X plane +\param dy distance in Y plane +\return 3-part linear approximation of \f$\sqrt{\Delta x^2 + \Delta y^2}\f$ + +see http://www.flipcode.com/archives/Fast_Approximate_Distance_Functions.shtml +*/ +uint32_t approx_distance( uint32_t dx, uint32_t dy ) +{ + uint32_t min, max, approx; + + if ( dx < dy ) + { + min = dx; + max = dy; + } else { + min = dy; + max = dx; + } + + approx = ( max * 1007 ) + ( min * 441 ); + if ( max < ( min << 4 )) + approx -= ( max * 40 ); + + // add 512 for proper rounding + return (( approx + 512 ) >> 10 ); +} + +// courtesy of http://www.oroboro.com/rafael/docserv.php/index/programming/article/distance +/*! linear approximation 3d distance formula +\param dx distance in X plane +\param dy distance in Y plane +\param dz distance in Z plane +\return 3-part linear approximation of \f$\sqrt{\Delta x^2 + \Delta y^2 + \Delta z^2}\f$ + +see http://www.oroboro.com/rafael/docserv.php/index/programming/article/distance +*/ +uint32_t approx_distance_3( uint32_t dx, uint32_t dy, uint32_t dz ) +{ + uint32_t min, med, max, approx; + + if ( dx < dy ) + { + min = dy; + med = dx; + } else { + min = dx; + med = dy; + } + + if ( dz < min ) + { + max = med; + med = min; + min = dz; + } else if ( dz < med ) { + max = med; + med = dz; + } else { + max = dz; + } + + approx = ( max * 860 ) + ( med * 851 ) + ( min * 520 ); + if ( max < ( med << 1 )) approx -= ( max * 294 ); + if ( max < ( min << 2 )) approx -= ( max * 113 ); + if ( med < ( min << 2 )) approx -= ( med * 40 ); + + // add 512 for proper rounding + return (( approx + 512 ) >> 10 ); +} + +/*! +integer square root algorithm +\param a find square root of this number +\return sqrt(a - 1) < returnvalue <= sqrt(a) + +see http://www.embedded-systems.com/98/9802fe2.htm +*/ +// courtesy of http://www.embedded-systems.com/98/9802fe2.htm +uint16_t int_sqrt(uint32_t a) { + uint32_t rem = 0; + uint32_t root = 0; + uint16_t i; + + for (i = 0; i < 16; i++) { + root <<= 1; + rem = ((rem << 2) + (a >> 30)); + a <<= 2; + root++; + if (root <= rem) { + rem -= root; + root++; + } + else + root--; + } + return (uint16_t) ((root >> 1) & 0xFFFFL); +} + +// this is an ultra-crude pseudo-logarithm routine, such that: +// 2 ^ msbloc(v) >= v +/*! crude logarithm algorithm +\param v value to find \f$log_2\f$ of +\return floor(log(v) / log(2)) +*/ +const uint8_t msbloc (uint32_t v) { + uint8_t i; + uint32_t c; + for (i = 31, c = 0x80000000; i; i--) { + if (v & c) + return i; + c >>= 1; + } + return 0; +} + + + +void move(int32_t dx, int32_t dy, int32_t dz, int32_t de, uint32_t f) { + uint32_t distance = 0; + uint32_t x_delta, y_delta, z_delta, e_delta; + uint32_t x_speed, y_speed, z_speed, e_speed; + uint32_t x_accel_distance, y_accel_distance, z_accel_distance, e_accel_distance; + uint32_t x_c, y_c, z_c, e_c; + int32_t x_n, y_n, z_n, e_n; + uint32_t x_cr, y_cr, z_cr, e_cr; + uint32_t x_minc, y_minc, z_minc, e_minc; + uint32_t x_accel = X_ACCEL_MM_S_S * 1000.0, y_accel = Y_ACCEL_MM_S_S * 1000.0, z_accel = Z_ACCEL_MM_S_S * 1000.0, e_accel = E_ACCEL_MM_S_S * 1000.0; + uint32_t duration; + + uint32_t accel_distance, decel_distance; + + uint32_t elapsed_ticks, total_ticks; + + // distance is micrometers + if ((dx != 0 || dy != 0) && dz == 0) + distance = approx_distance(dx * X_UM_PER_STEP, dy * Y_UM_PER_STEP); + if (dx == 0 && dy == 0 && dz != 0) + distance = dz * Z_UM_PER_STEP; + if (distance < 2 && de != 0) + distance = de * E_UM_PER_STEP; + if (distance == 0) + return; + + printf("distance: %dum\n", distance); + + // duration is microseconds + duration = distance * 3UL * (F_CPU / 50UL / f); + + printf("duration: %d ticks (%ldms)\n", duration, duration / (F_CPU / 1000UL)); + + // deltas are in steps + x_delta = labs(dx); + y_delta = labs(dy); + z_delta = labs(dz); + e_delta = labs(de); + + // speeds are in um per second + if (x_delta) + x_speed = x_delta * X_UM_PER_STEP * F_CPU / duration; + if (y_delta) + y_speed = y_delta * Y_UM_PER_STEP * F_CPU / duration; + if (z_delta) + z_speed = z_delta * Z_UM_PER_STEP * F_CPU / duration; + if (e_delta) + e_speed = e_delta * E_UM_PER_STEP * F_CPU / duration; + + printf("X speed: %dum/s, Y speed: %dum/s\n", x_speed, y_speed); + + accel_distance = 0; + +// n = w^2 / 2aw' +// my $x_steps_to_accel = $x_speed * $x_speed * $x_steps_per_mm / 2 / $x_accel_mm_s_s; +// x_accel_steps = (x_speed * x_speed / 1000000) * X_STEPS_PER_MM / 2 / X_ACCEL_MM_S_S; +// x_accel_steps = (x_delta * 1000 / X_STEPS_PER_MM * F_CPU / duration * x_delta * 1000 / X_STEPS_PER_MM * F_CPU / duration / 1000000) * X_STEPS_PER_MM / 2 / X_ACCEL_MM_S_S; +// x_accel_steps = (x_delta / X_STEPS_PER_MM * F_CPU / (distance * F_CPU * 3 / 50 / f) * x_delta / X_STEPS_PER_MM * F_CPU / (distance * F_CPU * 3 / 50 / f)) * X_STEPS_PER_MM / 2 / X_ACCEL_MM_S_S; +// x_accel_steps = (x_delta * x_delta / X_STEPS_PER_MM * F_CPU / distance / F_CPU / 3 * 50 * f / X_STEPS_PER_MM * F_CPU / distance / F_CPU / 3 * 50 * f) * X_STEPS_PER_MM / 2 / X_ACCEL_MM_S_S; +// x_accel_steps = (x_delta * x_delta / X_STEPS_PER_MM / distance / 3 * 50 * f / X_STEPS_PER_MM / distance / 3 * 50 * f) * X_STEPS_PER_MM / 2 / X_ACCEL_MM_S_S; +// x_accel_steps = (x_delta * x_delta * 1250 * f * f / X_STEPS_PER_MM / distance / distance / 3 / 3) / X_ACCEL_MM_S_S; +// x_accel_steps = (x_delta * f / distance / 3) * (x_delta * f / distance / 3) * 1250 / X_STEPS_PER_MM / X_ACCEL_MM_S_S; +// x_accel_distance = x_accel_steps * X_UM_PER_STEP; + #warning This calculation is susceptible to overflow! + if (x_delta) { + x_accel_distance = (x_delta * f / distance / 3UL) * (x_delta * f / distance / 3UL) * 1250UL / X_STEPS_PER_MM / X_ACCEL_MM_S_S * 1000UL / X_STEPS_PER_MM; + if (x_accel_distance > accel_distance) + accel_distance = x_accel_distance; + } + if (y_delta) { + y_accel_distance = (y_delta * f / distance / 3UL) * (y_delta * f / distance / 3UL) * 1250UL / Y_STEPS_PER_MM / Y_ACCEL_MM_S_S * 1000UL / Y_STEPS_PER_MM; + if (y_accel_distance > accel_distance) + accel_distance = y_accel_distance; + } + if (z_delta) { + z_accel_distance = (z_delta * f / distance / 3UL) * (z_delta * f / distance / 3UL) * 1250UL / Z_STEPS_PER_MM / Z_ACCEL_MM_S_S * 1000UL / Z_STEPS_PER_MM; + if (z_accel_distance > accel_distance) + accel_distance = z_accel_distance; + } + if (e_delta) { + e_accel_distance = (e_delta * f / distance / 3UL) * (e_delta * f / distance / 3UL) * 1250UL / E_STEPS_PER_MM / E_ACCEL_MM_S_S * 1000UL / E_STEPS_PER_MM; + if (e_accel_distance > accel_distance) + accel_distance = e_accel_distance; + } + + printf("Accel Distance: %dum\n", accel_distance); + + // n = w^2 / 2aw' + // w' = w^2 / 2an + // w' = w^2 * steps_per_mm / 2n + // x_accel = x_speed * x_speed * X_STEPS_PER_MM / 2 / (accel_distance * X_STEPS_PER_MM) + // x_accel = x_speed * x_speed / 2 / accel_distance / 1000 + // let's store in um/s2 instead of mm/s2 for precision + #warning This calculation is susceptible to overflow! + if (x_accel_distance < accel_distance) + x_accel = x_speed * x_speed / accel_distance / 2UL; + if (y_accel_distance < accel_distance) + y_accel = y_speed * y_speed / accel_distance / 2UL; + if (z_accel_distance < accel_distance) + z_accel = z_speed * z_speed / accel_distance / 2UL; + if (e_accel_distance < accel_distance) + e_accel = e_speed * e_speed / accel_distance / 2UL; + + printf("X accel: %dum/s2, Y accel: %dum/s2\n", x_accel, y_accel); + + // c0 = f . sqrt(2a / accel) + // = F_CPU * sqrt(2 / accel * steps_per_mm) + // = F_CPU * sqrt(2) / sqrt(accel / 1000) / sqrt(steps_per_mm) + // = F_CPU * sqrt(2) / int_sqrt(accel * steps_per_mm / 1000) + // = F_CPU * sqrt(2) * sqrt(1000) / int_sqrt(accel * steps_per_mm) + // = F_CPU / int_sqrt(accel * steps_per_mm) * (20 * sqrt(5)) + // 20.sqrt(5) ~= 313/7 (0.12%) + // = F_CPU / int_sqrt(accel * steps_per_mm) * 313 / 7 + // 2**32 / 313 is about 13MHz, so we can't start with F_CPU * 313 if F_CPU is above 13MHz + if (x_delta) { +// printf("x_accel(%u) * X_STEPS_PER_MM(%u) = %u, sqrt() = %u\n", x_accel, ((uint32_t) X_STEPS_PER_MM), x_accel * ((uint32_t) X_STEPS_PER_MM), int_sqrt(x_accel * ((uint32_t) X_STEPS_PER_MM))); + x_c = ((F_CPU * 256UL) / int_sqrt(x_accel * X_STEPS_PER_MM)) * 313UL / 7UL; +// x_c = F_CPU * sqrt(2.0 / x_accel * X_UM_PER_STEP); + x_minc = (F_CPU * 256UL) / (x_speed * X_STEPS_PER_MM); + } + if (y_delta) { + y_c = (F_CPU * 256UL / int_sqrt(y_accel * Y_STEPS_PER_MM)) * 313UL / 7UL; +// y_c = F_CPU * sqrt(Y_UM_PER_STEP / y_accel) * 1.414; + y_minc = (F_CPU * 256UL) / (y_speed * Y_STEPS_PER_MM); + } + if (z_delta) { + z_c = (F_CPU * 256UL / int_sqrt(z_accel * Z_STEPS_PER_MM)) * 313UL / 7UL; + z_minc = (F_CPU * 256UL) / (z_speed * Z_STEPS_PER_MM); + } + if (e_delta) { + e_c = (F_CPU * 256UL / int_sqrt(e_accel * E_STEPS_PER_MM)) * 313UL / 7UL; + e_minc = (F_CPU * 256UL) / (e_speed * E_STEPS_PER_MM); + } + + printf("Xc: %d, Yc: %d\n", x_c >> 8, y_c >> 8); + printf("Xminc: %d, Yminc: %d\n", x_minc >> 8, y_minc >> 8); + + x_n = y_n = z_n = e_n = 1; + + x_cr = x_c >> 8; + y_cr = y_c >> 8; + z_cr = z_c >> 8; + e_cr = e_c >> 8; + + total_ticks = 0; + + while (x_delta > 0 || y_delta > 0 || z_delta > 0 || e_delta > 0) { + if (x_cr <= 0 && x_delta > 0) { + x_delta--; +// printf("x_c(%d) = %u", x_n, x_c >> 8); + if (x_n == 1) + x_c = x_c * 0.4056; + else + x_c = x_c - ((2 * x_c) / ((4 * x_n) + 1)); +// printf(" -> %u\n", x_c >> 8); + if (x_c < x_minc) + x_c = x_minc; + x_cr = x_c >> 8; + x_n++; + } + if (y_cr <= 0 && y_delta > 0) { + y_delta--; + if (y_n == 1) + y_c = y_c * 0.4056; + else + y_c = y_c - ((2 * y_c) / ((4 * y_n) + 1)); + if (y_c < y_minc) + y_c = y_minc; + y_cr = y_c >> 8; + y_n++; + } + if (z_cr <= 0 && z_delta > 0) { + z_delta--; + if (z_n == 1) + z_c = z_c * 0.4056; + else + z_c = z_c - ((2 * z_c) / ((4 * z_n) + 1)); + if (z_c < z_minc) + z_c = z_minc; + z_cr = z_c >> 8; + z_n++; + } + if (e_cr <= 0 && e_delta > 0) { + e_delta--; + if (e_n == 1) + e_c = e_c * 0.4056; + else + e_c = e_c - ((2 * e_c) / ((4 * e_n) + 1)); + if (e_c < e_minc) + e_c = e_minc; + e_cr = e_c >> 8; + e_n++; + } + +// printf("[xc: %d, xd: %d, yc: %d, yd: %d, ", x_cr, x_delta, y_cr, y_delta); + fprintf(stderr, "%u %.3f %.3f %u(%u) %u %u(%u) %u ", total_ticks, x_delta * X_UM_PER_STEP, y_delta * Y_UM_PER_STEP, x_c, x_c >> 8, x_n, y_c, y_c >> 8, y_n); + + elapsed_ticks = 0x7FFFFFFF; + if ((x_delta > 0) && (x_cr < elapsed_ticks)) + elapsed_ticks = x_cr; + if ((y_delta > 0) && (y_cr < elapsed_ticks)) + elapsed_ticks = y_cr; + if ((z_delta > 0) && (z_cr < elapsed_ticks)) + elapsed_ticks = z_cr; + if ((e_delta > 0) && (e_cr < elapsed_ticks)) + elapsed_ticks = e_cr; + + fprintf(stderr, "+%u", elapsed_ticks); + // printf("e: %u]\n", elapsed_ticks); + + x_cr -= elapsed_ticks; + y_cr -= elapsed_ticks; + z_cr -= elapsed_ticks; + e_cr -= elapsed_ticks; + + total_ticks += elapsed_ticks; + + fprintf(stderr, "\n"); + } +} + +int main(int argc, char **argv) { + float x = 40, + y = 34, + z = 0, + e = 0, + f = 1500; + + move(x * X_STEPS_PER_MM, y * Y_STEPS_PER_MM, z * Z_STEPS_PER_MM, e * E_STEPS_PER_MM, f); + + return 0; +} diff --git a/research/alg.make b/research/alg.make new file mode 100755 index 0000000..c81a113 --- /dev/null +++ b/research/alg.make @@ -0,0 +1,4 @@ +#!/bin/bash + +perl alg.pl 2>alg.data && gnuplot alg.plot && display alg.png + diff --git a/research/alg.pl b/research/alg.pl new file mode 100755 index 0000000..02c5de4 --- /dev/null +++ b/research/alg.pl @@ -0,0 +1,218 @@ +#!/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"; + } +} diff --git a/research/alg.plot b/research/alg.plot new file mode 100644 index 0000000..db8f576 --- /dev/null +++ b/research/alg.plot @@ -0,0 +1,15 @@ +# set terminal x11 persist raise +set terminal png size 1024,768 +set output "alg.png" + +set title "Move from [40,34] to [0,0] with acceleration [9,5] and deceleration [3,8]\n\ +showing geometric correctness as a result of acceleration and deceleration trimming" + +set xlabel "seconds" +set x2label "millimeters" +set ylabel "millimeters" + +plot "alg.data" using ($1 / 16000000):($2 / 1000) with lines title "X vs time", \ + "alg.data" using ($1 / 16000000):($3 / 1000) with lines title "Y vs time", \ + "alg.data" using ($2 / 1000):($3 / 1000) with lines axes x2y1 title "X vs Y", \ + (x * 34 / 40) with points axes x2y1 title "Ideal"