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;
}
