diff --git a/cnc_ctrl_v1/Axis.cpp b/cnc_ctrl_v1/Axis.cpp index f4dda6d6..528ad75c 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,26 @@ 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); - + motorGearboxEncoder.encoder.write((newAxisPosition * *_encoderSteps)/ *_mmPerRotation); } long Axis::steps(){ @@ -85,6 +117,7 @@ void Axis::setSteps(const long& steps){ void Axis::computePID(){ + if (FAKE_SERVO_STATE == FAKE_SERVO_PERMITTED) { if (motorGearboxEncoder.motor.attached()){ // Adds up to 10% error just to simulate servo noise @@ -224,12 +257,28 @@ void Axis::detachIfIdle(){ } void Axis::endMove(const float& finalTarget){ + + _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; + } + } - _timeLastMoved = millis(); - _pidSetpoint = finalTarget/ *_mmPerRotation; - + _pidSetpoint = finalTarget/ *_mmPerRotation; + } + void Axis::stop(){ /* diff --git a/cnc_ctrl_v1/Config.h b/cnc_ctrl_v1/Config.h index 668d76f9..ce14e8b3 100644 --- a/cnc_ctrl_v1/Config.h +++ b/cnc_ctrl_v1/Config.h @@ -20,12 +20,17 @@ // 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. // Use 'B99 ON' to turn FAKE_SERVO mode on and set EEPROM[ 4095 ] to '1', @@ -55,12 +60,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 +80,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..497d2da1 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? @@ -371,11 +372,75 @@ byte executeBcodeLine(const String& gcodeLine){ // 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. // FAKE_SERVO mode causes the Firmware to mimic a servo, // updating the encoder steps even if no servo is connected. // Useful for testing on an arduino only (e.g. without motors). - // The status of FAKE_SERVO mode is stored in EEPROM[ 4095 ] + // The status of FAKE_SERVO mode is stored in EEPROM[ 4095 ] // to persist between resets. That byte is set to 'FAKE_SERVO_PERMITTED' when FAKE_SERVO // is on, '0' when off. settingsWipe(SETTINGS_RESTORE_ALL) clears the // EEPROM to '0', sothat stores '0' at EEPROM[ 4095 ] as well. @@ -398,7 +463,21 @@ 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 (sys.SpindlePower){ + if (setSpindleSpeed(sNumber)){ + sys.SpindleSpeed = sNumber; + } + }else{ + Serial.println("spindle speed not set when spindle is off"); + } +} void executeGcodeLine(const String& gcodeLine){ /* @@ -566,6 +645,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 +742,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 +865,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 +875,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..e504d76a 100644 --- a/cnc_ctrl_v1/GCode.h +++ b/cnc_ctrl_v1/GCode.h @@ -25,6 +25,8 @@ Copyright 2014-2017 Bar Smith*/ extern String readyCommandString; //next command queued up and ready to send extern String gcodeLine; //The next individual line of gcode (for example G91 G01 X19 would be run as two lines) +extern int SpindlePowerControlPin; + void initGCode(); void gcodeExecuteLoop(); void readSerialCommands(); @@ -34,6 +36,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&); @@ -44,7 +47,7 @@ void G4(const String&); void G10(const String&); void G38(const String&); void setInchesToMillimetersConversion(float); -extern int SpindlePowerControlPin; + extern int ProbePin; extern int FAKE_SERVO_STATE; diff --git a/cnc_ctrl_v1/Kinematics.cpp b/cnc_ctrl_v1/Kinematics.cpp index ed2b3d97..c0162884 100644 --- a/cnc_ctrl_v1/Kinematics.cpp +++ b/cnc_ctrl_v1/Kinematics.cpp @@ -98,6 +98,8 @@ void Kinematics::_inverse(float xTarget,float yTarget, float* aChainLength, flo void Kinematics::_quadrilateralInverse(float xTarget,float yTarget, float* aChainLength, float* bChainLength){ + //Confirm that the coordinates are on the wood + _verifyValidTarget(&xTarget, &yTarget); // OK //coordinate shift to put (0,0) in the center of the plywood from the left sprocket y = (halfHeight) + sysSettings.motorOffsetY - yTarget; x = (sysSettings.distBetweenMotors/2.0) + xTarget; diff --git a/cnc_ctrl_v1/Kinematics.h b/cnc_ctrl_v1/Kinematics.h index 64fe6356..bfdeb8b3 100644 --- a/cnc_ctrl_v1/Kinematics.h +++ b/cnc_ctrl_v1/Kinematics.h @@ -50,7 +50,8 @@ float _YOffsetEqn(const float& YPlus, const float& Denominator, const float& Psi); void _MatSolv(); void _MyTrig(); - float x = 0; //target router bit coordinates. + //target router bit coordinates. + float x = 0; float y = 0; float _xCordOfMotor; float _yCordOfMotor; diff --git a/cnc_ctrl_v1/Maslow.h b/cnc_ctrl_v1/Maslow.h index 46104a7d..0ff1da08 100644 --- a/cnc_ctrl_v1/Maslow.h +++ b/cnc_ctrl_v1/Maslow.h @@ -18,7 +18,7 @@ #define maslow_h // Maslow Firmware Version tracking -#define VERSIONNUMBER 51.28 +#define VERSIONNUMBER 51.29 // Define standard libraries used by maslow. #include 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/Motor.cpp b/cnc_ctrl_v1/Motor.cpp index 245694ec..76adb4eb 100644 --- a/cnc_ctrl_v1/Motor.cpp +++ b/cnc_ctrl_v1/Motor.cpp @@ -48,6 +48,15 @@ int Motor::setupMotor(const int& pwmPin, const int& pin1, const int& pin2){ digitalWrite(_pin1, LOW); digitalWrite(_pin2, LOW) ; + }else if (TB6643 == true ) { + // EBS V1.5 + pinMode(_pwmPin, INPUT); + pinMode(_pin1, OUTPUT); + pinMode(_pin2, OUTPUT); + + //stop the motor + digitalWrite(_pin1, LOW); + digitalWrite(_pin2, LOW); } else if (TLE9201 == true) { //set pinmodes pinMode(_pwmPin, OUTPUT); @@ -81,7 +90,11 @@ void Motor::detach(){ //stop the motor digitalWrite(_pin1, LOW); digitalWrite(_pin2, LOW) ; - } else if (TLE9201 == true) { + }else if (TB6643 == true){ + //stop the motor + digitalWrite(_pin1, LOW); + digitalWrite(_pin2, LOW) ; + } else if (TLE9201 == true) { //stop the motor digitalWrite(_pin2, HIGH); // TLE9201 ENABLE pin, LOW = on analogWrite(_pwmPin, 0); @@ -122,7 +135,7 @@ void Motor::write(int speed, bool force){ bool usePin1 = ((_pin1 != 4) && (_pin1 != 13) && (_pin1 != 11) && (_pin1 != 12)); // avoid PWM using timer0 or timer1 bool usePin2 = ((_pin2 != 4) && (_pin2 != 13) && (_pin2 != 11) && (_pin2 != 12)); // avoid PWM using timer0 or timer1 bool usepwmPin = ((TLE5206 == false) && (_pwmPin != 4) && (_pwmPin != 13) && (_pwmPin != 11) && (_pwmPin != 12)); // avoid PWM using timer0 or timer1 - if (!(TLE5206 || TLE9201)) { // L298 boards + if (!(TLE5206 || TLE9201)) { // NOT L298 boards if (forward){ if (usepwmPin){ digitalWrite(_pin1 , HIGH ); @@ -228,6 +241,19 @@ void Motor::write(int speed, bool force){ digitalWrite(dirPin, dirCMD); // TLE9201 DIR pin analogWrite (_pwmPin, speed); // TLE9201 PWM pin digitalWrite(enablePin, LOW); // TLE9201 ENABLE pin, HIGH = disable + }else if (TB6643) { //EBS 1.5 + if (forward) { + if (speed > 0) { + analogWrite(_pin1 , speed); + digitalWrite(_pin2 , LOW); + } else { // speed = 0 so put on the brakes + digitalWrite(_pin1 , HIGH); + digitalWrite(_pin2 , HIGH); + } + } else { // reverse + digitalWrite(_pin1 , LOW); + analogWrite(_pin2 , speed); + } } else {} // add new boards here } diff --git a/cnc_ctrl_v1/Motor.h b/cnc_ctrl_v1/Motor.h index f75e0e71..8ad61203 100644 --- a/cnc_ctrl_v1/Motor.h +++ b/cnc_ctrl_v1/Motor.h @@ -51,6 +51,7 @@ }; extern bool TLE5206; extern bool TLE9201; + extern bool TB6643; extern int ENA; extern int ENB; extern int ENC; diff --git a/cnc_ctrl_v1/Settings.cpp b/cnc_ctrl_v1/Settings.cpp index c92fd9b3..431cc960 100644 --- a/cnc_ctrl_v1/Settings.cpp +++ b/cnc_ctrl_v1/Settings.cpp @@ -63,54 +63,56 @@ void settingsReset() { so that if a value is not changed by a user or is not used, it doesn't need to be updated here. */ - sysSettings.machineWidth = 2438.4; // float machineWidth; - sysSettings.machineHeight = 1219.2; // float machineHeight; - sysSettings.distBetweenMotors = 2978.4; // float distBetweenMotors; - sysSettings.motorOffsetY = 463.0; // float motorOffsetY; - sysSettings.sledWidth = 310.0; // float sledWidth; - sysSettings.sledHeight = 139.0; // float sledHeight; - sysSettings.sledCG = 79.0; // float sledCG; - sysSettings.kinematicsType = 1; // byte kinematicsType; - sysSettings.rotationDiskRadius = 250.0; // float rotationDiskRadius; - sysSettings.axisDetachTime = 2000; // int axisDetachTime; - sysSettings.chainLength = 3360; // int maximum length of chain; - sysSettings.originalChainLength = 1651; // int originalChainLength; - sysSettings.encoderSteps = 8113.73; // float encoderSteps; - sysSettings.distPerRot = 63.5; // float distPerRot; - sysSettings.maxFeed = 700; // int maxFeed; - sysSettings.zAxisAttached = true; // zAxisAttached; - sysSettings.spindleAutomateType = NONE; // bool spindleAutomate; - sysSettings.maxZRPM = 12.60; // float maxZRPM; - sysSettings.zDistPerRot = 3.17; // float zDistPerRot; - sysSettings.zEncoderSteps = 7560.0; // float zEncoderSteps; - sysSettings.KpPos = 1300.0; // float KpPos; - sysSettings.KiPos = 0.0; // float KiPos; - sysSettings.KdPos = 34.0; // float KdPos; - sysSettings.propWeightPos = 1.0; // float propWeightPos; - sysSettings.KpV = 5.0; // float KpV; - sysSettings.KiV = 0.0; // float KiV; - sysSettings.KdV = 0.28; // float KdV; - sysSettings.propWeightV = 1.0; // float propWeightV; - sysSettings.zKdPos = 1300.0; // float zKpPos; - sysSettings.zKiPos = 0.0; // float zKiPos; - sysSettings.zKdPos = 34.0; // float zKdPos; - sysSettings.zPropWeightPos = 1.0; // float zPropWeightPos; - sysSettings.zKpV = 5.0; // float zKpV; - sysSettings.zKiV = 0.0; // float zKiV; - sysSettings.zKdV = 0.28; // float zKdV; - sysSettings.zPropWeightV = 1.0; // float zPropWeightV; - sysSettings.chainSagCorrection = 0.0; // float chainSagCorrection; - sysSettings.chainOverSprocket = 1; // byte chainOverSprocket; - sysSettings.fPWM = 3; // byte fPWM; - 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.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.eepromValidData = EEPROMVALIDDATA; // byte eepromValidData; -} + sysSettings.machineWidth = 2438.4; // float machineWidth; 4 + sysSettings.machineHeight = 1219.2; // float machineHeight; 4 + sysSettings.distBetweenMotors = 2978.4; // float distBetweenMotors; 4 + sysSettings.motorOffsetY = 463.0; // float motorOffsetY; 4 + sysSettings.sledWidth = 310.0; // float sledWidth; 4 + sysSettings.sledHeight = 139.0; // float sledHeight; 4 + sysSettings.sledCG = 79.0; // float sledCG; 4 - + sysSettings.kinematicsType = 1; // byte kinematicsType; 1 + sysSettings.rotationDiskRadius = 250.0; // float rotationDiskRadius; 1 + sysSettings.axisDetachTime = 2000; // int axisDetachTime; 2 + sysSettings.chainLength = 3360; // int maximum length of chain; 2 + sysSettings.originalChainLength = 1651; // int originalChainLength; 2 + sysSettings.encoderSteps = 8113.73; // float encoderSteps; 4 + sysSettings.distPerRot = 63.5; // float distPerRot; 4 + sysSettings.maxFeed = 900; // int maxFeed; 2 + sysSettings.zAxisAttached = true; // zAxisAttached; 1 + sysSettings.spindleAutomateType = SPEED_CONTROL_RELAY_ACTIVE_HIGH; // 1 bool spindleAutomate; + sysSettings.maxZRPM = 12.60; // float maxZRPM; 4 + sysSettings.zDistPerRot = 3.17; // float zDistPerRot; 4 + sysSettings.zEncoderSteps = 7560.0; // float zEncoderSteps; 4 + sysSettings.KpPos = 1300.0; // float KpPos; 4 - + sysSettings.KiPos = 0.0; // float KiPos; 4 + sysSettings.KdPos = 34.0; // float KdPos; 4 + sysSettings.propWeightPos = 1.0; // float propWeightPos; 4 + sysSettings.KpV = 5.0; // float KpV; 4 + sysSettings.KiV = 0.0; // float KiV; 4 + sysSettings.KdV = 0.28; // float KdV; 4 - + sysSettings.propWeightV = 1.0; // float propWeightV; 4 + sysSettings.zKdPos = 1300.0; // float zKpPos; 4 + sysSettings.zKiPos = 0.0; // float zKiPos; 4 + sysSettings.zKdPos = 34.0; // float zKdPos; 4 + sysSettings.zPropWeightPos = 1.0; // float zPropWeightPos; 4 + sysSettings.zKpV = 5.0; // float zKpV; 4 - + sysSettings.zKiV = 0.0; // float zKiV; 4 + sysSettings.zKdV = 0.28; // float zKdV; 4 + sysSettings.zPropWeightV = 1.0; // float zPropWeightV; 4 + sysSettings.chainSagCorrection = 0.0; // float chainSagCorrection; 4 + sysSettings.chainOverSprocket = 1; // byte chainOverSprocket; 1 + sysSettings.fPWM = 3; // byte fPWM; 1 + sysSettings.leftChainTolerance = 0.0; // float leftChainTolerance; 4 + sysSettings.rightChainTolerance = 0.0; // float rightChainTolerance;4 - + sysSettings.positionErrorLimit = 2.0; // float positionErrorLimit; 4 + sysSettings.zAxisUpperLimit = NAN; // float zAxisUpperLimit 4 + sysSettings.zAxisLowerLimit = NAN; // float zAxisLowerLimit 4 + sysSettings.chainElongationFactor = 8.1E-6; // m/m/N 4 + sysSettings.sledWeight = 11.6*9.8; // Newtons. For a sled with one ring4 kit, one Rigid 2200 router and two 2.35kg bricks on a 5/8" thick mdf 18" diameter base. + sysSettings.spindleMin = 4000; // 2 + sysSettings.spindleMax = 24000; // 2 + sysSettings.eepromValidData = EEPROMVALIDDATA; // byte eepromValidData;1 +} // 36 * 4 bytes, 6* 2 bytes, 7* 1 byte = 144+12+7= 163 bytes void settingsWipe(byte resetType){ /* @@ -429,6 +431,14 @@ byte settingsStoreGlobalSetting(const byte& parameter,const float& value){ sysSettings.sledWeight = value; kinematics.init();; break; + case 60: + sysSettings.spindleMin = (int)value; + Serial.print(F("\r\nnew spindle min="));Serial.print(sysSettings.spindleMin); + break; + case 61: + sysSettings.spindleMax = (int)value; + Serial.print(F("\r\nnew spindle max="));Serial.print(sysSettings.spindleMax); + break; default: return(STATUS_INVALID_STATEMENT); } diff --git a/cnc_ctrl_v1/Settings.h b/cnc_ctrl_v1/Settings.h index 29fb3944..5ae48609 100644 --- a/cnc_ctrl_v1/Settings.h +++ b/cnc_ctrl_v1/Settings.h @@ -20,7 +20,7 @@ Copyright 2014-2017 Bar Smith*/ #ifndef settings_h #define settings_h -#define SETTINGSVERSION 6 // The current version of settings, if this doesn't +#define SETTINGSVERSION 7 // The current version of settings, if this doesn't // match what is in EEPROM then settings on // machine are reset to defaults #define EEPROMVALIDDATA 56 // This is just a random byte value that is used @@ -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; @@ -85,6 +87,10 @@ typedef struct { // I think this is about ~128 bytes in size if I counted corre 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; 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..c8a198b2 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) { /* @@ -29,18 +31,21 @@ void setSpindlePower(bool powerState) { SpindleAutomationType spindleAutomateType = sysSettings.spindleAutomateType; int delayAfterChange = 1000; // milliseconds - int servoIdle = 90; // degrees - int servoOn = 180; // degrees - int servoOff = 0; // degrees - int servoDelay = 2000; // milliseconds - + // Now for the main code #if defined (verboseDebug) && verboseDebug > 1 Serial.print(F("Spindle control uses pin ")); - Serial.print(SpindlePowerControlPin); + Serial.println(SpindlePowerControlPin); Serial.print(F("Spindle automation type ")); - Serial.print(spindleAutomateType); + Serial.println(spindleAutomateType); #endif + #ifndef SPINDLE_SPEED + + int servoIdle = 90; // degrees + int servoOn = 180; // degrees + int servoOff = 0; // degrees + int servoDelay = 2000; // milliseconds + if (spindleAutomateType == SERVO) { // use a servo to control a standard wall switch #if defined (verboseDebug) && verboseDebug > 1 Serial.print(F(" with servo (idle=")); @@ -54,6 +59,8 @@ void setSpindlePower(bool powerState) { myservo.attach(SpindlePowerControlPin); // start servo control myservo.write(servoIdle); // move servo to idle position maslowDelay(servoDelay); // wait for move to complete + sys.SpindlePower = 0; + sys.SpindleSPeed = 0; if(sys.stop){return;} if (powerState) { // turn on spindle Serial.println(F("Turning Spindle On")); @@ -67,10 +74,53 @@ void setSpindlePower(bool powerState) { if(sys.stop){return;} myservo.write(servoIdle); // return servo to idle position maslowDelay(servoDelay); // wait for move to complete + sys.SpindlePower = 1; + sys.SpindleSPeed = 0; 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); + sys.SpindlePower = 1; + } + else { // turn off spindle + Serial.println(F("Turning Spindle Off")); + digitalWrite(SpindlePowerControlPin, LOW); + digitalWrite(SpindleSpeedPin, LOW); + sys.SpindlePower = 0; + sys.SpindleSpeed = 0; + } + } + 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); + sys.SpindlePower = 1; + } + else { // turn off spindle + Serial.println(F("Turning Spindle Off")); + digitalWrite(SpindlePowerControlPin, HIGH); + digitalWrite(SpindleSpeedPin, LOW); + sys.SpindlePower = 0; + sys.SpindleSpeed = 0; + } + } + if (spindleAutomateType == RELAY_ACTIVE_HIGH) { #if defined (verboseDebug) && verboseDebug > 1 Serial.print(F(" as digital output, active high")); #endif @@ -78,10 +128,14 @@ void setSpindlePower(bool powerState) { if (powerState) { // turn on spindle Serial.println(F("Turning Spindle On")); digitalWrite(SpindlePowerControlPin, HIGH); + sys.SpindlePower = 1; + sys.SpindleSpeed = 0; } else { // turn off spindle Serial.println(F("Turning Spindle Off")); digitalWrite(SpindlePowerControlPin, LOW); + sys.SpindlePower = 0; + sys.SpindleSpeed = 0; } } else if (spindleAutomateType == RELAY_ACTIVE_LOW) { // use a digital I/O pin to control a relay @@ -92,10 +146,14 @@ void setSpindlePower(bool powerState) { if (powerState) { // turn on spindle Serial.println(F("Turning Spindle On")); digitalWrite(SpindlePowerControlPin, LOW); + sys.SpindlePower = 0; + sys.SpindleSpeed = 0; } else { // turn off spindle Serial.println(F("Turning Spindle Off")); digitalWrite(SpindlePowerControlPin, HIGH); + sys.SpindlePower = 0; + sys.SpindleSpeed = 0; } } if (spindleAutomateType != NONE) { @@ -103,6 +161,40 @@ void setSpindlePower(bool powerState) { } } +int setSpindleSpeed(int spindleSpd){ + // map 0-255 min spindle speed to max spindle speed + // output 0-255 + if (spindleSpd == 0){ + digitalWrite(SpindleSpeedPin,LOW); + sys.SpindleSpeed = 0; + } + else{ + if(spindleSpd < sysSettings.spindleMin){ + spindleSpd = sysSettings.spindleMin; + } + if (spindleSpd > sysSettings.spindleMax){ + spindleSpd = sysSettings.spindleMax; + } + sys.SpindleSpeed = spindleSpd; + Serial.print(F("spindle max = "));Serial.println(sysSettings.spindleMax); + Serial.print(F("spindle min = "));Serial.println(sysSettings.spindleMin); + Serial.print(F("new spindle speed = "));Serial.println(spindleSpd); + int spd = constrain(map (spindleSpd, 0, sysSettings.spindleMax, 0, 255),0,255); + Serial.print(F("new pwm output = "));Serial.println(spd); + if (sys.SpindlePower == 1){ + analogWrite(SpindleSpeedPin,spd); + return(1); + }else{ + digitalWrite(SpindleSpeedPin,LOW); + return(0); + } + } + //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); } @@ -386,20 +421,29 @@ int getPCBVersion(){ * but the board version number silkscreened on those boards * and reported by the firmware was zero-based - a source of * confusion when creating new boards. -* Beginning with board v1.6, the value from the gpio pins for new boards -* will be zero-based to match the number reported by the firmware +* Beginning with board v1.8, the value from the gpio pins for new boards +* will be zero based to match the number reported by the firmware * and the silkscreen. -* +* * * * "x" = not used * #53-#52 #27-#26 #25-#24 #23-#22 * ------- ------- ------- ------- -* GND GND PU PU PU PU GND PU -> rev.0001 PCB v1.0 aka beta release board -* GND GND PU PU PU PU PU GND -> rev.0002 PCB v1.1 -* GND GND PU PU PU PU PU PU -> rev.0003 PCB v1.2 -* x x x x GND PU GND GND -> PCB v1.3 and 1.4 TLE5206 -* x x GND GND GND PU GND PU -> reserved for v1.4 TLE5206, v1.5 is unused -* x x GND GND GND PU PU GND -> PCB v1.6 TLE9201 +* 128 64 32 16 8 4 2 1 binary math values. add if value is PU +* GND GND PU PU PU PU GND PU -> rev.0001 PCB v1.0 aka beta release board = 61 +* GND GND PU PU PU PU PU GND -> rev.0002 PCB v1.1 = 62 +* GND GND PU PU PU PU PU PU -> rev.0003 PCB v1.2 = 63 +* X X X X PU PU PU PU -> rev 0.000 PCB v1.2b = 15 -silk screened as 1.2b with all 4 pins direct connected to 5V and all disconnected pins if set to pullup will read high +* x x x x GND PU GND GND -> PCB v1.3 and 1.4 TLE5206 = 4 +* x x GND GND GND PU GND PU -> reserved for v1.4 TLE5206 = 5 + X x GND GND GND PU PU GND -> PCB v1.5b is TB6643 = 6 +* x x GND GND GND PU PU [PU] -> PCB v1.6 TLE9201 = 7 --> need to manually cut trace or pin and disconnect pin 22 (vers1 label) +* +* All above this line are 1 based. below this line will be zero based. 8 will = B001000 ^^^ was off by one. +* As of 11/2020, the next board will be version 1.8 and should have pin 25 pulled high with no connection and 22, 23, 24 pins all shorted to ground. +* +* X X PU GND GND GND -> RESERVED for next board = 8 +* X X PU PU PU PU -> ALREADy TAKEN AS 1.2B = 15 */ pinMode(VERS1,INPUT_PULLUP); pinMode(VERS2,INPUT_PULLUP); @@ -408,24 +452,41 @@ int getPCBVersion(){ pinMode(VERS5,INPUT_PULLUP); pinMode(VERS6,INPUT_PULLUP); int pinCheck = (32*digitalRead(VERS6) + 16*digitalRead(VERS5) + 8*digitalRead(VERS4) + 4*digitalRead(VERS3) + 2*digitalRead(VERS2) + 1*digitalRead(VERS1)); + // everything should be grounded except those pulled high for counting + // according to the above pincheck should result in , 1 = 000001, 2 = 000010, 3 = 000100, 4 = 001000, 5 = 010000, 6 = 100000; + // ideally switch (pinCheck) { // boards v1.1, v1.2, v1.3 don't strap VERS3-6 low - case B111101: case B111110: case B111111: // v1.1, v1.2, v1.3 + case B111101: case B111110: case B111111: case 001111:// v1.0, v1.1, v1.2, v1.3 pinCheck &= B000011; // strip off the unstrapped bits TLE5206 = false; TLE9201 = false; + TB6643 = false; break; - case B110100: case B000100: // some versions of board v1.4 don't strap VERS5-6 low - pinCheck &= B000111; // strip off the unstrapped bits + case B111100: case B000100: case B000101: // some versions of board v1.4 don't strap VERS5-6 low + pinCheck &= B000101; // strip off the unstrapped bits + TB6643 = false; TLE5206 = true; TLE9201 = false; break; - case B000110: + case B000110: // v 5 + pinCheck &= B000110; // this should be 101 for 5, not 110 for 6 all above are off by 1 + TB6643 = true; + TLE5206 = false; + TLE9201 = false; + break; + case B000111: //v 1.6 AND V 1.7 (1.7 is a place holder and will not exist because 1.6 uses the same pins) + pinCheck &= B000111; + TB6643 = false; TLE5206 = false; TLE9201 = true; break; + case B01000: // V8 RESERVED for board version 1.8 + pinCheck &= B001000; + break; } - return pinCheck<6 ? pinCheck-1 : pinCheck; + + return pinCheck<8 ? pinCheck-1 : pinCheck; } @@ -588,6 +649,8 @@ void systemReset(){ rightAxis.detach(); zAxis.detach(); setSpindlePower(false); + // TODO: define in this scope: + //LaserOff(); // Reruns the initial setup function and calls stop to re-init state sys.stop = true; setup(); @@ -748,6 +811,7 @@ byte systemExecuteCmdstring(String& cmdString){ // } // } else { // Store global setting. if(!readFloat(cmdString, char_counter, value)) { return(STATUS_BAD_NUMBER_FORMAT); } + Serial.print(F("the value received: "));Serial.println(value); if((cmdString[char_counter] != 0) || (parameter > 255)) { return(STATUS_INVALID_STATEMENT); } return(settingsStoreGlobalSetting((byte)parameter, value)); // } diff --git a/cnc_ctrl_v1/System.h b/cnc_ctrl_v1/System.h index 25bea189..8fa046b8 100644 --- a/cnc_ctrl_v1/System.h +++ b/cnc_ctrl_v1/System.h @@ -60,6 +60,8 @@ 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 + bool SpindlePower; // store the spindle power state + 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 @@ -76,7 +78,8 @@ extern Axis zAxis; extern maslowRingBuffer incSerialBuffer; extern Kinematics kinematics; extern byte systemRtExecAlarm; -extern int SpindlePowerControlPin; +extern int SpindlePowerControlPin; // relay for spindle power +extern int SpindleSpeedPin; // pwm output for spindle control extern int LaserPowerPin; extern int ProbePin; extern int SO1; @@ -96,4 +99,6 @@ void systemSaveAxesPosition(); void systemReset(); byte systemExecuteCmdstring(String&); void setPWMPrescalers(int prescalerChoice); +void configAuxLow(int A1, int A2, int A3, int A4, int A5, int A6); +void configAuxHigh(int A7, int A8, int A9); #endif diff --git a/cnc_ctrl_v1/cnc_ctrl_v1.ino b/cnc_ctrl_v1/cnc_ctrl_v1.ino index 4b171177..9f8379cc 100644 --- a/cnc_ctrl_v1/cnc_ctrl_v1.ino +++ b/cnc_ctrl_v1/cnc_ctrl_v1.ino @@ -1,8 +1,4 @@ -/*This file is part of the Maslow Control Software. - The Maslow Control Software is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. +/*This file is pa Maslow Control Software is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the @@ -33,6 +29,7 @@ // TLE9201 version // TLE5206 version +// EastBaySource TB6643 V1.5 #include "Maslow.h" #include @@ -64,6 +61,7 @@ void setup(){ Serial.print(getPCBVersion()); if (TLE5206 == true) { Serial.print(F(" TLE5206 ")); } if (TLE9201 == true) { Serial.print(F(" TLE9201 ")); } + if (TB6643 == true){Serial.print(F(" TB6643 "));} Serial.println(F(" Detected")); sys.inchesToMMConversion = 1; sys.writeStepsToEEPROM = false; @@ -71,7 +69,7 @@ void setup(){ if (FAKE_SERVO_STATE == FAKE_SERVO_PERMITTED) { // only this value is accepted Serial.println(F("FAKE_SERVO on")); // to turn this on } else { - Serial.println(F("FAKE_SERVO off")); // otherwise + Serial.println(F("FAKE_SERVO off")); // otherwise EEPROM[ FAKE_SERVO ] = 0; // force it to the 'off' value } settingsLoadFromEEprom(); @@ -115,6 +113,7 @@ void loop(){ if (sys.stop){ // only called on sys.stop to prevent stopping initMotion(); // on USB disconnect. Might consider removing setSpindlePower(false); // this restriction for safety if we are + laserOff(); } // comfortable that USB disconnects are // not a common occurrence anymore kinematics.init();