Also: Embedded CNC for other uC
This code converts a line in 3 dimensions into the pulse and direction signals necessary to make a 3 axis stepper motor driven CNC machine follow that line while maintaining predefined maximum acceleration and maximum velocity parameters.
/*
3D Line to Stepper Axis Pulses
by JamesNewton@MassMind.org (please do email comments, corrections, bug reports, etc...)
Licensed under the MIT license: http://www.opensource.org/licenses/mit-license.php
Purpose: Convert a line in 3 dimensions into the pulse and direction signals necessary to make a 3
axis stepper motor driven CNC machine follow that line while maintaining predefined maximum
acceleration and maximum velocity parameters.
Limitations: Must fit in a small microcontroller. e.g. PIC 16F690 with 4K of code space and 256
bytes of RAM.
DONE:
- steps and directions on port pins... Motors turning!
- figure out how to adapt Bresenham to 3d... Appears to work
- code up some velocity and acceleration control.. Could be done
- figure out why mv and ma are not being returned to their original values. Is sd not being
calculated correctly? yeah, sd needed to just be incremented and lots of other tweaks...
TODO:
- acceleration ramping is NOT correct. Must incorporate the fact that a given change in the delay
between steps has more effect when the delay is small, as compared to when it is large.
E.g. motor spinning at 500uS between pulses, is reduced by 250uS, that doubles the motor speed.
But when /increased/ by 250uS, that does NOT halve the motor speed. Formula may be:
starting velocity * (1/sqrt(x))
where x is the step... or the time interval... not clear yet.
- simulate, debug, test, etc... with xaxis=bigaxis
- looks like sd is always just half of i?
- change each axis variables into a structure so they can easily passed as parameters
- handle y and z being the big axis
- decide how the starting and goal positions are going to get set. Minimal G code interpreter?
- decide how the max velocity and acceleration settings are getting set. G code?
- clean up order of declarations, subroutines, move things into include files and all that other
crap C programmers are expected to do for some strange reason.
LINKS:
http://forums.reprap.org/read.php?12,9459 Arduino G code to stepper. See user.cpp file.
Compiler notes: Using >>1 throughout in place of /2 because some compilers are seriously too stupid
to optimize a divide by two in to a right shift.
*/
// OPTIONS
#define DEBUG
/* When defined, printf's the step,stopping distance, x,y,z and velocity and acceleration
to the console */
#define ACCELERATION
/* When defined, the code will attempt to accelerate from STARTING_VELOCITY to MAXIMUM_VELOCITY*/
#define RAMP_ACCELERATION
/* When defined, the code attempt to smooth the change in acceleration and so the G-Force
experienced by the load being moved through the lines path will build and fall over some time.
When not defined, the acceleration is a fixed value so the G-Force at the load will jump from
0 to a fixed value as soon as
motion begins. */
#define STARTING_VELOCITY 800
//mS between steps
#define MAXIMUM_VELOCITY 20
//ms between steps
#define MAXIMUM_ACCELERATION 20
//maximum acceleration as change in uSecond delay time between steps.
#define MAXIMUM_DELTA_ACCELERATION 10
//maximum change in acceleration. Must be less than MAXIMUM_ACCELERATION
#include <stdlib.h>
#ifdef DEBUG
#include <stdio.h>
#endif
#define STEPX RA1
#define STEPY RA2
#define STEPZ RA4
#define STEPA RA4
#define DIR RA5
typedef enum {cw=0,ccw=1} tdirection; // so clock wise is false
typedef enum {xaxis=1,yaxis=2,zaxis=4,aaxis=8} taxis;
#define tcoordinate int
/* tcoordinate must be a type which can hold the range of positions through which the system
will travel when expressed as individual steps. */
//Current position.
// Must be maintained from one movement to the next. step() does this job
static tcoordinate xp; //current position in the X axis.
static tcoordinate yp; //current position in the Y axis.
static tcoordinate zp; //current position in the Z axis.
//current direction for each axis, assume forward(true)
tdirection xd=cw; //X direction
tdirection yd=cw; //Y direction
tdirection zd=cw; //Z direction
void step(taxis anaxis, tdirection adir, tcoordinate *apos) {
// DIR = 0;
STEPX = STEPY = STEPZ = STEPA = 0;
__delay_ms(8); //otherwise you can't see the pulse.
DIR = adir;
/* if (cw==adir) {
(*apos)++;
}
else {
(*apos)--;
}
*/ (cw == adir) ? ((*apos)++) : ((*apos)--);
if (anaxis & aaxis) {STEPA = 1;}
if (anaxis & zaxis) {STEPZ = 1;}
if (anaxis & yaxis) {STEPY = 1;}
if (anaxis & xaxis) {STEPX = 1;}
__delay_ms(8); //otherwise you can't see the pulse.
STEPX = 0; //X LED is 2 GND, so turn it back off now
}
//goal position
tcoordinate xg; //X axis goal position
tcoordinate yg; //Y axis goal position
tcoordinate zg; //Z axis goal position
/* Limits. We are assuming that all axis have the same max values, which is unlikely, but
simplifies the code, and can be functional, as long as we limit all axis to the weakest
axis. */
#define sv STARTING_VELOCITY
//starting velocity as uSecond delay time between steps.
short mv = MAXIMUM_VELOCITY;
//maximum velocity as uSecond delay time between steps.
#ifdef RAMP_ACCELERATION
short ma = MAXIMUM_DELTA_ACCELERATION;
//maximum acceleration
#define mda MAXIMUM_DELTA_ACCELERATION
//maximum change in acceleration
#else
#define ma MAXIMUM_ACCELERATION
//maximum acceleration as change in uSecond delay time between steps.
#endif
//velocity of each axis, expressed in timer ticks between steps.
unsigned short xv=sv; //velocity of the X axis
unsigned short yv=sv; //velocity of the Y axis
unsigned short zv=sv; //velocity of the Z axis
void posgoal2stepdir(tcoordinate *p, tcoordinate *g, tcoordinate *s, tdirection *d) {
/*translate our current position, and the goal position, into the number of steps required to
reach that goal and the direction to travel. */
signed tcoordinate i;
i = *g - *p ; //how far do we have to move?
*d = cw;
if (i<0) { //is the goal less than the current position?
*d = ccw; //set the direction to reverse(false)
i = (i>0?i:-i); //change to a count
};
*s = (tcoordinate)i; //convert to a distance in the coordinate plain.
}
void main() {
/* stopping distance. This keeps track of how long we expect to take to get stopped once we get
going. */
tcoordinate sd = 0; //stopping distance
//number of steps required
tcoordinate xs; //X axis
tcoordinate ys; //Y axis
tcoordinate zs; //Z axis
init();
//some test values
// xg=10;yg=5;zg=2;
// xp=20;xg=10;yg=5;zg=2; //
xp=20;xg=100;yp=5;yg=50;zg=2; //
//figure out distance and direction for each axis
posgoal2stepdir(&xp,&xg,&xs,&xd);
posgoal2stepdir(&yp,&yg,&ys,&yd);
posgoal2stepdir(&zp,&zg,&zs,&zd);
//find which axis has the largest move
taxis bigstep = xaxis;
if (ys>xs) bigstep = yaxis;
if (xs>xs) bigstep = zaxis;
/* Now we know which axis needs to travel the farthest. So that one should be
the one we step most often. The other two axis get stepped when the error in
their position exceeds one step as per the Bresenham algorithm.
http://en.wikipedia.org/wiki/Bresenham's_line_algorithm
The other axis can not possibly exceed the velocity and acceleration of the
“big” axis and should be a scaled version of that maximum.
*/
if (xaxis == bigstep) {
//Bresenham error
signed tcoordinate ye; //Y axis
signed tcoordinate ze; //Z axis
ye = ze = (xs >> 1); //error for each smaller distance starts as half the largest distance
#ifdef DEBUG
puts("step:stop\tx\ty\tz\tvel\tacc\r");
#endif
for ( tcoordinate i = xs; i > 0; i--) { //i counts down steps to the end of this movement
step(xaxis, xd, &xp); // we always move the big axis and step does dir and pos
//pretty much pure Bresenham here except step() manages direction so we don’t have to.
ye -= ys;
ze -= zs;
if (ye < 0) {
step(yaxis, yd, &yp);
ye += xs;
};
if (ze < 0) {
step(zaxis, zd, &zp);
ze += xs;
};
#ifdef ACCELERATION
//figure out how long to wait for the next step by considering position, velocity, and acceleration
if ( i > sd ) { //unless we are nearing the end...
if ( xv > mv ) { // unless we are at max velocity
// Note: _greater_ because smaller values mean less delay and so faster
xv -= ma; // reduce delay to increase speed by max acceleration.
#ifdef RAMP_ACCELERATION
if (xv >= (mv+((sv-mv)>>1))) { // greater because smaller is faster
ma += mda; //increase acceleration,
} // we are not yet half way to max velocity
else { // xv is now less than twice mv
ma -= mda; //decrease acceleration,
} // we are reaching max velocity
#endif
sd++; // more speed means more distance needed to stop
}
}
else { //we are nearing the end
xv += ma; // increase delay to reduce speed by max (de)acceleration.
#ifdef RAMP_ACCELERATION
if (xv <= (sv-((sv-mv)>>1))) { // less because smaller is faster
ma += mda; //decrease deceleration,
} // we are coming to a halt.
else { // xv is now less than twice mv
ma -= mda; //increase deceleration,
} // we are still nearer max velocity
#endif
} // end if ( i > sd )
#endif //ACCELERATION
#ifdef DEBUG
printf("%04d:%04d\t%d\t%d\t%d\t%d\t%d\r",i,sd,xp,yp,zp,xv,ma);
#endif
delay(xv); //pause to give the motors time to get to the new step
} // end for
} // end if (xaxis=bigaxis)
//And all that again for the case of Y being the bigaxis or Z...
if (yaxis == bigstep) { };
//...
if (zaxis == bigstep) { };
//...
//Or we could make a subroutine...
}
Comments: