diff --git a/cnc_ctrl_v1/Axis.cpp b/cnc_ctrl_v1/Axis.cpp index f4dda6d6..7e606168 100644 --- a/cnc_ctrl_v1/Axis.cpp +++ b/cnc_ctrl_v1/Axis.cpp @@ -44,9 +44,26 @@ void Axis::initializePID(const unsigned long& loopInterval){ } void Axis::write(const float& targetPosition){ - _timeLastMoved = millis(); - _pidSetpoint = targetPosition/ *_mmPerRotation; - return; + + _timeLastMoved = millis(); + + // Check that the Z axis position is within limits specified in Settings + + if(_axisName == 'Z'){ + if(!isnan(sysSettings.zAxisUpperLimit) && targetPosition > sysSettings.zAxisUpperLimit){ + _pidSetpoint = sysSettings.zAxisUpperLimit/ *_mmPerRotation; + return; + } + + if(!isnan(sysSettings.zAxisLowerLimit) && targetPosition < sysSettings.zAxisLowerLimit){ + _pidSetpoint = sysSettings.zAxisLowerLimit/ *_mmPerRotation; + return; + } + } + + // update target position + _pidSetpoint = targetPosition/ *_mmPerRotation; + return; } float Axis::read(){ @@ -61,11 +78,24 @@ float Axis::setpoint(){ } void Axis::set(const float& newAxisPosition){ + // update the zAxis upper and lower limits relative to the new axis position + if(_axisName == 'Z'){ + if(!isnan(sysSettings.zAxisUpperLimit)){ + sysSettings.zAxisUpperLimit += newAxisPosition - read(); + } + + if(!isnan(sysSettings.zAxisLowerLimit)){ + sysSettings.zAxisLowerLimit += newAxisPosition - read(); + } + + if(!isnan(sysSettings.zAxisUpperLimit) || !isnan(sysSettings.zAxisLowerLimit)){ + settingsSaveToEEprom(); + } + } //reset everything to the new value _pidSetpoint = newAxisPosition/ *_mmPerRotation; motorGearboxEncoder.encoder.write((newAxisPosition * *_encoderSteps)/ *_mmPerRotation); - } long Axis::steps(){ @@ -225,8 +255,23 @@ void Axis::detachIfIdle(){ void Axis::endMove(const float& finalTarget){ - _timeLastMoved = millis(); - _pidSetpoint = finalTarget/ *_mmPerRotation; + _timeLastMoved = millis(); + + // Check that the Z axis position is within limits specified in Settings + + if(_axisName == 'Z'){ + if(!isnan(sysSettings.zAxisUpperLimit) && finalTarget > sysSettings.zAxisUpperLimit){ + _pidSetpoint = sysSettings.zAxisUpperLimit/ *_mmPerRotation; + return; + } + + if(!isnan(sysSettings.zAxisLowerLimit) && finalTarget < sysSettings.zAxisLowerLimit){ + _pidSetpoint = sysSettings.zAxisLowerLimit/ *_mmPerRotation; + return; + } + } + + _pidSetpoint = finalTarget/ *_mmPerRotation; } diff --git a/cnc_ctrl_v1/Config.h b/cnc_ctrl_v1/Config.h index 668d76f9..221dfefd 100644 --- a/cnc_ctrl_v1/Config.h +++ b/cnc_ctrl_v1/Config.h @@ -20,11 +20,15 @@ // Debugging Options #define verboseDebug 0 // set to 0 for no debug messages, 1 for single-line messages, 2 to also output ring buffer contents + #define misloopDebug 0 // set to 1 for a warning every time the movement loop fails // to complete before being interrupted, helpful for loop // LOOPINTERVAL tuning + #define KINEMATICSDBG 0 // set to 1 for additional kinematics debug messaging +#define SPINDLE_SPEED 1 // set to 1 for pwm spindle speed control + #define FAKE_SERVO_PERMITTED 42 // store this value #define FAKE_SERVO 4095 // store the state of FAKE_SERVO in EEPROM[ 4095 ] to preserve // the state of FAKE_SERVO mode over resets. @@ -55,12 +59,15 @@ // Serial variables #define INCBUFFERLENGTH 128 // The number of bytes(characters) allocated to the // incoming buffer. + #define EXPGCODELINE 60 // Maximum expected Gcode line length in characters // including line ending character(s). Assumes // client will not send more than this. Ground // Control is currently set to 60. NIST spec allows // 256. This value must be <= INCBUFFERLENGTH + #define MAXBUFFERLINES 4 // The maximum number of lines allowed in the buffer + #define POSITIONTIMEOUT 200 // The minimum number of milliseconds between // position reports sent to Ground Control. This // cannot be larger than the connection timout in @@ -72,6 +79,7 @@ #define CMD_RESET 0x18 // ctrl-x., if received the program should do a soft reset // if received the program should do a soft reset + #define CMD_RESET2 '`' // alternate char because GC won't use control characters in a macro #endif diff --git a/cnc_ctrl_v1/GCode.cpp b/cnc_ctrl_v1/GCode.cpp index 19017669..a84d877d 100644 --- a/cnc_ctrl_v1/GCode.cpp +++ b/cnc_ctrl_v1/GCode.cpp @@ -20,6 +20,7 @@ Copyright 2014-2017 Bar Smith*/ #include "Maslow.h" #include +#include "math.h" maslowRingBuffer incSerialBuffer; String readyCommandString = ""; //KRK why is this a global? @@ -369,6 +370,51 @@ byte executeBcodeLine(const String& gcodeLine){ return STATUS_OK; } + // Use 'B99 ON' to set FAKE_SERVO mode on, + // 'B99' with no parameter, or any parameter other than 'ON' + if(gcodeLine.substring(0, 3) == "B17"){ + //The B17 sets the current Z position as Upper Limit for zAxis + if(!isnan(zAxis.read())){ + settingsStoreGlobalSetting(byte(43), zAxis.read()); + Serial.print(F("Upper limit set to: ")); + Serial.println(isnan(sysSettings.zAxisUpperLimit) ? F(" "): String(sysSettings.zAxisUpperLimit / sys.inchesToMMConversion)); + return STATUS_OK; + } else { + Serial.println(F("Error setting limit")); + return STATUS_GCODE_INVALID_TARGET; + } + } + + if(gcodeLine.substring(0, 3) == "B18"){ + //The B18 sets the current Z position as Lower Limit for zAxis + if(!isnan(zAxis.read())){ + settingsStoreGlobalSetting(byte(44), zAxis.read()); + Serial.print(F("Lower limit set to: ")); + Serial.println(isnan(sysSettings.zAxisLowerLimit) ? F(" "): String(sysSettings.zAxisLowerLimit / sys.inchesToMMConversion)); + return STATUS_OK; + } else { + Serial.println(F("Error setting limit")); + return STATUS_GCODE_INVALID_TARGET; + } + } + + if(gcodeLine.substring(0, 3) == "B19"){ + //The B19 clears limits for zAxis + settingsStoreGlobalSetting(byte(43), NAN); + settingsStoreGlobalSetting(byte(44), NAN); + Serial.println(F("Z Axis Limits Cleared")); + return STATUS_OK; + } + + if(gcodeLine.substring(0, 3) == "B20"){ + //The B20 echoes limits for zAxis + Serial.print(F("Z Axis Upper Limit: ")); + Serial.print(isnan(sysSettings.zAxisUpperLimit) ? F("NAN") : String(sysSettings.zAxisUpperLimit / sys.inchesToMMConversion)); + Serial.print(F(" Lower Limit: ")); + Serial.println(isnan(sysSettings.zAxisLowerLimit) ? F("NAN"): String(sysSettings.zAxisLowerLimit / sys.inchesToMMConversion)); + return STATUS_OK; + } + // Use 'B99 ON' to set FAKE_SERVO mode on, // 'B99' with no parameter, or any parameter other than 'ON' // turns FAKE_SERVO mode off. @@ -398,7 +444,17 @@ byte executeBcodeLine(const String& gcodeLine){ } return STATUS_INVALID_STATEMENT; } +void executeScodeLine(const String& gcodeLine){ + /* executes a single line of gcode beginning with the character 'S' for spindle speed */ + int sNumber = extractGcodeValue(gcodeLine,'S',-1); + if (sNumber == -1){ + sNumber = sys.SpindleSpeed; + } + if (setSpindleSpeed(sNumber)){ + sys.SpindleSpeed = sNumber; + } +} void executeGcodeLine(const String& gcodeLine){ /* @@ -566,6 +622,9 @@ void sanitizeCommandString(String& cmdString){ cmdString.remove(pos, 1); if (line_flags & LINE_FLAG_COMMENT_PARENTHESES) { line_flags &= ~(LINE_FLAG_COMMENT_PARENTHESES); } } + else { + cmdString.remove(pos, 1); + } } else { if (cmdString[pos] < ' ') { @@ -660,6 +719,13 @@ byte interpretCommandString(String& cmdString){ Serial.println(gcodeLine); executeMcodeLine(gcodeLine); } + else if(gcodeLine[0] == 'S'){ + #if defined (verboseDebug) && verboseDebug > 0 + Serial.print(F("iCS executing S code: ")); + #endif + Serial.println(gcodeLine); + executeScodeLine(gcodeLine); + } else { #if defined (verboseDebug) && verboseDebug > 0 Serial.print(F("iCS executing G code: ")); @@ -776,7 +842,9 @@ void G2(const String& readString, int G2orG3){ 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); + // Read target Z position from gcode. If it is not specified then set it to NAN so it will not be used. + float Z2 = sys.inchesToMMConversion*extractGcodeValue(readString, 'Z', NAN); + float R = sys.inchesToMMConversion*extractGcodeValue(readString, 'R', NAN); 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); @@ -784,8 +852,42 @@ void G2(const String& readString, int G2orG3){ float centerX = X1 + I; float centerY = Y1 + J; + // Calculate center point using radius R if it is provided. + if (!isnan(R) && R && (X2 != X1 || Y2 != Y1)) { + // e: clockwise -1, counterclockwise 1 + const float e = (G2orG3 == 2) ? -1 : 1, + // X and Y differences + dx = X2 - X1, + dy = Y2 - Y1, + // Linear distance between the points. + d = hypot(dx, dy), + // Distance to the arc pivot-point. + h = sqrt(sq(R) - sq(d * 0.5)), + // Point between the two points. + mx = (X1 + X2) * 0.5, + my = (Y1 + Y2) * 0.5, + // Slope of the perpendicular bisector. + sx = -dy / d, + sy = dx / d; + // Pivot-point of the arc. + centerX = mx + e * h * sx; + centerY = my + e * h * sy; + #if defined (verboseDebug) && verboseDebug > 0 + Serial.print(F("G0")); + Serial.print(G2orG3); + Serial.print(F(" Radius center: ")); + Serial.print(centerX); + Serial.print(F(",")); + Serial.println(centerY); + #endif + } + sys.feedrate = constrain(sys.feedrate, 1, sysSettings.maxFeed); //constrain the maximum feedrate, 35ipm = 900 mmpm + // If there is no target Z (Z2) then set Z1 to be NAN so it is not used. + if (isnan(Z2)) { + Z1 = Z2; + } if (G2orG3 == 2){ arc(X1, Y1, Z1, X2, Y2, Z2, centerX, centerY, sys.feedrate, CLOCKWISE); } diff --git a/cnc_ctrl_v1/GCode.h b/cnc_ctrl_v1/GCode.h index 880a601d..08c037c4 100644 --- a/cnc_ctrl_v1/GCode.h +++ b/cnc_ctrl_v1/GCode.h @@ -34,6 +34,7 @@ float extractGcodeValue(const String&, char, const float&); byte executeBcodeLine(const String&); void executeGcodeLine(const String&); void executeMcodeLine(const String&); +void executeScodeLine(const String&); void executeOtherCodeLine(const String&); int findNextGM(const String&, const int&); void sanitizeCommandString(String&); diff --git a/cnc_ctrl_v1/Motion.cpp b/cnc_ctrl_v1/Motion.cpp index 358dc222..a0b80e7b 100644 --- a/cnc_ctrl_v1/Motion.cpp +++ b/cnc_ctrl_v1/Motion.cpp @@ -237,7 +237,7 @@ int arc(const float& X1, const float& Y1, const float& Z1, const float& X2, co */ - //compute geometry + // Compute geometry. float pi = 3.1415; float radius = sqrt( sq(centerX - X1) + sq(centerY - Y1) ); float circumference = 2.0*pi*radius; @@ -245,12 +245,18 @@ int arc(const float& X1, const float& Y1, const float& Z1, const float& X2, co float startingAngle = atan2(Y1 - centerY, X1 - centerX); float endingAngle = atan2(Y2 - centerY, X2 - centerX); - // compute chord height of arc + // Compute chord height of arc. float chordSquared = sqrt(sq(X2 - X1) + sq(Y2 - Y1)); float tau = sqrt( sq(radius) - (chordSquared/4.0)); float chordHeight = radius - tau; - //compute angle between lines + // Determine if Z axis will be used. + bool useZ = true; + if (isnan(Z1) || isnan(Z2) || !sysSettings.zAxisAttached) { + useZ = false; + } + + // Compute angle between lines. float theta = endingAngle - startingAngle; if (direction == COUNTERCLOCKWISE){ if (theta <= 0){ @@ -269,7 +275,10 @@ int arc(const float& X1, const float& Y1, const float& Z1, const float& X2, 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) + " Z" + String(Z2 / sys.inchesToMMConversion, 3) + " "; + gcodeSubstitution += String(X2 / sys.inchesToMMConversion, 3) + " Y" + String(Y2 / sys.inchesToMMConversion, 3); + if (useZ) { + gcodeSubstitution += " Z" + String(Z2 / sys.inchesToMMConversion, 3) + " "; + } Serial.println("Large-radius arc replaced by straight line to improve accuracy: " + gcodeSubstitution); G1(gcodeSubstitution, 1); return 1; @@ -287,32 +296,37 @@ int arc(const float& X1, const float& Y1, const float& Z1, const float& X2, co 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; + float zFeedRate, zMaxFeed, zStepSizeMM; + + if (useZ) { + zFeedRate = calculateFeedrate(fabs(zDistanceToMoveInMM/finalNumberOfSteps), delayTime); + zMaxFeed = sysSettings.maxZRPM * abs(zAxis.getPitch()); + // zStepSizeMM = computeStepSize(zMaxFeed); + zStepSizeMM = zDistanceToMoveInMM/finalNumberOfSteps; - if (zFeedRate > zMaxFeed){ - zStepSizeMM = computeStepSize(zMaxFeed); - finalNumberOfSteps = fabs(zDistanceToMoveInMM/zStepSizeMM); - stepSizeMM = arcLengthMM/finalNumberOfSteps; - feedMMPerMin = calculateFeedrate(stepSizeMM, delayTime); + if (zFeedRate > zMaxFeed){ + // Recalculate all movement to fit within zMaxFeed parameter. + zStepSizeMM = computeStepSize(zMaxFeed); + finalNumberOfSteps = fabs(zDistanceToMoveInMM/zStepSizeMM); + stepSizeMM = arcLengthMM/finalNumberOfSteps; + feedMMPerMin = calculateFeedrate(stepSizeMM, delayTime); + } + + zStepSizeMM = zDistanceToMoveInMM/finalNumberOfSteps; } - - zStepSizeMM = zDistanceToMoveInMM/finalNumberOfSteps; - //Compute the starting position + // Compute the starting position. float angleNow = startingAngle; float degreeComplete = 0.0; float aChainLength; float bChainLength; - float zPosition = Z1 + zStepSizeMM; + float zPosition = Z1; - //attach the axes + // Attach the axes. leftAxis.attach(); rightAxis.attach(); - if (sysSettings.zAxisAttached) { + if (useZ) { zAxis.attach(); } @@ -335,17 +349,16 @@ int arc(const float& X1, const float& Y1, const float& Z1, const float& X2, co leftAxis.write(aChainLength); rightAxis.write(bChainLength); - if(sysSettings.zAxisAttached){ + if(useZ){ + zPosition += zStepSizeMM; zAxis.write(zPosition); } movementUpdate(); - // Run realtime commands + // Run real-time commands. execSystemRealtime(); if (sys.stop){return 1;} - numberOfStepsTaken++; - zPosition += zStepSizeMM; } } #if misloopDebug > 0 @@ -355,6 +368,9 @@ int arc(const float& X1, const float& Y1, const float& Z1, const float& X2, co kinematics.inverse(X2,Y2,&aChainLength,&bChainLength); leftAxis.endMove(aChainLength); rightAxis.endMove(bChainLength); + if (useZ) { + zAxis.endMove(Z2); + } sys.xPosition = X2; sys.yPosition = Y2; diff --git a/cnc_ctrl_v1/Report.cpp b/cnc_ctrl_v1/Report.cpp index 7b5f8077..ac1f8836 100644 --- a/cnc_ctrl_v1/Report.cpp +++ b/cnc_ctrl_v1/Report.cpp @@ -188,10 +188,12 @@ void reportMaslowSettings() { Serial.print(F("$40=")); Serial.println(sysSettings.leftChainTolerance, 8); Serial.print(F("$41=")); Serial.println(sysSettings.rightChainTolerance, 8); Serial.print(F("$42=")); Serial.println(sysSettings.positionErrorLimit, 8); - Serial.print(F("$43=")); Serial.println(sysSettings.reserved1, 8); - Serial.print(F("$44=")); Serial.println(sysSettings.reserved2, 8); + Serial.print(F("$43=")); Serial.println(sysSettings.zAxisUpperLimit, 8); + Serial.print(F("$44=")); Serial.println(sysSettings.zAxisLowerLimit, 8); Serial.print(F("$45=")); Serial.println(sysSettings.chainElongationFactor, 8); Serial.print(F("$46=")); Serial.println(sysSettings.sledWeight, 8); + Serial.print(F("$60=")); Serial.println(sysSettings.spindleMin,8); + Serial.print(F("$61=")); Serial.println(sysSettings.spindleMax,8); #else Serial.print(F("$0=")); Serial.print(sysSettings.machineWidth); @@ -236,11 +238,13 @@ void reportMaslowSettings() { Serial.print(F(" (PWM frequency value 1=39,000Hz, 2=4,100Hz, 3=490Hz)\r\n$40=")); Serial.print(sysSettings.leftChainTolerance, 8); Serial.print(F(" (chain tolerance, left chain, mm)\r\n$41=")); Serial.print(sysSettings.rightChainTolerance, 8); Serial.print(F(" (chain tolerance, right chain, mm)\r\n$42=")); Serial.print(sysSettings.positionErrorLimit, 8); - Serial.print(F(" (position error alarm limit, mm)\r\n$43=")); Serial.print(sysSettings.reserved1,8); - Serial.print(F(" (reserved1, deg)\r\n$44="));Serial.print(sysSettings.reserved2,8); - Serial.print(F(" (reserved2, mm)\r\n$45=")); Serial.print(sysSettings.chainElongationFactor,8); - Serial.print(F(" (chain stretch factor, m/m/N)\r\n$46=")); Serial.print(sysSettings.sledWeight,8); - Serial.print(F(" (Sled Weight, N)\r\n")); + Serial.print(F(" (position error alarm limit, mm)\r\n$43=")); Serial.print(sysSettings.zAxisUpperLimit,8); + Serial.print(F(" (z axis upper limit, mm)\r\n$44="));Serial.print(sysSettings.zAxisLowerLimit,8); + Serial.print(F(" (z axis lower limit, mm)\r\n$45=")); Serial.print(sysSettings.chainElongationFactor,8); + Serial.print(F(" (chain stretch factor, m/m/N)\r\n$46=")); Serial.print(sysSettings.sledWeight,8); + Serial.print(F(" (Sled Weight, N)\r\n"));Serial.println(sysSettings.spindleMin,8); + Serial.print(F(" (Spindle turn on speed \r\n")); Serial.println(sysSettings.spindleMax,8); + Serial.print(F(" (Spindle maximum rpm \r\n)); Serial.println(); #endif } diff --git a/cnc_ctrl_v1/Settings.cpp b/cnc_ctrl_v1/Settings.cpp index c92fd9b3..5856ce92 100644 --- a/cnc_ctrl_v1/Settings.cpp +++ b/cnc_ctrl_v1/Settings.cpp @@ -79,7 +79,7 @@ void settingsReset() { sysSettings.distPerRot = 63.5; // float distPerRot; sysSettings.maxFeed = 700; // int maxFeed; sysSettings.zAxisAttached = true; // zAxisAttached; - sysSettings.spindleAutomateType = NONE; // bool spindleAutomate; + sysSettings.spindleAutomateType = SPEED_CONTROL_RELAY_ACTIVE_HIGH; // bool spindleAutomate; sysSettings.maxZRPM = 12.60; // float maxZRPM; sysSettings.zDistPerRot = 3.17; // float zDistPerRot; sysSettings.zEncoderSteps = 7560.0; // float zEncoderSteps; @@ -105,10 +105,12 @@ void settingsReset() { sysSettings.leftChainTolerance = 0.0; // float leftChainTolerance; sysSettings.rightChainTolerance = 0.0; // float rightChainTolerance; sysSettings.positionErrorLimit = 2.0; // float positionErrorLimit; - sysSettings.reserved1 = 0.0; - sysSettings.reserved2 = 0.0; + sysSettings.zAxisUpperLimit = NAN; // float zAxisUpperLimit + sysSettings.zAxisLowerLimit = NAN; // float zAxisLowerLimit sysSettings.chainElongationFactor = 8.1E-6; // m/m/N sysSettings.sledWeight = 11.6*9.8; // Newtons. For a sled with one ring kit, one Rigid 2200 router and two 2.35kg bricks on a 5/8" thick mdf 18" diameter base. + sysSettings.spindleMin = 4000; + sysSettings.spindleMax = 24000; sysSettings.eepromValidData = EEPROMVALIDDATA; // byte eepromValidData; } @@ -418,7 +420,7 @@ byte settingsStoreGlobalSetting(const byte& parameter,const float& value){ kinematics.init(); break; case 44: - sysSettings.reserved2 = value; + sysSettings.zAxisLowerLimit = value; kinematics.init(); break; case 45: @@ -429,6 +431,10 @@ byte settingsStoreGlobalSetting(const byte& parameter,const float& value){ sysSettings.sledWeight = value; kinematics.init();; break; + case 60: + sysSettings.spindleMin = value; + case 61: + sysSettings.spindleMax = value; default: return(STATUS_INVALID_STATEMENT); } diff --git a/cnc_ctrl_v1/Settings.h b/cnc_ctrl_v1/Settings.h index 29fb3944..6213395d 100644 --- a/cnc_ctrl_v1/Settings.h +++ b/cnc_ctrl_v1/Settings.h @@ -36,7 +36,9 @@ enum SpindleAutomationType { NONE, SERVO, RELAY_ACTIVE_HIGH, - RELAY_ACTIVE_LOW }; + RELAY_ACTIVE_LOW, + SPEED_CONTROL_RELAY_ACTIVE_HIGH, + SPEED_CONTROL_RELAY_ACTIVE_LOW}; typedef struct { // I think this is about ~128 bytes in size if I counted correctly float machineWidth; @@ -81,10 +83,15 @@ typedef struct { // I think this is about ~128 bytes in size if I counted corre float leftChainTolerance; float rightChainTolerance; float positionErrorLimit; - float reserved1; - float reserved2; float chainElongationFactor; // m/m/N. This is the ratio of chain length increase due to chain tension. typically 8x10E-6; // m/m/N float sledWeight; // Newtons. simply multiply kg by 9.8 or pounds by 2.2*9.8 + int spindleMin; //added for pwm spindle control + int spindleMax; //added for pwm spindle control + float zAxisUpperLimit; // A safety feature to prevent motors from attempting to move the z axis beyong a user specified constraint. + float zAxisLowerLimit; + float positionErrorLimit; + float chainElongationFactor; // m/m/N. This is the ratio of chain length increase due to chain tension. typically 8x10E-6; // m/m/N + float sledWeight; // Newtons. simply multiply kg by 9.8 or pounds by 2.2*9.8 byte eepromValidData; // This should always be last, that way if an error // happens in writing, it will not be written and we } settings_t; // will know to reset the settings diff --git a/cnc_ctrl_v1/Spindle.cpp b/cnc_ctrl_v1/Spindle.cpp index 853a1d64..c7b2c0f0 100644 --- a/cnc_ctrl_v1/Spindle.cpp +++ b/cnc_ctrl_v1/Spindle.cpp @@ -20,7 +20,9 @@ // the variables SpindlePowerControlPin and LaserPowerPin are assigned in setupAxes() in System.cpp // Globals for Spindle control, both poorly named +#ifndef SPINDLE_SPEED Servo myservo; // create servo object to control a servo +#endif void setSpindlePower(bool powerState) { /* @@ -41,6 +43,7 @@ void setSpindlePower(bool powerState) { Serial.print(F("Spindle automation type ")); Serial.print(spindleAutomateType); #endif + #ifndef SPINDLE_SPEED if (spindleAutomateType == SERVO) { // use a servo to control a standard wall switch #if defined (verboseDebug) && verboseDebug > 1 Serial.print(F(" with servo (idle=")); @@ -70,7 +73,42 @@ void setSpindlePower(bool powerState) { if(sys.stop){return;} myservo.detach(); // stop servo control } - else if (spindleAutomateType == RELAY_ACTIVE_HIGH) { + #endif + if (spindleAutomateType == SPEED_CONTROL_RELAY_ACTIVE_HIGH){ + #if defined (verboseDebug) && verboseDebug > 1 + Serial.print(F(" as digital power and pwm speed control, active high")); + #endif + pinMode(SpindlePowerControlPin, OUTPUT); + if (powerState) { // turn on spindle + Serial.println(F("Turning Spindle On")); + digitalWrite(SpindlePowerControlPin, HIGH); + Serial.println(F("Adjusting Spindle Speed to Minimum")); + setSpindleSpeed(sysSettings.spindleMin); + } + else { // turn off spindle + Serial.println(F("Turning Spindle Off")); + digitalWrite(SpindlePowerControlPin, LOW); + digitalWrite(SpindleSpeedControlPin, LOW); + } + } + if (spindleAutomateType == SPEED_CONTROL_RELAY_ACTIVE_LOW){ + #if defined (verboseDebug) && verboseDebug > 1 + Serial.print(F(" as digital power and pwm speed control, active low")); + #endif + pinMode(SpindlePowerControlPin, OUTPUT); + if (powerState) { // turn on spindle + Serial.println(F("Turning Spindle On")); + digitalWrite(SpindlePowerControlPin, LOW); + Serial.println(F("Adjusting Spindle Speed to Minimum")); + setSpindleSpeed(sysSettings.spindleMin); + } + else { // turn off spindle + Serial.println(F("Turning Spindle Off")); + digitalWrite(SpindlePowerControlPin, HIGH); + digitalWrite(SpindleSpeedControlPin, LOW); + } + } + if (spindleAutomateType == RELAY_ACTIVE_HIGH) { #if defined (verboseDebug) && verboseDebug > 1 Serial.print(F(" as digital output, active high")); #endif @@ -103,6 +141,24 @@ void setSpindlePower(bool powerState) { } } +int setSpindleSpeed(int spindleSpeed){ + // map 0-255 min spindle speed to max spindle speed + // output 0-255 + if (spindleSpeed < sysSettings.spindleMin){ + spindleSpeed = sysSettings.spindleMin; + } + if (spindleSpeed > sysSettings.spindleMax){ + spindleSpeed = sysSettings.spindleMax; + } + long spd = map (spindleSpeed, 0, sysSettings.spindleMax, 0, 255); + analogWrite(SpindleSpeedControlPin,spd); + return(1); + //for (i=0;i 0) pinMode(aux3,INPUT); if (aux5 > 0) pinMode(aux5,INPUT); if (aux6 > 0) pinMode(aux6,INPUT); - if (aux7 > 0) pinMode(aux7,INPUT); + #ifndef SPINDLE_SPEED + if (aux7 > 0) pinMode(aux7,INPUT); + #endif if (aux8 > 0) pinMode(aux8,INPUT); if (aux9 > 0) pinMode(aux9,INPUT); } diff --git a/cnc_ctrl_v1/System.h b/cnc_ctrl_v1/System.h index 25bea189..3f0d88b7 100644 --- a/cnc_ctrl_v1/System.h +++ b/cnc_ctrl_v1/System.h @@ -60,6 +60,7 @@ typedef struct { bool useRelativeUnits; // unsigned long lastSerialRcvd; // The millis of the last rcvd serial command, used by watchdo int lastGCommand; //Stores the value of the last command run eg: G01 -> 1 + int SpindleSpeed; // store the current spindle speed -> 4000 int lastTool; //Stores the value of the last tool number eg: T4 -> 4 int nextTool; //Stores the value of the next tool number eg: T4 -> 4 float inchesToMMConversion; //Used to track whether to convert from inches, can probably be done in a way that doesn't require RAM