Hi Guys! Ive done buckets of trouble shooting on this, and i THINK i have identified where the problem is, but i cannt work out how to correct it. In short i am programming my CNC router (converting GCode into CNC movements)
I am working on circular interpolation. Because of the problems i have been having i have stripped back the code as far as i reasonably can. This sketch below parses out GCode instructions, interpolates 10 points between each GCode position and sends them to the Serial port in CSV format. This way i can quickly paste them into excel and live-update a scatter chart. I should be plotting a circle. Linerar interpolation works great, i am ending up with a (12 sided? Dodecagon?) circle.
circular interpolation is still causing me issues. I am using basic pythagarous to calculate the positions, and have put my calculations though a simple sketch that only does the pythagarous part and had the same result
http://duxtecharduino.blogspot.co.uk/2010/10/arduino-math-continued-pythagorean.html
If my reworking of the sketch on the link above is anything to go by, the problem is using pythagarous with large numbers.
If i run the sketch and leave the highlighted line as an INT, it runs through, but the math result is all wrong (repeatable on the super simple sketch)
if i make it a double, i get the correct result on the super simple sketch, but on the CNC sketch it freezes the program.
Why is the highlighted line freezing the sketch? Overflowing perhaps? How can i change the math to ensure the correct result (i think it would work if it was a double) without freezing the sketch up.
Hope this makes sense. Cheers. Olly. My sketch is below, Ive highlighted the line in Bold, line 339 if you paste it into the IDE. Sorry i cant make the forum format it as proper code for you!
#include <AccelStepper.h> // Copyright (C) 2009 Mike McCauley
#include <SD.h>
#include <math.h>
//Connect Stepper motors
AccelStepper stepperX(AccelStepper::FULL4WIRE, 7, 6, 5, 4); //Red-10, Yellow-11, Blue-9, Orange-8
AccelStepper stepperY(AccelStepper::FULL4WIRE, 14, 15, 16, 17); // Red-5, Yellow-4, Blue-6, Orange-7
//Connect SD Card
const int chipSelect = 10; // adafruit SD breakout on pins 10, 11, 12 & 13
// Connect other parts
const int Interlock = 2; // Interlock switch sensor
const int button = 3;// attach a generic button
const int pin13LED = 13; // use arduino onboard LED
const int MOT = 8; // attach tool run Motor
const int SOL = 18; // attach tool lift solenoid
const int pot = A5; // attach tool run Motor
//SD Card file
File myFile; // instance of a file
// WORKING VALUES
byte inbyte;
char inputCharString [100];
char inputChar;
int stringIndex = 0; // String stringIndexing int;
String stringG; // substrings store Values
String stringX;
String stringY;
String stringZ;
String stringI;
String stringJ;
int readG;// integers for saving substrings
int readX;
int readY;
int readI;
int readJ;
double h;
int prevX; // calculation ints
int prevY;
int xDist;
int yDist;
int Feed = 100000; // adjust feed speed in SPS! max 500 (loses track at 500?)
int MaxFeed = 100000; // adjust feed speed in SPS! max 500 (loses track at 500?)
int iter; // iterations of interpolation
int i;// iteration counter
int printX;
int printY;
boolean prevToolPos; // previous pen position, for comparison.
boolean toolDown = false; // tool position.
void setup()
{
Serial.begin(9600); // Serial port
//Define Pin Modes
pinMode (Interlock, INPUT); // interlock sensor pin. Could return error if lid is open
pinMode (SOL, OUTPUT);
pinMode (MOT, OUTPUT);
pinMode (button, INPUT);
pinMode (pot, INPUT);
pinMode (pin13LED, OUTPUT);
digitalWrite (MOT,LOW); // HIGH is running
digitalWrite (SOL,HIGH); // HIGH is up
//Setup Motors
stepperX.setCurrentPosition(-3000); //(starting pos)
stepperY.setCurrentPosition(-2600);
stepperX.setMaxSpeed(Feed); // Max Feed for smooth operation
stepperY.setMaxSpeed(Feed); // Max Feed for smooth operation
stepperX.setAcceleration(100.0); // Acceleration not utilised
stepperY.setAcceleration(100.0); // Acceleration not utilised
//Initialize SD card
// Serial.print("Motors ready, Initializing SD card...");
pinMode(SS, OUTPUT); // make sure that the default chip select pin is set to output, even if you don't use it:
if (!SD.begin(10,11,12,13)) { // see if the card is present and can be initialized:
Serial.println("Card failed, or not present");
while (1) ; // don't do anything more:
}
// Serial.println("card initialized."); //Open file to read
myFile = SD.open("FILE.txt");// FILE NAME *******************************
if (! myFile) {
Serial.println("error opening file"); // If failed to open Wait forever
while (1) ;
}
digitalWrite (pin13LED, HIGH);
// Serial.println("Button press will Run"); //hold
while (digitalRead (button) == LOW){
//hold
}
}
void loop()
{
// digitalWrite (MOT, HIGH); // Comment out this line to prevent Code controlled motor run. (Can still be run manually)*********************
{
while (myFile.available()) {
inputChar = myFile.read(); // Gets one byte from serial buffer
if (inputChar != 10) // Write to string until "end of line" ascii recieved
{
inputCharString[stringIndex] = inputChar; // Store it
stringIndex++; // Increment where to write next
}
else
{
{ // in this bracket pair, 2dp cooridnates extracted from Gcode.
inputCharString[stringIndex] = 0; // Add last char as ascii "void" to stop from reading the rest of the string (from prev lines longer than current one)
String inputString = inputCharString;
if (inputString[0] == 'G') // if line starts with a G process it, if not discard it
{
stringG = inputString.substring(1,3); // extract G value
int Xpos = inputString.indexOf('X'); //locate the position of X in the string
int Ypos = inputString.indexOf('Y'); //locate the position of Y in the string
int Zpos = inputString.indexOf('Z'); //locate the position of Z in the string
int Ipos = inputString.indexOf('I'); //locate the position of Z in the string
int Jpos = inputString.indexOf('J'); //locate the position of Z in the string
if (Xpos > 0) {
stringX = inputString.substring(Xpos+1,Ypos-1) ; // value for X is located between X and Y. If it exists, cut it into a substring
} // if it doesnt exist it will remain as previous
if (Ypos > 0) {
stringY = inputString.substring(Ypos+1,Zpos-1) ; // value for Y is located between Y and Z. If it exists, cut it into a substring
} // if it doesnt exist it will remain as previous
if (Zpos > 0) {
stringZ = inputString.substring(Zpos,Ipos-1) ; // value for Y is located between Y and Z. If it exists, cut it into a substring
} // if it doesnt exist it will remain as previous
if (Ipos > 0) {
stringI = inputString.substring(Ipos+1,Jpos-1) ; // value for Y is located between Y and Z. If it exists, cut it into a substring
} // if it doesnt exist it will remain as previous
if (Jpos > 0) {
stringJ = inputString.substring(Jpos+1,Jpos+10) ; // value for Z is located after the Z. no more than 10 chars needed. If it exists, cut it into a substring
} // if it doesnt exist it will remain as previous
// TRANSFER Z STRING INTO BOOLEAN PEN POSITION
if (stringZ.charAt(1) == '1') { // Pen up pen down, Z1.000000 is pen in up position
toolDown = false;
}
else if (stringZ.charAt(1) == '-') { // Z-0.12Feed0 pen is in down position
toolDown = true;
}
}
// TRANSFER G STRING INTO FLOAT (2dec place)
char carrayG[stringG.length() + 1]; //determine size of the array
stringG.toCharArray(carrayG, sizeof(carrayG)); //put readStringinto an array
int readG = atof(carrayG); //convert the array into an Integer
// TRANSFER X STRING INTO FLOAT (2dec place)
char carrayX[stringX.length() + 1]; //determine size of the array
stringX.toCharArray(carrayX, sizeof(carrayX)); //put readStringinto an array
double readX = atof(carrayX); //convert the array into an Integer
// TRANSFER Y STRING INTO FLOAT (2dec place)
char carrayY[stringY.length() + 1]; //determine size of the array
stringY.toCharArray(carrayY, sizeof(carrayY)); //put readStringinto an array
double readY = atof(carrayY); //convert the array into an Integer
// TRANSFER I STRING INTO FLOAT (2dec place)
char carrayI[stringI.length() + 1]; //determine size of the array
stringI.toCharArray(carrayI, sizeof(carrayI)); //put readStringinto an array
double readI = atof(carrayI); //convert the array into an Integer
// TRANSFER J STRING INTO FLOAT (2dec place)
char carrayJ[stringJ.length() + 1]; //determine size of the array
stringJ.toCharArray(carrayJ, sizeof(carrayJ)); //put readStringinto an array
double readJ = atof(carrayJ); //convert the array into an Integer
/*
Serial.print("G: "); // print the values to Serial Port
Serial.print(readG);
Serial.print(" X: ");
Serial.print(readX);
Serial.print(" Y: ");
Serial.print(readY);
Serial.print(" I: ");
Serial.print(readI);
Serial.print(" J: ");
Serial.print(readJ);
Serial.print(" Zbool: ");
Serial.print(toolDown); // (boolean, not value)
Serial.print(" Feed: ");
Serial.println(Feed); // Feed Speed
*/
readX = readX * 100; // convert 2dp mm into steps
readY = readY * 100; // convert 2dp mm into steps
readI = readI * 100; // convert 2dp mm into steps
readJ = readJ * 100; // convert 2dp mm into steps
//***necessary values are now populated and presented as required. actions below this line***
// Tool position:
if (toolDown != prevToolPos) {// compare tool pos with previous. if different from prev update
if (toolDown == true){
digitalWrite (SOL,LOW);
delay (1000);
}
else if (toolDown == false){
digitalWrite (SOL,HIGH);
delay (1000);
}
}
/*
Encountered instructions on first example "JA": Cases required for each of these instances
GCode Parser (above) currently discards anything that doesnt start with a G, thereofre M and F dont work
M3: Spindle Run (CW)
F: Feed rate (Stepper max speed)
G21: Define prgramming in mm
G00: Rapid Position: Move fast (without cutting)
G01: Straight line cut
G02: Clockwise radial cut
G03: Counter clockwise radial cut
M5: Spindle Stop
M2: End of Program
*/
switch(readG){
case 0: // G00, fast move
stepperX.moveTo(readX);
stepperY.moveTo(readY);
stepperX.setSpeed(MaxFeed);
stepperY.setSpeed(MaxFeed);
while (stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0) {
stepperX.runSpeedToPosition();
stepperY.runSpeedToPosition();
}
printX = stepperX.currentPosition(); //****
printY = stepperY.currentPosition(); //****
Serial.print(readG);//********************
Serial.print(",");//***********************
Serial.print(printX);//********************
Serial.print(",");//***********************
Serial.println(printY);//******************
break;
case 1: // G01: Straight line cut
xDist = (readX - prevX);
yDist = (readY - prevY);
Feed = analogRead(pot); // reads the value of the potentiometer (value between 0 and 1023)
Feed = map(Feed, 0, 1023, 10, MaxFeed); // converts to feed speed
if (xDist + yDist < 500) {
iter = 10;
}
else {
iter = 10; //<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 100
}
for (int i=1; i <= iter; i++){
stepperX.moveTo(prevX+(xDist/iter*(i)));
stepperY.moveTo(prevY+(yDist/iter*(i)));
stepperX.setSpeed(Feed);
stepperY.setSpeed(Feed);
while (stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0) {
stepperX.runSpeedToPosition();
stepperY.runSpeedToPosition();
}
printX = stepperX.currentPosition(); //****
printY = stepperY.currentPosition(); //****
Serial.print(readG);//********************
Serial.print(",");//***********************
Serial.print(printX);//********************
Serial.print(",");//***********************
Serial.println(printY);//******************
}
stepperX.moveTo(readX);
stepperY.moveTo(readY);
stepperX.setSpeed(Feed);
stepperY.setSpeed(Feed);
while (stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0) {
stepperX.runSpeedToPosition();
stepperY.runSpeedToPosition();
}
break;
case 2: // G02: Clockwise radial cut /
/*
PrevX: previous position
readX: target
readI: XOffset (+ or -)
readJ: JOffset (+ or -)
h = hyp
*/
// Feed Speed & iters
Feed = analogRead(pot); // reads the value of the potentiometer (value between 0 and 1023)
Feed = map(Feed, 0, 1023, 10, MaxFeed); // converts to feed speed
iter = 10;
// Dimensions
xDist = (readX - prevX);
h = sqrt(sq(readJ) + sq(readI)); // hypotenuse
for (int i=1; i <= iter; i++){ //****************************************
double Xmove = (xDist/iter)*i; // divide Xside into 10
int Xside = (Xmove+readI); // length of XSide
double Yside = sqrt (sq(h) - sq(Xside)); // <<<<<< Curves??
stepperY.moveTo(prevY+readJ+Yside);
stepperX.moveTo(prevX+Xmove);
stepperX.setSpeed(Feed);
stepperY.setSpeed(Feed);
while (stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0) {
stepperX.runSpeedToPosition();
stepperY.runSpeedToPosition();
}
printX = stepperX.currentPosition(); //****
printY = stepperY.currentPosition(); //****
Serial.print(readG);//********************
Serial.print(",");//***********************
Serial.print(printX);//********************
Serial.print(",");//***********************
Serial.print(printY);//******************
Serial.print(",");//***********************
Serial.print(Xside);//******************
Serial.print(",");//***********************
Serial.print(Yside);//******************
Serial.print(",");//***********************
Serial.println(h);//************
} //****************************
stepperX.moveTo(readX);
stepperY.moveTo(readY);
stepperX.setSpeed(Feed);
stepperY.setSpeed(Feed);
while (stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0) {
stepperX.runSpeedToPosition();
stepperY.runSpeedToPosition();
}
printX = stepperX.currentPosition(); //****
printY = stepperY.currentPosition(); //****
Serial.print(readG);//********************
Serial.print(",");//***********************
Serial.print(printX);//********************
Serial.print(",");//***********************
Serial.println(printY);//******************
break;
case 3: // G03: Counter clockwise radial cut // CURRENTLY AS G01
break;
case 21: // all units in mm
break;
}// end of cases
}
prevToolPos = toolDown; // update prev Pen Pos
stringIndex = 0; // clear the String index value for the next cycle
readG = 0;
i = 0; // reset iteration counter
prevX = stepperX.currentPosition(); //****
prevY = stepperY.currentPosition(); //****
}
}
myFile.close();
digitalWrite (MOT, LOW);
}
stepperX.moveTo(-3001); // return to park at end of job
stepperY.moveTo(-2601);
stepperX.setSpeed(MaxFeed);
stepperY.setSpeed(MaxFeed);
while (stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0) {
stepperX.runSpeedToPosition();
stepperY.runSpeedToPosition();
}
}