PIC Microcontroller based Computer Numerical Control

3D Line to Stepper Axis Pulses in a PIC16F

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...

	}