Programming linear motor interpolation

Hi!

Im programming an arduino CNC firmware for my DIY CNC
machine and everything works fine for now. I can
send commands via the serial interface and controll the
machine as I like.

I tried some shapes with a pen and observed the problem
of the lack of “linear interpolation”. I attached a picture below.

  • The coordinates (2,2) go in a nice straight line as it
    should (turquoise line).
  • The coordinates (4,2) should also go in a straight
    line (orange line). But what the machine actually does is
    that one axis is faster at its position than the other so
    I get the really bad red line.

Should the motors run at different speeds to achieve a
straight line here? If thats true how to calculate the speed
of each motor?

Im not really sure where to start the mathematics here.
Can someone give me a hint?!

(sry if this is the wrong forum)

Thanks in advance!
Regards!

linearInterpolation.jpg

I haven't made a CNC machine, but it sounds like you might need to tweak the speed settings by trial and error, to get the line you want. Or, install a rotary encoder to get the feedback so you know how many turns each axis has done.

Unless you have really high resolution in your speed setting you will never get perfect angled lines.

Another way to do it , is calculate the line length, (sqrt (x*y + Y*Y) pythagorian theorem) and then calculate you move based on the line length and then using a bit of trigonometry calulate the X & Y position and everytime either axis needs to move more than a step (they will be moving fractional steps) move the axis and update your position counter and repeat...

Hope that makes sense =

Thank you guys for reply!

@Nick:
Since I use stepper motors I always keep track of current
position of each motor. So the problem is more the relationship
of each position in space mathematicly.

@Kf2qd:
Hm okay I will look a this. What do you mean by “they will be moving
fractional steps”?

Maybe I found a good explanation and actual code for my
problem:
http://sol.gfxile.net/interpolation/index.html

Looks like this is what I need:

for (i = 0; i < N; i++)
{
  v = i / N;
  X = (A * v) + (B * (1 - v));
}

I have to investigate further! :slight_smile:

Thanks again!

Ok I tried a bit further.

But I got really stuck. Im able to linear interpolate between 2 coordinates
but I cant translate that to my motor motion.

What I do is: I write the newPos to the axis instance and before that
I write the last newPos as the oldPos. I init both of them with 0.

If the command is recieved (Serial string eg. “2;0;1#” in millimeters,
each axis gets the newPos via atof(str), no problem here newPos and
oldPos are written) the main code executes.

X-Axis:
The x-axis checks every loop if the currPos (set after one step is complete)
is equal to the newPos, if not: the motor steps (direction is set in motorStep).
Y-Axis:
The y-axis has an additional parameter: nextPos. NextPos is for the
interpolation and is only set if the X new and old arent the same (because
that time there is no need for interpolation).

The problem is now, that

  1. X is somtimes (!) again faster than Y… :frowning:
  2. Sometimes I cant set coordinates back to 0, nothing happens
  3. I get a lot vibration and a loss in speed when I draw not
    vertical or horizontal lines (-> interpolation error??)

Here is the main code (Sry the code is incomplete because
the post would be to big but I hope you understand):

if(!readyForNextCmd){
    
      if(y.getCurrPos() == y.getNextPos()){
         x2 = x.getCurrPos();
         y2 = linearInterpolate(x.getOldPos(),y.getOldPos(),x.getNewPos(),y.getNewPos(),x2); 
       }
       
       if(x.getNewPos() == x.getOldPos())
       {
         y.setNextPos(y.getNewPos()); 
       } else {
         y.setNextPos(y2);
       }
       
       if(x.getCurrPos() != x.getNewPos()){
         x.motorStep();
       }
       if(y.getCurrPos() != y.getNextPos()){
         y.motorStep();
       }
       
       if(x.getCurrPos() == x.getNewPos() && y.getCurrPos() == y.getNewPos())
       {
         readyForNextCmd = true;
       }
}

long linearInterpolate(long x1, long y1, long x2, long y2, long X)
{
  long _Y = 0;
  _Y = y1 + ((X-x1)*y2 - (X-x1)*y1)/(x2-x1);
  return _Y;
}

Here is the step-code:

