Skip to content

Commit

Permalink
Merge pull request #473 from blurfl/G2/G3-Z-Axis-coordinated-move
Browse files Browse the repository at this point in the history 
G2/g3 z axis coordinated move
  • Loading branch information
@MaslowCommunityGardenRobot
MaslowCommunityGardenRobot authored Oct 27, 2018
2 parents 063fb13 + f21b659 commit 3b3638c
Show file tree
Hide file tree
Showing 3 changed files with 37 additions and 12 deletions.
6 changes: 4 additions & 2 deletions cnc_ctrl_v1/GCode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -709,9 +709,11 @@ void G2(const String& readString, int G2orG3){

float X1 = sys.xPosition; //does this work if units are inches? (It seems to)
float Y1 = sys.yPosition;
float Z1 = zAxis.read(); // I don't know why we treat the zaxis differently

float X2 = sys.inchesToMMConversion*extractGcodeValue(readString, 'X', X1/sys.inchesToMMConversion);
float Y2 = sys.inchesToMMConversion*extractGcodeValue(readString, 'Y', Y1/sys.inchesToMMConversion);
float Z2 = sys.inchesToMMConversion*extractGcodeValue(readString, 'Z', Z1/sys.inchesToMMConversion);
float I = sys.inchesToMMConversion*extractGcodeValue(readString, 'I', 0.0);
float J = sys.inchesToMMConversion*extractGcodeValue(readString, 'J', 0.0);
sys.feedrate = sys.inchesToMMConversion*extractGcodeValue(readString, 'F', sys.feedrate/sys.inchesToMMConversion);
Expand All @@ -722,10 +724,10 @@ void G2(const String& readString, int G2orG3){
sys.feedrate = constrain(sys.feedrate, 1, sysSettings.maxFeed); //constrain the maximum feedrate, 35ipm = 900 mmpm

if (G2orG3 == 2){
arc(X1, Y1, X2, Y2, centerX, centerY, sys.feedrate, CLOCKWISE);
arc(X1, Y1, Z1, X2, Y2, Z2, centerX, centerY, sys.feedrate, CLOCKWISE);
}
else {
arc(X1, Y1, X2, Y2, centerX, centerY, sys.feedrate, COUNTERCLOCKWISE);
arc(X1, Y1, Z1, X2, Y2, Z2, centerX, centerY, sys.feedrate, COUNTERCLOCKWISE);
}
}

Expand Down
41 changes: 32 additions & 9 deletions cnc_ctrl_v1/Motion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ int coordinatedMove(const float& xEnd, const float& yEnd, const float& zEnd, f
float stepSizeMM = computeStepSize(MMPerMin);
float finalNumberOfSteps = abs(distanceToMoveInMM/stepSizeMM);
float delayTime = LOOPINTERVAL;
float zFeedrate = calculateFeedrate(abs(zDistanceToMoveInMM/finalNumberOfSteps), delayTime);
float zFeedrate = calculateFeedrate(fabs(zDistanceToMoveInMM/finalNumberOfSteps), delayTime);

//throttle back federate if it exceeds zaxis max
if (zFeedrate > zMaxFeed){
Expand Down Expand Up @@ -229,7 +229,7 @@ void singleAxisMove(Axis* axis, const float& endPos, const float& MMPerMin){
int sign(double x) { return x<0 ? -1 : 1; }

// why does this return anything
int arc(const float& X1, const float& Y1, const float& X2, const float& Y2, const float& centerX, const float& centerY, const float& MMPerMin, const float& direction){
int arc(const float& X1, const float& Y1, const float& Z1, const float& X2, const float& Y2, const float& Z2, const float& centerX, const float& centerY, const float& MMPerMin, const float& direction){
/*
Move the machine through an arc from point (X1, Y1) to point (X2, Y2) along the
Expand Down Expand Up @@ -269,33 +269,52 @@ int arc(const float& X1, const float& Y1, const float& X2, const float& Y2, co
// In either case, the gcode cut was essentially a straight line, so
// Replace it with a G1 cut to the endpoint
String gcodeSubstitution = "G1 X";
gcodeSubstitution = gcodeSubstitution + String(X2 / sys.inchesToMMConversion, 3) + " Y" + String(Y2 / sys.inchesToMMConversion, 3) + " ";
gcodeSubstitution = gcodeSubstitution + String(X2 / sys.inchesToMMConversion, 3) + " Y" + String(Y2 / sys.inchesToMMConversion, 3) + " Z" + String(Z2 / sys.inchesToMMConversion, 3) + " ";
Serial.println("Large-radius arc replaced by straight line to improve accuracy: " + gcodeSubstitution);
G1(gcodeSubstitution, 1);
return 1;
}

float arcLengthMM = circumference * (theta / (2*pi) );
float arcLengthMM = fabs(circumference * (theta / (2*pi) ));
float zDistanceToMoveInMM = Z2 - Z1;

//set up variables for movement
long numberOfStepsTaken = 0;

float stepSizeMM = computeStepSize(MMPerMin);
float feedMMPerMin = constrain(MMPerMin, 1, sysSettings.maxFeed);
float stepSizeMM = computeStepSize(feedMMPerMin);

//the argument to abs should only be a variable -- splitting calc into 2 lines
long finalNumberOfSteps = arcLengthMM/stepSizeMM;
//finalNumberOfSteps = abs(finalNumberOfSteps);
long finalNumberOfSteps = arcLengthMM/stepSizeMM;
float delayTime = LOOPINTERVAL;

float zFeedRate = calculateFeedrate(fabs(zDistanceToMoveInMM/finalNumberOfSteps), delayTime);
float zMaxFeed = sysSettings.maxZRPM * abs(zAxis.getPitch());
// float zStepSizeMM = computeStepSize(zMaxFeed);
float zStepSizeMM = zDistanceToMoveInMM/finalNumberOfSteps;

if (zFeedRate > zMaxFeed){
zStepSizeMM = computeStepSize(zMaxFeed);
finalNumberOfSteps = fabs(zDistanceToMoveInMM/zStepSizeMM);
stepSizeMM = arcLengthMM/finalNumberOfSteps;
feedMMPerMin = calculateFeedrate(stepSizeMM, delayTime);
}

zStepSizeMM = zDistanceToMoveInMM/finalNumberOfSteps;

//Compute the starting position
float angleNow = startingAngle;
float degreeComplete = 0.0;

float aChainLength;
float bChainLength;
float zPosition = Z1 + zStepSizeMM;

//attach the axes
leftAxis.attach();
rightAxis.attach();
if (sysSettings.zAxisAttached) {
zAxis.attach();
}

while(numberOfStepsTaken < abs(finalNumberOfSteps)){
#if misloopDebug > 0
Expand All @@ -311,18 +330,22 @@ int arc(const float& X1, const float& Y1, const float& X2, const float& Y2, co

sys.xPosition = radius * cos(angleNow) + centerX;
sys.yPosition = radius * sin(angleNow) + centerY;

kinematics.inverse(sys.xPosition,sys.yPosition,&aChainLength,&bChainLength);

leftAxis.write(aChainLength);
rightAxis.write(bChainLength);
if(sysSettings.zAxisAttached){
zAxis.write(zPosition);
}
movementUpdate();

// Run realtime commands
execSystemRealtime();
if (sys.stop){return 1;}

numberOfStepsTaken++;
zPosition += zStepSizeMM;
}
}
#if misloopDebug > 0
Expand Down
2 changes: 1 addition & 1 deletion cnc_ctrl_v1/Motion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ extern volatile bool movementUpdated;
void initMotion();
int coordinatedMove(const float&, const float&, const float&, float);
void singleAxisMove(Axis*, const float&, const float&);
int arc(const float&, const float&, const float&, const float&, const float&, const float&, const float&, const float&);
int arc(const float&, const float&, const float&, const float&, const float&, const float&, const float&, const float&, const float&, const float&);
float calculateFeedrate(const float&, const float&);
float computeStepSize(const float&);
void movementUpdate();
Expand Down

0 comments on commit 3b3638c

Please sign in to comment.