25 Mar 09:01
machctl: r18 - sys
Author: vedge Date: 2009-03-25 05:01:55 -0300 (Wed, 25 Mar 2009) New Revision: 18 Modified: sys/cnc_quintic.c sys/cnc_quintic.h Log: - fix typo in case evaluation - small optimizations in cnc_quintic_init() Modified: sys/cnc_quintic.c =================================================================== --- sys/cnc_quintic.c 2009-02-09 14:05:57 UTC (rev 17) +++ sys/cnc_quintic.c 2009-03-25 08:01:55 UTC (rev 18) @@ -1,6 +1,5 @@ -/* $OpenBSD$ */ /* - * Copyright (c) 2007 Hypertriton, Inc. <http://www.hypertriton.com/> + * Copyright (c) 2007-2009 Hypertriton, Inc. <http://www.hypertriton.com/> * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -50,94 +49,96 @@ * velocity profile under acceleration and jerk constraints. */ int -cnc_quintic_init(struct cnc_quintic_profile *q, const struct cnc_insn *insn, - cnc_real_t L) +cnc_quintic_init(struct cnc_quintic_profile *q, cnc_real_t L, cnc_real_t v0, + cnc_real_t F, cnc_real_t Amax, cnc_real_t Jmax) { - cnc_real_t F = ((cnc_real_t)insn->i_F); - cnc_real_t Amax = ((cnc_real_t)insn->i_Amax); - cnc_real_t Jmax = ((cnc_real_t)insn->i_Jmax); - cnc_real_t k; + cnc_real_t AmaxP2 = Amax*Amax; + cnc_real_t AmaxP3 = AmaxP2*Amax; + cnc_real_t AmaxP4 = AmaxP3*Amax; + cnc_real_t JmaxP2 = Jmax*Jmax; + cnc_real_t F_P2 = F*F; + cnc_real_t F_P3 = F_P2*F; + cnc_real_t x, y; q->F = F; -// if (F >= CNC_PI_2*((Amax*Amax)/Jmax) ) { + if (F >= CNC_PI_2*(AmaxP2/Jmax) ) { /* * Feedrate F is achievable by using maximum acceleration * and maximum jerk. */ - if (L >= ( (L*L)/Amax + CNC_PI_2*((Amax*F)/Jmax) )){ - /* Feedrate F is achievable at Amax acceleration. */ - printf("case 1.1\n"); + x = F_P2/Amax + CNC_PI_2*((Amax*F)/Jmax); + y = (CNC_PI_P2/2.0)*(AmaxP3/JmaxP2); + if (L >= x) { + /* + * Feedrate F is achievable at Amax acceleration. + */ + printf("cnc: F achievable at Amax (#1.1)\n"); q->Ts = CNC_PI_2*(Amax/Jmax); - q->Ta = (F/Amax) + q->Ts; + q->Ta = q->Ts + (F/Amax); q->To = L/F; - } else if (L < ( (F*F)/Amax + CNC_PI_2*((Amax*F)/Jmax) ) && - L >= ( (CNC_PI*CNC_PI)/2.0) * (Amax*Amax*Amax) / - (Jmax*Jmax) ) { - /* Feedrate F is unachievable, Amax is achievable. */ - printf("case 1.2\n"); + } else if (L < x && L >= y) { + /* + * Feedrate F is unachievable, Amax is achievable. + */ + printf("cnc: F is unachievable, but Amax is (#1.2)\n"); q->Ts = CNC_PI_2*(Amax/Jmax); - q->Ta = ( (-CNC_PI*(Amax*Amax) + - cnc_sqrt((CNC_PI*CNC_PI)*(Amax*Amax*Amax*Amax) + - 16.0*Amax*(Jmax*Jmax)*L) ) / - (4.0*Jmax*Amax) ) + q->Ts; + q->Ta = ( (-CNC_PI*AmaxP2 + cnc_sqrt(CNC_PI_P2*AmaxP4 + 16.0*Amax*JmaxP2*L) ) / + (4.0*Jmax*Amax) ) + q->Ts; q->To = q->Ta; - } else if (L <= ( ((CNC_PI*CNC_PI)/2.0)*(Amax*Amax*Amax) / - (Jmax*Jmax) )) { - /* Neither F nor Amax are achievable. */ - printf("case 1.3\n"); + } else if (L <= y) { + /* + * Neither F nor Amax are achievable. + */ + printf("cnc: neither F nor Amax are achievable (#1.3)\n"); q->Ts = cnc_cbrt( (CNC_PI*L)/(4.0*Jmax) ); q->Ta = 2.0*q->Ts; q->To = q->Ta; -// } else { -// printf("MOVE: Illegal parameters (1)\n"); -// return (-1); } -// } else if (F < CNC_PI_2*((Amax*Amax)/Jmax) ) { + } else if (F < CNC_PI_2*(AmaxP2/Jmax) ) { + x = cnc_sqrt( (2.0*CNC_PI*F_P3)/Jmax ); /* * Feedrate F is achievable without using maximum * acceleration. */ - if (L >= cnc_sqrt( (2.0*CNC_PI*(F*F*F))/Jmax ) ) { - /* F is achievable, Amax is unachievable. */ - printf("case 2.1\n"); + if (L >= x) { + /* + * F is achievable, Amax is unachievable. + */ + printf("cnc: F is achievable, but Amax is not (#2.1)\n"); q->Ts = cnc_sqrt( (CNC_PI*F)/(2.0*Jmax) ); q->Ta = 2.0*q->Ts; q->To = L/F; - } else if (L < cnc_sqrt( (2.0*CNC_PI*F*F*F)/Jmax )) { - /* Neither F nor Amax are achievable. */ - printf("case 2.2\n"); + } else if (L < x) { + /* + * Neither F nor Amax are achievable. + */ + printf("cnc: neither F nor Amax are achievable (#2.2)\n"); q->Ts = cnc_cbrt( (CNC_PI*L)/(4.0*Jmax) ); q->Ta = 2.0*q->Ts; q->To = q->Ta; } else { - printf("MOVE: Illegal parameters (2)\n"); + printf("cnc: Illegal parameters to CNC_MOVE\n"); return (-1); } -// } + } - k = CNC_PI/(2.0*q->Ts); - q->Aref = L/(q->Ta - q->Ts) * q->To; - q->v0 = ((cnc_real_t)insn->i_v0); - q->v1 = (q->Aref/2.0) * (q->Ts - cnc_sin(2.0*k*q->Ts)/(2.0*k) ); + x = CNC_PI/(2.0*q->Ts); + q->Aref = L/((q->Ta - q->Ts)*q->To); + q->v0 = v0; + q->v1 = (q->Aref/2.0)*(q->Ts - cnc_sin(2.0*x*q->Ts)/(2.0*x) ); q->v2 = q->Aref*(q->Ta - 2.0*q->Ts) + q->v1; - - printf("MOVE: "); - printf("F=%s ", cnc_fmt_real(F)); - printf("v1=%s ", cnc_fmt_real(q->v1)); - printf("v2=%s ", cnc_fmt_real(q->v2)); - printf("v3=%s ", cnc_fmt_real(q->v1+q->v2)); - printf("Amax=%s ", cnc_fmt_real(Amax)); - printf("Jmax=%s ", cnc_fmt_real(Jmax)); - printf("Ta=%s ", cnc_fmt_real(q->Ta)); - printf("Ts=%s ", cnc_fmt_real(q->Ts)); - printf("To=%s\n", cnc_fmt_real(q->To)); return (0); } +/* + * Evaluate the velocity at t under the specified quintic velocity profile. + * Returns computed velocity in steps/second. + */ cnc_real_t cnc_quintic_step(const struct cnc_quintic_profile *q, cnc_real_t t) { + cnc_real_t v = q->v0; cnc_real_t t2, t5, t6; cnc_real_t k = CNC_PI/(2.0*q->Ts); @@ -146,22 +147,26 @@ t6 = q->To + q->Ta - q->Ts; if (t <= q->Ts) { - return (q->Aref/2.0)*( t - cnc_sin(2.0*k*t)/(2.0*k) ); + v += (q->Aref/2.0)*( t - cnc_sin(2.0*k*t)/(2.0*k) ); } else if (t <= (q->Ta - q->Ts)) { - return q->Aref*(t - q->Ts) + q->v1; + v += q->Aref*(t - q->Ts) + q->v1; } else if (t <= q->Ta) { - return (q->Aref/2.0)*( (t-t2) + cnc_sin(2.0*k*(t-t2))/(2.0*k) ) - + q->v2; + v += (q->Aref/2.0)*( (t-t2) + cnc_sin(2.0*k*(t-t2))/(2.0*k) ) + + q->v2; } else if (t <= q->To) { - return (q->v1 + q->v2); + v += (q->v1 + q->v2); } else if (t <= t5) { - return -(q->Aref/2.0) * - ( (t - q->To) - cnc_sin(2.0*k*(t - q->To))/(2.0*k) ) - + (q->v1 + q->v2); + v += (q->v1 + q->v2); + v += -(q->Aref/2.0)*( (t - q->To) - + cnc_sin(2.0*k*(t - q->To))/(2.0*k) ); } else if (t <= t6) { - return -q-≥Aref*(t-t5) + q->v2; + v += -q-≥Aref*(t-t5) + q->v2; } else { - return -(q->Aref/2.0)*( (t-t6) + cnc_sin(2.0*k*(t-t6))/(2.0*k) ) - + q->v1; + v += -(q->Aref/2.0)*( (t-t6) + cnc_sin(2.0*k*(t-t6))/(2.0*k) ) + + q->v1; } + if (v < 10.0) { + v = 10.0; + } + return (v); } Modified: sys/cnc_quintic.h =================================================================== --- sys/cnc_quintic.h 2009-02-09 14:05:57 UTC (rev 17) +++ sys/cnc_quintic.h 2009-03-25 08:01:55 UTC (rev 18) @@ -11,6 +11,6 @@ cnc_real_t v2; /* Velocity at Ta-Ts */ }; -int cnc_quintic_init(struct cnc_quintic_profile *, const struct cnc_insn *, - cnc_real_t); +int cnc_quintic_init(struct cnc_quintic_profile *, cnc_real_t, + cnc_real_t, cnc_real_t, cnc_real_t, cnc_real_t); cnc_real_t cnc_quintic_step(const struct cnc_quintic_profile *, cnc_real_t);
RSS Feed