void AxisCtrl::motorStep()
{
	unsigned long currentMicros = micros();
	if(currentMicros - _previousCycle > _motorSpeed) {
		if(_stepDone == 0){
			setMotorDir();
			incrementCurrStep();
		}
		_previousCycle = currentMicros;
		if(_motorPinState == 0){
			_motorPinState = 1;
		} else {
			_motorPinState = 0;
		}
		digitalWrite(_motorPin,_motorPinState);
		++_stepDone;
		if(_stepDone == 2){
			_stepDone = 0;
			if(_currPos > 0){
				_axisHomed = false;
			}
		}
	}

}

Thanks in advance!

I realy dont want to spam, bump or annoy anyone with this thread
but this behaviour has to be an programming / buffer overflow (long value?) error
that I miss.

Eg. the coordinates A(50,10) are drawn as they should and interpolated
correct. All cubic diagonal lines are correct too.
The coordinates B(50,20) are correct in the beginning but breaking
at a certain point → interpolation function returns suddenly negative
numbers (please see attachments). See also in the big drawings from
the machine.

I made also a video for the problem (please watch in 720p and fullscreen to
read the screenshot at the end):

Why does the interpolation equation suddenly returns at a certain point
a negative number? It happens always on the same number. A long value
can handle these big numbers, rigth?? I also tried abs() but even abs() does
not prevent from getting negative numbers from the function.

Im hammering my head on the table… : :~ =(

I would be realy realy gratefull if somebody could help me =(

Here is my full code:

Main.cpp

#include "AxisCtrl.h"
#include <string.h>

AxisCtrl x(12,9,7,1600,0.8,14);
AxisCtrl y(11,9,6,1600,0.8,3);
AxisCtrl z(10,9,8,1600,0.8,14);

boolean homeAll = false;
int ledPin = 13;
long x2 = 0;
long y2 = 0;
long z2 = 0;
long nextStep = 0;

String cmdString;
boolean serialHandshake = false;
boolean cmdComplete;
boolean readyForNextCmd = true;
boolean otherCmdRecieved = false;
float oDist;
float interpolationVal = 0.5;


void setup() {
  //Serial conncetion
  cmdString.reserve(200);  
  Serial.begin(115200);
  Serial.println("Init machine...#");
  x.begin();
  y.begin();
  z.begin();
  Serial.println("Machine ready.#");
  pinMode(ledPin,OUTPUT);
  //Establish contact
  Serial.write("R");
}


void loop() {
  
  if(readyForNextCmd){
    if(serialHandshake){
      Serial.write("R");  
      serialHandshake = false;
    }
  }
  
  if (cmdComplete) {
    cmdStringHandler();
  }
  
  if(!readyForNextCmd){
      
         x2 = x.getCurrPos();
         y2 = linearInterpolate(x.getOldPos(),y.getOldPos(),x.getNewPos(),y.getNewPos(),x2); 
       
       if(x.getNewPos() == x.getOldPos())
       {
         y.setNextPos(y.getNewPos()); 
       } else {
         y.setNextPos(y2);
       }
       
       if(y.newPos()>x.newPos(){
         //Fehler wenn x kleiner als y: es werden keine y2 mehr 
         //berechnet, wenn x bereits erreicht
       }
       
       if(x.getCurrPos() != x.getNewPos()){
         x.motorStep();
       }
       if(y.getCurrPos() != y.getNextPos()){
         y.motorStep();
       }
       
       if(x.getCurrPos() == x.getNewPos() && y.getCurrPos() == y.getNewPos())
       {
         readyForNextCmd = true;
       }
       
       Serial.print("X:");
       Serial.print(x.getCurrPos());
       Serial.print(" Y:");
       Serial.print(y.getCurrPos());
       Serial.print(" Y2:");
       Serial.println(y2);
       
    }
}

void cmdStringHandler(){
      otherCmdRecieved = false;
      char cmdBuff[30];
      cmdString.toCharArray(cmdBuff,30);

      if(!otherCmdRecieved){
        char *p = cmdBuff;
        char *str;
        int theStrPos = 0;
        while ((str = strtok_r(p, ";", &p)) != NULL)
        {
          if(theStrPos == 0){
           x.setNewPos(atof(str));
          } 
          if(theStrPos == 1){
           y.setNewPos(atof(str));
          } 
          if(theStrPos == 2){
           z.setNewPos(atof(str));
          } 
          ++theStrPos;
        }
      }

      cmdString = "";
      cmdComplete = false;
      serialHandshake = true;
}

void serialEvent() {
  if(readyForNextCmd){
    while (Serial.available()) {
      char inChar = (char)Serial.read();
      if (inChar == '#') {
        readyForNextCmd = false;
        cmdComplete = true;
      }
      if(cmdComplete == false){
        cmdString += inChar;
      }
    }
  }
}

long linearInterpolate(long x1, long y1, long x2, long y2, long X)
{
  long _Y = 0;
  _Y = y1 + ((X-x1)*y2 - (X-x1)*y1)/(x2-x1);
  return _Y;
}

Classes

#include "Arduino.h"
#include "AxisCtrl.h"

AxisCtrl::AxisCtrl(int motorPin, int dirPin, int limitSwitchPin, int steps, float leadPitch, float maxAxisLength)
{
	//Init values ///////////////////////////////////
	_maxAxisLength = maxAxisLength;
	_limitSwitchPin = limitSwitchPin;
	_motorPin = motorPin;
	_dirPin = dirPin;
	_leadPitch = leadPitch;
	_stepPerRev = steps;
	_normLenghtPerStep = _leadPitch / _stepPerRev;
	// Init defaults ////////////////////////////////
	step = false;
        interpolate = true;
	overrideUp = false;
	overrideDown = false;
	_axisHomed = false;
	_motorSpeed = 80; //80 good value befor strange motor behavior
	_currStep = 0;
	_currStepPos = 0;
	_previousCycle = 0;
	_motorPinState = 0;
	_stepDone = 0;
        _oldPos = 0;
        _newPos = 0;
        _nextPos = 0;
}

void AxisCtrl::begin()
{
	pinMode(_motorPin, OUTPUT);
	pinMode(_dirPin, OUTPUT);
	pinMode(_limitSwitchPin,INPUT);
	
	digitalWrite(_dirPin, LOW);
	digitalWrite(_motorPin, LOW);
}

void AxisCtrl::incrementCurrStep()
{
	++_currStep;
	if(_motorDir == true){
		++_currPos;
	} else {
		--_currPos;
	}
}

void AxisCtrl::home()
{
	motorOverride2(0);
	if(limitSwitchState()){
		_axisHomed = true;
		resetPositions();
	} 
}

void AxisCtrl::motorStep()
{
	unsigned long currentMicros = micros();
	if(currentMicros - _previousCycle > _motorSpeed) {
		if(_stepDone == 0){
			setMotorDir();
			incrementCurrStep();
		}
		_previousCycle = currentMicros;
		if(_motorPinState == 0){
			_motorPinState = 1;
		} else {
			_motorPinState = 0;
		}
		digitalWrite(_motorPin,_motorPinState);
		++_stepDone;
		if(_stepDone == 2){
			_stepDone = 0;
			if(_currPos > 0){
				_axisHomed = false;
			}
		}
	}

}

void AxisCtrl::motorOverride2(int dir)
{
	unsigned long currentMicros = micros();
	if(currentMicros - _previousCycle > _motorSpeed) {
		_previousCycle = currentMicros;
		if(_motorPinState == 0){
			_motorPinState = 1;
		} else {
			_motorPinState = 0;
		}
		digitalWrite(_dirPin, dir);
		digitalWrite(_motorPin,_motorPinState);
	}
}

int AxisCtrl::limitSwitchState()
{
	boolean state;
	int val = digitalRead(_limitSwitchPin);
	if(val){
		state = true;
	} else {
		state = false;
	}
	return state;
}

void AxisCtrl::motorOverride(long revs, int dir)
{
	digitalWrite(_dirPin, dir);
	for(int i=0;i<revs;i++){
		digitalWrite(_motorPin, HIGH);
		delayMicroseconds(_motorSpeed);
		digitalWrite(_motorPin, LOW); 
		delayMicroseconds(_motorSpeed);
	}
}

void AxisCtrl::checkOverrideState(int dir){
	if(dir == 1){
		if(overrideUp){
		  overrideUp = false;
		}else{
		  overrideUp = true;
		}	
	} else {
		 if(overrideDown){
			overrideDown = false;
		}else{
			overrideDown = true;
		}
	}
}

void AxisCtrl::resetPositions(){
	_currStepPos = 0;
	_currPos = 0;
	_cmdSteps = 0;
	_currStep = 0;
	_cmdPos = 0;
}


void AxisCtrl::setMotorDir()
{
	if(_currPos<_newPos){
		digitalWrite(_dirPin, HIGH);
		_motorDir = true;
	}else{
		digitalWrite(_dirPin, LOW);
		_motorDir = false;
	}	
}

void AxisCtrl::setCmdPos(float pos)
{
	_cmdPos = pos;
	_cmdSteps = abs((_cmdPos / _normLenghtPerStep)-_currStepPos);
	_currStep = 0;
	_motorDir = true;
}

void AxisCtrl::setCurrStepPos(long pos)
{
	_currStepPos = pos;
}

long AxisCtrl::getCurrStepPos()
{
	return _currStepPos;
}

float AxisCtrl::getCmdPos()
{
	return _cmdPos;
}

void AxisCtrl::setCmdSteps(long steps)
{
	_cmdSteps = steps;
}

long AxisCtrl::getCmdSteps()
{
	return _cmdSteps;
}

void AxisCtrl::togglePin(int val)
{
	digitalWrite(_motorPin, val);
}

void AxisCtrl::setHomed(boolean val)
{
	_axisHomed = val;
}

boolean AxisCtrl::getHomed()
{
	return _axisHomed;
}

int AxisCtrl::getMotorPin()
{
	return _motorPin;
}

int AxisCtrl::getMotorSpeed()
{
	return _motorSpeed;
}

void AxisCtrl::setMotorSpeed(int speed)
{
	_motorSpeed = speed;
}

void AxisCtrl::setCurrStep(long step)
{
	_currStep = step;
}

long AxisCtrl::getCurrStep()
{
	return _currStep;
}

void AxisCtrl::setNewPos(float val)
{
  long steps = getSteps(val);
  _oldPos = _newPos;
  _newPos = steps;
}

long AxisCtrl::getNewPos()
{
  return _newPos;
}

void AxisCtrl::setOldPos(long val)
{
  _oldPos = val;
}

long AxisCtrl::getOldPos(){
  return _oldPos;
}

long AxisCtrl::getSteps(float val){
  long steps;
  steps = val/_normLenghtPerStep;
  return steps;
}

void AxisCtrl::decrPos()
{
   _currPos -= _normLenghtPerStep;
}

void AxisCtrl::incrPos()
{
  _currPos += _normLenghtPerStep;
}

long AxisCtrl::getCurrPos()
{
  return _currPos; 
}

void AxisCtrl::setCurrPos(long val)
{
  _currPos = val;
}

long AxisCtrl::getNextPos()
{
  return _nextPos;
}

void AxisCtrl::setNextPos(long val)
{
  _nextPos = val;
}

2012-07-03_092621.jpg

long linearInterpolate(long x1, long y1, long x2, long y2, long X)
{
  long _Y = 0;
  _Y = y1 + ((X-x1)*y2 - (X-x1)*y1)/(x2-x1);
  return _Y;
}

What does this code do if x1 and x2 are equal? You really need to perform input validation.

Thanks PaulS!

But unfortunately the problem still happens. The value suddenly inverts into negative. Even when returning a unsigned long value. =(

unsigned long linearInterpolate(long x1, long y1, long x2, long y2, long X)
{
  unsigned long _Y = 0;
  if(x1 == x2){
    _Y = y2;
  } else {
    _Y = y1 + ((X-x1)*y2 - (X-x1)*y1)/(x2-x1);
  }
  return _Y;
}

The code still return suddenly:

(...)
X:53683 Y:21473 Y2:21473
X:53684 Y:21473 Y2:21473
X:53684 Y:21473 Y2:21473
X:53685 Y:21473 Y2:21473
X:53685 Y:21473 Y2:21474
X:53686 Y:21474 Y2:21474
X:53686 Y:21474 Y2:21474
X:53687 Y:21474 Y2:21474
X:53687 Y:21474 Y2:21475
X:53688 Y:21475 Y2:21475
X:53688 Y:21475 Y2:-21474
X:53689 Y:21476 Y2:-21474
X:53689 Y:21476 Y2:-21474
X:53690 Y:21477 Y2:-21474
X:53690 Y:21477 Y2:-21473
X:53691 Y:21478 Y2:-21473
X:53691 Y:21478 Y2:-21473
X:53692 Y:21479 Y2:-21473
X:53692 Y:21479 Y2:-21473
X:53693 Y:21480 Y2:-21473
X:53693 Y:21480 Y2:-21472
X:53694 Y:21481 Y2:-21472
X:53694 Y:21481 Y2:-21472
(...)

The value suddenly inverts into negative.

Given what input values? Perhaps it is perfectly reasonable that that happens.

Hm, I think the input values are all right.

I outputed the input values too:

X:53687 Y:21474 NEWX:99999 NEWY:40000 OLDX:0 OLDY:0 Y2:21474
X:53687 Y:21474 NEWX:99999 NEWY:40000 OLDX:0 OLDY:0 Y2:21475
X:53688 Y:21475 NEWX:99999 NEWY:40000 OLDX:0 OLDY:0 Y2:21475
X:53688 Y:21475 NEWX:99999 NEWY:40000 OLDX:0 OLDY:0 Y2:-21474
X:53689 Y:21476 NEWX:99999 NEWY:40000 OLDX:0 OLDY:0 Y2:-21474
X:53689 Y:21476 NEWX:99999 NEWY:40000 OLDX:0 OLDY:0 Y2:-21474

When you look at the value X:53688 the value afterwards is also X:53688 but now Y2: gets Y2:-21474.

For debugging sake I typed the input values in my calculator using the values that are used for the equation at the point it inverts:

(y1 + ((X-x1)*y2 - (X-x1)*y1)/(x2-x1))

x1 = 0 y1 = 0 x2 = 99999 y2 = 40000 X = 53689 / the first value where the functions returns a inverted number

0 + ((53689-0)*40000 - (53689-0)*0)/(99999-0) = 21475.81476

So no negative number is to expect.

53689-0)*40000 -

That's negative right there.

That's negative right there.

The function is using longs, so that value should be in range.

X:53688 Y:21475 NEWX:99999 NEWY:40000 OLDX:0 OLDY:0 Y2:21475 X:53688 Y:21475 NEWX:99999 NEWY:40000 OLDX:0 OLDY:0 Y2:-21474

Unless I'm missing something (like the code that printed this), two identical inputs produced two different outputs. That is normally not possible.

One question, though. If all of the inputs are signed longs, why is the output an unsigned long?

The function is using longs, so that value should be in range.

I agree it should, but it isn’t.

53688 Y:21475 NEWX:99999 NEWY:40000 OLDX:0 OLDY:0 Y2:-21474

0xD1B8 * 0x9c40 = 0x80008e00

Thanks AWOL! But that changes not in the middle of the equation!? Thanks PaulS too!

I just tried the following calculation inside my linearInterpolation:

(53687-0)*40000 = 2.147.480.000 (53688-0)*40000 = 2.147.520.000 (53689-0)*40000 = 2.147.560.000

Long only supports 2.147.483.647 so thats excatly where my equation breaks... :astonished: But _Y is an unsigned long so it should be at 4.294.967.295, right?? AAAhhh thats what you mean AWOL??

Maybe I should make the calculation in millimeters and not steps? So the values dont get that big?! Could that be the problem??? :~

I don't know what your coordinate system is, but why not make x1, y1, x2, y2 all unsigned longs too? Failing that, you'd need to go to long long datatypes.

Could that be the problem???

Haven't you already answered that question for yourself. I believe you have.

The maximum table size is: 130x130mm (1mm=2000Steps)

2000x130 = 260.000 260.000*260.000 = 6.760.000.000

so even an unsigned long is not sufficient. I tried long long but that datatype slows the whole system/steppers down considerably. I tried rewriting everything to float but my if() comparision does not work with float.

Anyway thank you both! I will go with 100*100mm that is in range. :)

I tried rewriting everything to float but my if() comparision does not work with float.

It should. There is nothing inherent in the the float type to prevent if tests from working (except the equality test; it is rare for two floats to be exactly equal).

Specifically, which if comparison doesn't work?

The machine does not stop when every values are float when doing these comparisions:

       if(x.getCurrPos() != x.getNewPos()){
         x.motorStep();
       }
       if(y.getCurrPos() != y.getNextPos()){
         y.motorStep();
       }
       if(z.getCurrPos() != z.getNewPos())
       {
         z.motorStep();
       }
       
       if(x.getCurrPos() == x.getNewPos() && y.getCurrPos() == y.getNewPos() && z.getCurrPos() == z.getNewPos())
       {
         readyForNextCmd = true;
       }

It does step and the values dont change anymore when the final position is reached, but it does not stop: because readyForNextCmd has to become true for that state (last if()).

You can't rely on equality/non-equality comparisons when using floating point values. Best test for some minimum difference.