Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

Version 0.07b Released.

  • Loading branch information...
commit f71e2ae8426c99799cb1cac09e5675b543b31dce 1 parent df22984
@dewy721 authored
View
BIN  Downloads/EmcArduino_07b.zip
Binary file not shown
View
2  Instructions.txt
@@ -19,7 +19,7 @@ How to going in 7 easy steps:
6. Open a Terminal window and do the following:
copy the 9axis_arduino file to /usr/bin/ with something like:
- sudo cp ~/Downloads/EmcArduino_06b/9axis_arduino /usr/bin
+ sudo cp ~/Downloads/EmcArduino_07b/9axis_arduino /usr/bin
then make it executable with,
sudo chmod +x /usr/bin/9axis_arduino
View
6 README
@@ -7,5 +7,7 @@ components such as old steppers and computer parts to high-end linear servos.
Right now We're just beginning, accepting advice, contributions of code and skill
as well as learning too.
-Got ideas, tips, suggestions? Want to start your own branch here?
-Would you like to be part of our team? Just let us know.
+ Got ideas, tips, suggestions?
+ Would you like to be part of our team?
+ Just let us know.
+ http://emc2arduino.wordpress.com
View
152 pre-release/EmcArduino_07a/EmcArduino_07a.ino
@@ -42,7 +42,8 @@ Note concerning switches: Be smart!
#include <digitalWriteFast.h> // http://code.google.com/p/digitalwritefast/
#define BAUD (115200)
-#define MAX_BUF (64)
+#define VERSION "0.07a"
+#define ROLE "ALL-IN-ONE"
#define stepsPerInchX 3200
#define stepsPerInchY 3200
@@ -206,10 +207,14 @@ Note concerning switches: Be smart!
#define stepPin A7 // CNC Program step switch. Optional
// Spindle pin config
-#define spindleEnablePin -1 // Optional
-#define spindleDirection -1 // Optional
-#define spindleInverted false // Set to true if spindle runs in reverse.
-#define spindleTach -1 // Must use an interrupt pin. Optional. *NOT YET IMPLEMENTED* COMMING SOON.
+#define spindleEnablePin -1 // Optional
+#define spindleEnableInverted false // Set to true if you need +5v to activate.
+#define spindleDirection -1 // Optional
+#define spindleDirectionInverted false // Set to true if spindle runs in reverse.
+
+#define spindleTach -1 // Must be an interrupt pin. Optional.
+ // UNO can use pin 2 or 3.
+ // Mega2560 can use 2,3,18,19,20 or 21.
#define coolantMistPin -1 // Controls coolant mist pump. Optional
#define coolantFloodPin -1 // Controls coolant flood pump. Optional
@@ -332,7 +337,7 @@ boolean dirState8=true;
/////////////////////////////END OF USER SETTINGS///////////////////////////////
////////////////////////////////////////////////////////////////////////////////
-char buffer[MAX_BUF];
+char buffer[128];
int sofar;
float pos_x;
@@ -345,8 +350,12 @@ float pos_u;
float pos_v;
float pos_w;
+float revs_in=0;
+float spindleRPSin=0;
+
boolean stepState=LOW;
unsigned long stepTimeOld=0;
+unsigned long spindleTimeOld=0;
long stepper0Pos=0;
long stepper0Goto=0;
long stepper1Pos=0;
@@ -447,6 +456,30 @@ long divisor=1000000; // input divisor. Our HAL script wont send the six decimal
// So here we have to put the decimal back to get the real numbers.
// Used in: processCommand()
+boolean psuState=powerSupplyInverted;
+boolean spindleState=spindleDirectionInverted;
+
+float fbx=1;
+float fby=1;
+float fbz=1;
+float fba=1;
+float fbb=1;
+float fbc=1;
+float fbu=1;
+float fbv=1;
+float fbw=1;
+
+float fbxOld=0;
+float fbyOld=0;
+float fbzOld=0;
+float fbaOld=0;
+float fbbOld=0;
+float fbcOld=0;
+float fbuOld=0;
+float fbvOld=0;
+float fbwOld=0;
+
+
void jog(float x, float y, float z, float a, float b, float c, float u, float v, float w)
{
pos_x=x;
@@ -458,7 +491,6 @@ void jog(float x, float y, float z, float a, float b, float c, float u, float v,
pos_u=u;
pos_v=v;
pos_w=w;
-
// Handle our limit switches.
// Compressed to save visual space. Otherwise it would be several pages long!
@@ -546,6 +578,8 @@ void processCommand()
float vv=pos_v;
float ww=pos_w;
+ float ss=revs_in;
+
char *ptr=buffer;
while(ptr && ptr<buffer+sofar)
{
@@ -564,7 +598,7 @@ void processCommand()
case 'w': case 'W': ww=atof(ptr+1); ww=ww/divisor; break;
// Spindle speed command. In revs per second
- case 's': case 'S': ww=atof(ptr+1); ww=ww/divisor; break;
+ case 's': case 'S': ss=atof(ptr+1); spindleRPSin=ss; break;
default: ptr=0; break;
}
@@ -576,27 +610,6 @@ void processCommand()
}
}
-float fbx=1;
-float fby=1;
-float fbz=1;
-float fba=1;
-float fbb=1;
-float fbc=1;
-float fbu=1;
-float fbv=1;
-float fbw=1;
-
-float fbxOld=0;
-float fbyOld=0;
-float fbzOld=0;
-float fbaOld=0;
-float fbbOld=0;
-float fbcOld=0;
-float fbuOld=0;
-float fbvOld=0;
-float fbwOld=0;
-
-
void stepLight() // Set by jog() && Used by loop()
{
unsigned long curTime=micros();
@@ -665,11 +678,69 @@ void stepMode(int axis, int mode) // May be omitted in the future. (Undecided)
if(axis == 8 || 9){if(mode!=stepModeW){digitalWriteFast2(chanWms1,ms1);digitalWriteFast2(chanWms2,ms2);digitalWriteFast2(chanWms3,ms3);stepModeW=count;}}
}
-boolean psuState=powerSupplyInverted;
-boolean spindleState=spindleInverted;
+int determinInterrupt(int val)
+{
+ if(val<0) return -1;
+ if(val==2) return 0;
+ if(val==3) return 1;
+ if(val==18) return 5;
+ if(val==19) return 4;
+ if(val==20) return 3;
+ if(val==21) return 2;
+}
+
+volatile unsigned long spindleRevs=0;
+float spindleRPS=0;
+float spindleRPM=0;
+
+void countSpindleRevs()
+{
+ spindleRevs++;
+}
+
+float updateSpindleRevs()
+{
+ unsigned long spindleTime=millis();
+ if(spindleTime - spindleTimeOld >= 100)
+ {
+ spindleRPS=spindleRevs*10.0;
+ spindleRPM=spindleRPS*60.0;
+ spindleRevs=0;
+ }
+}
+
+boolean spindleEnabled=false;
+boolean spindleEnableState=spindleEnableInverted;
+
+boolean spindleAtSpeed()
+{
+ if(spindleTach>0)
+ {
+ if(spindleRPSin<spindleRPS)
+ {
+ if(spindleRPSin*1.05<spindleRPS || !spindleEnabled)
+ { /* Slow down. */
+ if(spindleEnablePin>0){digitalWriteFast2(spindleEnablePin,!spindleEnableState);}
+ }else{
+ if(spindleEnabled)
+ { /* Speed up. */
+ if(spindleEnablePin>0){digitalWriteFast2(spindleEnablePin,spindleEnableState);}
+ }
+ }
+ return true;
+ }else{
+ return false;
+ }
+ }else{
+ return spindleEnabled; // No tach? We'll fake it.
+ }
+}
void setup()
{
+ // If using a spindle tachometer, setup an interrupt for it.
+ if(spindleTach>0){int result=determinInterrupt(spindleTach);attachInterrupt(result,countSpindleRevs,FALLING);}
+
// Setup Min limit switches.
if(useRealMinX){pinMode(xMinPin,INPUT);if(!xMinPinInverted)digitalWriteFast2(xMinPin,HIGH);}
if(useRealMinY){pinMode(yMinPin,INPUT);if(!yMinPinInverted)digitalWriteFast2(yMinPin,HIGH);}
@@ -764,7 +835,6 @@ void setup()
if(coolantMistPin>0){pinMode(coolantMistPin,OUTPUT);digitalWriteFast2(coolantMistPin,LOW);}
if(coolantFloodPin>0){pinMode(coolantFloodPin,OUTPUT);digitalWriteFast2(coolantFloodPin,LOW);}
if(powerSupplyPin>0){pinMode(powerSupplyPin,OUTPUT);digitalWriteFast2(powerSupplyPin,psuState);}
- if(powerSupplyPin>0){pinMode(powerSupplyPin,OUTPUT);digitalWriteFast2(powerSupplyPin,spindleState);}
// Setup idle indicator led.
pinMode(idleIndicator,OUTPUT);
@@ -779,6 +849,8 @@ void setup()
sofar=0;
}
+boolean spindleAtSpeedStateOld;
+
void loop()
{
if(useEstopSwitch==true){boolean eStopState=digitalReadFast2(eStopPin);if(eStopPinInverted){eStopState=!eStopState;}if(eStopState != eStopStateOld || eStopStateOld){eStopStateOld=eStopState;Serial.print("e");Serial.println(eStopState);delay(500);}}
@@ -801,8 +873,8 @@ void loop()
if(sofar>0 && buffer[sofar-3]=='+') {
if(sofar>0 && buffer[sofar-2]=='P') { /* Power LED & PSU ON */ if(powerLedPin>0){digitalWriteFast2(powerLedPin,HIGH);}if(powerSupplyPin>0){digitalWriteFast2(powerSupplyPin,psuState);}}
if(sofar>0 && buffer[sofar-2]=='E') { /* E-Stop Indicator ON */ if(eStopLedPin>0){digitalWriteFast2(eStopLedPin,HIGH);}}
- if(sofar>0 && buffer[sofar-2]=='S') { /* Spindle power ON */ if(spindleEnablePin>0){digitalWriteFast2(spindleEnablePin,HIGH);}}
- if(sofar>0 && buffer[sofar-2]=='D') { /* Spindle direction ON */ if(spindleDirection>0){digitalWriteFast2(spindleDirection,spindleState);}}
+ if(sofar>0 && buffer[sofar-2]=='S') { /* Spindle power ON */ spindleEnabled=true;}
+ if(sofar>0 && buffer[sofar-2]=='D') { /* Spindle direction CW */ if(spindleDirection>0){digitalWriteFast2(spindleDirection,spindleState);}}
if(sofar>0 && buffer[sofar-2]=='M') { /* Coolant Mist ON */ if(coolantMistPin>0){digitalWriteFast2(coolantMistPin,HIGH);}}
if(sofar>0 && buffer[sofar-2]=='F') { /* Coolant Flood ON */ if(coolantFloodPin>0){digitalWriteFast2(coolantFloodPin,HIGH);}}
}
@@ -811,12 +883,18 @@ void loop()
if(sofar>0 && buffer[sofar-3]=='-') {
if(sofar>0 && buffer[sofar-2]=='P') { /* Power LED & PSU OFF */ if(powerLedPin>0){digitalWriteFast2(powerLedPin,LOW);}if(powerSupplyPin>0){digitalWriteFast2(powerSupplyPin,!psuState);}}
if(sofar>0 && buffer[sofar-2]=='E') { /* E-Stop Indicator OFF */ if(eStopLedPin>0){digitalWriteFast2(eStopLedPin,LOW);}}
- if(sofar>0 && buffer[sofar-2]=='S') { /* Spindle power OFF */ if(spindleEnablePin>0){digitalWriteFast2(spindleEnablePin,LOW);}}
- if(sofar>0 && buffer[sofar-2]=='D') { /* Spindle direction OFF */ if(spindleDirection>0){digitalWriteFast2(spindleDirection,!spindleState);}}
+ if(sofar>0 && buffer[sofar-2]=='S') { /* Spindle power OFF */ spindleEnabled=false;}
+ if(sofar>0 && buffer[sofar-2]=='D') { /* Spindle direction CCW */ if(spindleDirection>0){digitalWriteFast2(spindleDirection,!spindleState);}}
if(sofar>0 && buffer[sofar-2]=='M') { /* Coolant Mist OFF */ if(coolantMistPin>0){digitalWriteFast2(coolantMistPin,LOW);}}
if(sofar>0 && buffer[sofar-2]=='F') { /* Coolant Flood OFF */ if(coolantFloodPin>0){digitalWriteFast2(coolantFloodPin,LOW);}}
}
+ // Received a "?" about something give an answer.
+ if(sofar>0 && buffer[sofar-3]=='?') {
+ if(sofar>0 && buffer[sofar-2]=='V') { /* Report version */ Serial.println(VERSION);}
+ if(sofar>0 && buffer[sofar-2]=='R') { /* Report role */ Serial.println(ROLE);}
+ }
+
// if we hit a semi-colon, assume end of instruction.
if(sofar>0 && buffer[sofar-1]==';') {
@@ -828,5 +906,7 @@ void loop()
// reset the buffer
sofar=0;
}
+ updateSpindleRevs();
+ if(!globalBusy){boolean spindleAtSpeedState=spindleAtSpeed();if(spindleAtSpeedState != spindleAtSpeedStateOld){spindleAtSpeedStateOld=spindleAtSpeedState;Serial.print("S");Serial.println(spindleAtSpeedState);}}
stepLight(); // call every loop cycle to update stepper motion.
}
View
435 pre-release/EmcArduino_07b/9axis_arduino
@@ -0,0 +1,435 @@
+#!/usr/bin/python
+# HAL userspace component to interface with Arduino board
+# by Colin Kingsbury (http://ckcnc.wordpress.com_)
+# Inspired by the earlier example from Jeff Epler
+#
+# Modified by Duane Bishop for use with EMC-2-Arduino
+# (https://github.com/dewy721/EMC-2-Arduino)
+#
+# This program 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 2 of the License, or
+# (at your option) any later version.
+#
+# This program 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
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+import serial
+import hal
+import sys
+import time
+
+#First we open the serial port. This should correspond to the port the Arduino
+#is connected to. This can be found in the Arduino IDE in Tools->Serial Port
+PORT = "/dev/ttyACM0"
+ser = serial.Serial(PORT, 115200, timeout=15)
+
+#Now we create the HAL component and its pins
+c = hal.component("arduino")
+c.newpin("switch-on",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("switch-off",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("eStop",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("probe",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("spindleEnable",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("spindleDirection",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("spindleAtSpeed",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("coolantMist",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("coolantFlood",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("start",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("stop",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("pause",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("resume",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("step",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("machine-state",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("axis0-cmd",hal.HAL_FLOAT,hal.HAL_IN)
+c.newpin("axis1-cmd",hal.HAL_FLOAT,hal.HAL_IN)
+c.newpin("axis2-cmd",hal.HAL_FLOAT,hal.HAL_IN)
+c.newpin("axis3-cmd",hal.HAL_FLOAT,hal.HAL_IN)
+c.newpin("axis4-cmd",hal.HAL_FLOAT,hal.HAL_IN)
+c.newpin("axis5-cmd",hal.HAL_FLOAT,hal.HAL_IN)
+c.newpin("axis6-cmd",hal.HAL_FLOAT,hal.HAL_IN)
+c.newpin("axis7-cmd",hal.HAL_FLOAT,hal.HAL_IN)
+c.newpin("axis8-cmd",hal.HAL_FLOAT,hal.HAL_IN)
+c.newpin("xMinLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("yMinLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("zMinLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("aMinLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("bMinLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("cMinLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("uMinLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("vMinLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("wMinLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("xMaxLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("yMaxLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("zMaxLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("aMaxLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("bMaxLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("cMaxLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("uMaxLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("vMaxLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("wMaxLmt",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("xHome",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("yHome",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("zHome",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("aHome",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("bHome",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("cHome",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("uHome",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("vHome",hal.HAL_BIT,hal.HAL_IN)
+c.newpin("wHome",hal.HAL_BIT,hal.HAL_IN)
+time.sleep(2)
+c.ready()
+
+#We save the machine state (i.e. whether it's off or on) so that we only
+#send a message to the Arduino when it changes
+machineState = c['machine-state']
+eStopState = c['eStop']
+axis0cmd = c['axis0-cmd']
+axis1cmd = c['axis1-cmd']
+axis2cmd = c['axis2-cmd']
+axis3cmd = c['axis3-cmd']
+axis4cmd = c['axis4-cmd']
+axis4cmd = c['axis5-cmd']
+axis6cmd = c['axis6-cmd']
+axis7cmd = c['axis7-cmd']
+axis8cmd = c['axis8-cmd']
+
+#Check if the machine is on and set the LED accordingly
+if(machineState != 1):
+ ser.write("+P")
+
+axis0cmdOld = 0;
+axis1cmdOld = 0;
+axis2cmdOld = 0;
+axis3cmdOld = 0;
+axis4cmdOld = 0;
+axis5cmdOld = 0;
+axis6cmdOld = 0;
+axis7cmdOld = 0;
+axis8cmdOld = 0;
+coolantMistOld = 0;
+coolantFloodOld = 0;
+spindleEnableOld = 0;
+spindleDirectionOld = 0;
+
+try:
+ while 1:
+ time.sleep(.01)
+
+ axis0cmd = c['axis0-cmd'];
+ if axis0cmd != axis0cmdOld:
+ axis0cmdOld = axis0cmd;
+ axis0icmd = c['axis0-cmd'] * 1000000
+ ser.write("jog X");
+ ser.write(repr(axis0icmd))
+ ser.write(";");
+ ser.write("\n");
+
+ axis1cmd = c['axis1-cmd'];
+ if axis1cmd != axis1cmdOld:
+ axis1cmdOld = axis1cmd;
+ axis1icmd = c['axis1-cmd'] * 1000000
+ ser.write("jog Y");
+ ser.write(repr(axis1icmd))
+ ser.write(";");
+ ser.write("\n");
+
+ axis2cmd = c['axis2-cmd'];
+ if axis2cmd != axis2cmdOld:
+ axis2cmdOld = axis2cmd;
+ axis2icmd = c['axis2-cmd'] * 1000000
+ ser.write("jog Z");
+ ser.write(repr(axis2icmd))
+ ser.write(";");
+ ser.write("\n");
+
+ axis3cmd = c['axis3-cmd'];
+ if axis3cmd != axis3cmdOld:
+ axis3cmdOld = axis3cmd;
+ axis3icmd = c['axis3-cmd'] * 1000000
+ ser.write("jog A");
+ ser.write(repr(axis3icmd))
+ ser.write(";");
+ ser.write("\n");
+
+ axis4cmd = c['axis4-cmd'];
+ if axis4cmd != axis4cmdOld:
+ axis4cmdOld = axis4cmd;
+ axis4icmd = c['axis4-cmd'] * 1000000
+ ser.write("jog B");
+ ser.write(repr(axis4icmd))
+ ser.write(";");
+ ser.write("\n");
+
+ axis5cmd = c['axis5-cmd'];
+ if axis5cmd != axis5cmdOld:
+ axis5cmdOld = axis5cmd;
+ axis5icmd = c['axis5-cmd'] * 1000000
+ ser.write("jog C");
+ ser.write(repr(axis5icmd))
+ ser.write(";");
+ ser.write("\n");
+
+ axis6cmd = c['axis6-cmd'];
+ if axis6cmd != axis6cmdOld:
+ axis6cmdOld = axis6cmd;
+ axis6icmd = c['axis6-cmd'] * 1000000
+ ser.write("jog U");
+ ser.write(repr(axis6icmd))
+ ser.write(";");
+ ser.write("\n");
+
+ axis7cmd = c['axis7-cmd'];
+ if axis7cmd != axis7cmdOld:
+ axis7cmdOld = axis7cmd;
+ axis7icmd = c['axis7-cmd'] * 1000000
+ ser.write("jog V");
+ ser.write(repr(axis7icmd))
+ ser.write(";");
+ ser.write("\n");
+
+ axis8cmd = c['axis8-cmd'];
+ if axis8cmd != axis8cmdOld:
+ axis8cmdOld = axis8cmd;
+ axis8icmd = c['axis8-cmd'] * 1000000
+ ser.write("jog W");
+ ser.write(repr(axis8icmd))
+ ser.write(";");
+ ser.write("\n");
+
+ spindleEnable = c['spindleEnable'];
+ if spindleEnable != spindleEnableOld:
+ spindleEnableOld = spindleEnable;
+ if spindleEnable == 1:
+ ser.write("+S;");
+ else:
+ ser.write("-S;");
+
+ spindleDirection = c['spindleDirection'];
+ if spindleDirection != spindleDirectionOld:
+ spindleDirectionOld = spindleDirection;
+ if spindleDirection == 1:
+ ser.write("+D;");
+ else:
+ ser.write("-D;");
+
+
+
+ coolantMist = c['coolantMist'];
+ if coolantMist != coolantMistOld:
+ coolantMistOld = coolantMist;
+ if coolantMist == 1:
+ ser.write("+M;");
+ else:
+ ser.write("-M;");
+
+ coolantFlood = c['coolantFlood'];
+ if coolantFlood != coolantFloodOld:
+ coolantFloodOld = coolantFlood;
+ if coolantFlood == 1:
+ ser.write("+F;");
+ else:
+ ser.write("-F;");
+
+ #Check the machine State
+ if(machineState != c['machine-state']):
+ if(c['machine-state'] == 1):
+ #The machine is on, so turn on the power LED
+ ser.write("+P;")
+ else:
+ #opposite of above
+ ser.write("-P;")
+ #update the machine state variable
+ machineState = c['machine-state']
+ #Check to see if we have a message waiting from the Arduino
+ while ser.inWaiting():
+ #This should be set to the length of whatever fixed-length message
+ #you're sending from the arduino. It does not have to be the same length
+ #as the outbound messages.
+ key = ser.read(2)
+ #The Arduino generates two different key events
+ #One when the key is pressed down (+S) and another when it is released (-S)
+ #In this case we are going to ignore the release
+
+# Set min limit triggers
+ if(key == "x0"):
+ c['xMinLmt'] = 1
+ if(key == "y0"):
+ c['yMinLmt'] = 1
+ if(key == "z0"):
+ c['zMinLmt'] = 1
+ if(key == "a0"):
+ c['aMinLmt'] = 1
+ if(key == "x0"):
+ c['bMinLmt'] = 1
+ if(key == "y0"):
+ c['cMinLmt'] = 1
+ if(key == "z0"):
+ c['uMinLmt'] = 1
+ if(key == "a0"):
+ c['vMinLmt'] = 1
+ if(key == "a0"):
+ c['wMinLmt'] = 1
+
+# Clear limit triggers
+ if(key == "x1"):
+ c['xMinLmt'] = 0
+ c['xMaxLmt'] = 0
+ if(key == "y1"):
+ c['yMinLmt'] = 0
+ c['yMaxLmt'] = 0
+ if(key == "z1"):
+ c['zMinLmt'] = 0
+ c['zMaxLmt'] = 0
+ if(key == "a1"):
+ c['aMinLmt'] = 0
+ c['aMaxLmt'] = 0
+ if(key == "b1"):
+ c['bMinLmt'] = 0
+ c['bMaxLmt'] = 0
+ if(key == "c1"):
+ c['cMinLmt'] = 0
+ c['cMaxLmt'] = 0
+ if(key == "u1"):
+ c['uMinLmt'] = 0
+ c['uMaxLmt'] = 0
+ if(key == "v1"):
+ c['vMinLmt'] = 0
+ c['vMaxLmt'] = 0
+ if(key == "w1"):
+ c['wMinLmt'] = 0
+ c['wMaxLmt'] = 0
+
+# Set max limit triggers
+ if(key == "x2"):
+ c['xMaxLmt'] = 1
+ if(key == "y2"):
+ c['yMaxLmt'] = 1
+ if(key == "z2"):
+ c['zMaxLmt'] = 1
+ if(key == "a2"):
+ c['aMaxLmt'] = 1
+ if(key == "b2"):
+ c['bMaxLmt'] = 1
+ if(key == "c2"):
+ c['cMaxLmt'] = 1
+ if(key == "u2"):
+ c['uMaxLmt'] = 1
+ if(key == "v2"):
+ c['vMaxLmt'] = 1
+ if(key == "w2"):
+ c['wMaxLmt'] = 1
+
+# set home switches
+ if(key == "x4"):
+ c['xHome'] = 0
+ if(key == "y4"):
+ c['yHome'] = 0
+ if(key == "z4"):
+ c['zHome'] = 0
+ if(key == "a4"):
+ c['aHome'] = 0
+ if(key == "b4"):
+ c['bHome'] = 0
+ if(key == "c4"):
+ c['cHome'] = 0
+ if(key == "u4"):
+ c['uHome'] = 0
+ if(key == "v4"):
+ c['vHome'] = 0
+ if(key == "w4"):
+ c['wHome'] = 0
+
+# unset home switches
+ if(key == "x5"):
+ c['xHome'] = 1
+ if(key == "y5"):
+ c['yHome'] = 1
+ if(key == "z5"):
+ c['zHome'] = 1
+ if(key == "a5"):
+ c['aHome'] = 1
+ if(key == "b5"):
+ c['bHome'] = 1
+ if(key == "c5"):
+ c['cHome'] = 1
+ if(key == "u5"):
+ c['uHome'] = 1
+ if(key == "v5"):
+ c['vHome'] = 1
+ if(key == "w5"):
+ c['wHome'] = 1
+
+#power
+ if(key == "p0"):
+ c['switch-on'] = 0
+ c['switch-off'] = 1
+ if(key == "p1"):
+ c['switch-on'] = 1
+ c['switch-off'] = 0
+ if(key == "pt"):
+ #If the machine is currently on, we turn it off, and vice-versa
+ if(machineState == 1):
+ c['switch-on'] = 1
+ c['switch-off'] = 0
+ else:
+ c['switch-on'] = 0
+ c['switch-off'] = 1
+
+#eStop
+ if(key == "e0"):
+ c['eStop'] = 1
+ if(key == "e1"):
+ c['eStop'] = 0
+
+#probe
+ if(key == "P0"):
+ c['probe'] = 1
+ if(key == "P1"):
+ c['probe'] = 0
+
+#program start
+ if(key == "h0"):
+ c['start'] = 0
+ if(key == "h1"):
+ c['start'] = 1
+
+#program stop
+ if(key == "h2"):
+ c['stop'] = 0
+ if(key == "h3"):
+ c['stop'] = 1
+
+#program pause
+ if(key == "h4"):
+ c['pause'] = 1
+ if(key == "h5"):
+ c['pause'] = 0
+
+#program resume
+ if(key == "h6"):
+ c['resume'] = 1
+ if(key == "h7"):
+ c['resume'] = 0
+
+#program step
+ if(key == "h8"):
+ c['step'] = 1
+ if(key == "h9"):
+ c['step'] = 0
+
+#spindle at speed signal
+ if(key == "S1"):
+ c['spindleAtSpeed'] = 1
+ if(key == "S0"):
+ c['spindleAtSpeed'] = 0
+
+except KeyboardInterrupt:
+ ser.write("-P;");
+ raise SystemExit
+
View
914 pre-release/EmcArduino_07b/EmcArduino_07b.ino
@@ -0,0 +1,914 @@
+/*
+This work is public domain.
+
+Please note: Although there are a LOT pin settings here.
+You can get by with as few as TWO pins per Axis. (dir & step)
+ie: 3 axies = 6 pins used. (minimum)
+ 9 axies = 18 pins or an entire UNO (using virtual limits switches only)
+
+Note concerning switches: Be smart!
+ AT LEAST use HOME switches.
+ Switches are cheap insurance.
+ You'll find life a lot easier if you use them entirely.
+
+ If you choose to build with threaded rod for lead screws but leave out the switches
+ You'll have one of two possible outcomes;
+ You'll get tired really quickly of resetting the machine by hand.
+ Or worse, you'll forget (only once) to reset it, and upon homing
+ it WILL destroy itself while you go -> WTF!? -> OMG! -> PANIC! -> FACEPALM!
+
+ List of axies. All 9 of them.
+ AXIS_0 = X (Left/Right)
+ AXIS_1 = Y (Near/Far) Lathes use this for tool depth.
+ AXIS_2 = Z (Up/Down) Not typically used for lathes. Except lathe/mill combo.
+ AXIS_3 = A (Rotation parallel to X axis) lathe chuck.
+ AXIS_4 = B (Rotation parallel to Y axis)
+ AXIS_5 = C (Rotation parallel to Z axis)
+ AXIS_6 = U (Rotation perpendicular to X axis)
+ AXIS_7 = V (Rotation perpendicular to Y axis)
+ AXIS_8 = W (Rotation perpendicular to Z axis)
+
+ DYI robot builders: You can monitor/control this sketch via a serial interface.
+ Example commands:
+
+ jog x200;
+ jog x-215.25 y1200 z0.002 a5;
+
+ PS: If you choose to control this with your own interface then also modify the
+ divisor variable further down.
+*/
+
+// You'll need this library. Get the interrupt safe version.
+#include <digitalWriteFast.h> // http://code.google.com/p/digitalwritefast/
+
+#define BAUD (115200)
+
+// These will be used in the near future.
+#define VERSION "00072" // 5 caracters
+#define ROLE "ALL-IN-ONE" // 10 characters
+
+#define stepsPerInchX 3200
+#define stepsPerInchY 3200
+#define stepsPerInchZ 3200
+#define stepsPerInchA 3200
+#define stepsPerInchB 3200
+#define stepsPerInchC 3200
+#define stepsPerInchU 3200
+#define stepsPerInchV 3200
+#define stepsPerInchW 3200
+
+#define minStepTime 25 //delay in MICROseconds between step pulses.
+
+// step pins (required)
+#define stepPin0 41
+#define stepPin1 40
+#define stepPin2 35
+#define stepPin3 34
+#define stepPin4 -1
+#define stepPin5 -1
+#define stepPin6 -1
+#define stepPin7 -1
+#define stepPin8 -1
+
+// dir pins (required)
+#define dirPin0 43
+#define dirPin1 42
+#define dirPin2 33
+#define dirPin3 32
+#define dirPin4 -1
+#define dirPin5 -1
+#define dirPin6 -1
+#define dirPin7 -1
+#define dirPin8 -1
+
+// microStepping pins (optional)
+#define chanXms1 45
+#define chanXms2 47
+#define chanXms3 49
+#define chanYms1 44
+#define chanYms2 46
+#define chanYms3 48
+#define chanZms1 31
+#define chanZms2 29
+#define chanZms3 27
+#define chanAms1 30
+#define chanAms2 28
+#define chanAms3 26
+#define chanBms1 -1
+#define chanBms2 -1
+#define chanBms3 -1
+#define chanCms1 -1
+#define chanCms2 -1
+#define chanCms3 -1
+#define chanUms1 -1
+#define chanUms2 -1
+#define chanUms3 -1
+#define chanVms1 -1
+#define chanVms2 -1
+#define chanVms3 -1
+#define chanWms1 -1
+#define chanWms2 -1
+#define chanWms3 -1
+
+#define xEnablePin 38
+#define yEnablePin 39
+#define zEnablePin 37
+#define aEnablePin 36
+#define bEnablePin -1
+#define cEnablePin -1
+#define uEnablePin -1
+#define vEnablePin -1
+#define wEnablePin -1
+
+#define useEstopSwitch true
+#define usePowerSwitch true
+#define useProbe true
+#define useStartSwitch true
+#define useStopSwitch true
+#define usePauseSwitch true
+#define useResumeSwitch true
+#define useStepSwitch true
+
+// Set to true if your using real switches for MIN positions.
+#define useRealMinX false
+#define useRealMinY false
+#define useRealMinZ false
+#define useRealMinA false
+#define useRealMinB false
+#define useRealMinC false
+#define useRealMinU false
+#define useRealMinV false
+#define useRealMinW false
+
+// Set to true if your using real switches for HOME positions.
+#define useRealHomeX false
+#define useRealHomeY false
+#define useRealHomeZ false
+#define useRealHomeA false
+#define useRealHomeB false
+#define useRealHomeC false
+#define useRealHomeU false
+#define useRealHomeV false
+#define useRealHomeW false
+
+// Set to false if your using real switches for MAX positions.
+#define useRealMaxX false
+#define useRealMaxY false
+#define useRealMaxZ false
+#define useRealMaxA false
+#define useRealMaxB false
+#define useRealMaxC false
+#define useRealMaxU false
+#define useRealMaxV false
+#define useRealMaxW false
+
+// If your using REAL switches you'll need real pins (ignored if using Virtual switches).
+// -1 = not used.
+#define xMinPin -1
+#define yMinPin -1
+#define zMinPin -1
+#define aMinPin -1
+#define bMinPin -1
+#define cMinPin -1
+#define uMinPin -1
+#define vMinPin -1
+#define wMinPin -1
+
+#define xHomePin -1
+#define yHomePin -1
+#define zHomePin -1
+#define aHomePin -1
+#define bHomePin -1
+#define cHomePin -1
+#define uHomePin -1
+#define vHomePin -1
+#define wHomePin -1
+
+#define xMaxPin -1
+#define yMaxPin -1
+#define zMaxPin -1
+#define aMaxPin -1
+#define bMaxPin -1
+#define cMaxPin -1
+#define uMaxPin -1
+#define vMaxPin -1
+#define wMaxPin -1
+
+#define powerSwitchIsMomentary true // Set to true if your using a momentary switch.
+#define powerPin A0 // Power switch. Optional
+#define powerLedPin -1 // Power indicator. Optional
+
+#define eStopPin A1 // E-Stop switch. You really, REALLY should have this one.
+#define eStopLedPin -1 // E-Stop indicator. Optional
+
+#define probePin A2 // CNC Touch probe input. Optional
+#define startPin A3 // CNC Program start switch. Optional
+#define stopPin A4 // CNC Stop program switch. Optional
+#define pausePin A5 // CNC Pause program switch. Optional
+#define resumePin A6 // CNC Resume program switch. Optional
+#define stepPin A7 // CNC Program step switch. Optional
+
+// Spindle pin config
+#define spindleEnablePin -1 // Optional
+#define spindleEnableInverted false // Set to true if you need +5v to activate.
+#define spindleDirection -1 // Optional
+#define spindleDirectionInverted false // Set to true if spindle runs in reverse.
+
+#define spindleTach -1 // Must be an interrupt pin. Optional.
+ // UNO can use pin 2 or 3.
+ // Mega2560 can use 2,3,18,19,20 or 21.
+
+#define coolantMistPin -1 // Controls coolant mist pump. Optional
+#define coolantFloodPin -1 // Controls coolant flood pump. Optional
+#define powerSupplyPin -1 // Controls power supply ON/OFF. Optional
+#define powerSupplyInverted true // Set to "true" for +5v = ON
+
+// Signal inversion for real switch users. (false = ground trigger signal, true = +5vdc trigger signal.)
+// Note: Inverted switches will need pull-down resistors (less than 10kOhm) to lightly ground the signal wires.
+#define xMinPinInverted false
+#define yMinPinInverted false
+#define zMinPinInverted false
+#define aMinPinInverted false
+#define bMinPinInverted false
+#define cMinPinInverted false
+#define uMinPinInverted false
+#define vMinPinInverted false
+#define wMinPinInverted false
+
+#define xHomePinInverted false
+#define yHomePinInverted false
+#define zHomePinInverted false
+#define aHomePinInverted false
+#define bHomePinInverted false
+#define cHomePinInverted false
+#define uHomePinInverted false
+#define vHomePinInverted false
+#define wHomePinInverted false
+
+#define xMaxPinInverted false
+#define yMaxPinInverted false
+#define zMaxPinInverted false
+#define aMaxPinInverted false
+#define bMaxPinInverted false
+#define cMaxPinInverted false
+#define uMaxPinInverted false
+#define vMaxPinInverted false
+#define wMaxPinInverted false
+
+#define eStopPinInverted false
+#define powerPinInverted false
+#define probePinInverted false
+#define startPinInverted false
+#define stopPinInverted false
+#define pausePinInverted false
+#define resumePinInverted false
+#define stepPinInverted false
+
+// Where should the VIRTUAL Min switches be set to (ignored if using real switches).
+// Set to whatever you specified in the StepConf wizard.
+#define xMin -5.1
+#define yMin -5.1
+#define zMin -5.1
+#define aMin -5.1
+#define bMin -5.1
+#define cMin -5.1
+#define uMin -5.1
+#define vMin -5.1
+#define wMin -5.1
+
+// Where should the VIRTUAL home switches be set to (ignored if using real switches).
+// Set to whatever you specified in the StepConf wizard.
+#define xHome 0
+#define yHome 0
+#define zHome 0
+#define aHome 0
+#define bHome 0
+#define cHome 0
+#define uHome 0
+#define vHome 0
+#define wHome 0
+
+// Where should the VIRTUAL Max switches be set to (ignored if using real switches).
+// Set to whatever you specified in the StepConf wizard.
+#define xMax 15.1
+#define yMax 15.1
+#define zMax 15.1
+#define aMax 15.1
+#define bMax 15.1
+#define cMax 15.1
+#define uMax 15.1
+#define vMax 15.1
+#define wMax 15.1
+
+#define giveFeedBackX false
+#define giveFeedBackY false
+#define giveFeedBackZ false
+#define giveFeedBackA false
+#define giveFeedBackB false
+#define giveFeedBackC false
+#define giveFeedBackU false
+#define giveFeedBackV false
+#define giveFeedBackW false
+
+/*
+ This indicator led will let you know how hard you pushing the Arduino.
+
+ To test: Issue a G0 in the GUI command to send all axies to near min limits then to near max limits.
+ Watch the indicator led as you do this. Adjust "Max Velocity" setting to suit.
+
+ MOSTLY ON = You can safely go faster.
+ FREQUENT BLINK = This is a safe speed. The best choice.
+ OCCASIONAL BLINK = Your a speed demon. Pushing it to the limits.
+ OFF COMPLETELY = Pushing it too hard. Slow down! The Arduino can't cope, your CNC will break bits and make garbage.
+
+*/
+#define idleIndicator 13
+
+// Invert direction of movement for an axis by setting to false.
+boolean dirState0=true;
+boolean dirState1=true;
+boolean dirState2=true;
+boolean dirState3=true;
+boolean dirState4=true;
+boolean dirState5=true;
+boolean dirState6=true;
+boolean dirState7=true;
+boolean dirState8=true;
+
+////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////END OF USER SETTINGS///////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+
+char buffer[128];
+int sofar;
+
+float pos_x;
+float pos_y;
+float pos_z;
+float pos_a;
+float pos_b;
+float pos_c;
+float pos_u;
+float pos_v;
+float pos_w;
+
+float revs_in=0;
+float spindleRPSin=0;
+
+boolean stepState=LOW;
+unsigned long stepTimeOld=0;
+unsigned long spindleTimeOld=0;
+long stepper0Pos=0;
+long stepper0Goto=0;
+long stepper1Pos=0;
+long stepper1Goto=0;
+long stepper2Pos=0;
+long stepper2Goto=0;
+long stepper3Pos=0;
+long stepper3Goto=0;
+long stepper4Pos=0;
+long stepper4Goto=0;
+long stepper5Pos=0;
+long stepper5Goto=0;
+long stepper6Pos=0;
+long stepper6Goto=0;
+long stepper7Pos=0;
+long stepper7Goto=0;
+long stepper8Pos=0;
+long stepper8Goto=0;
+int stepModeX=-1; // don't set these here, look at stepMode() for info.
+int stepModeY=-1;
+int stepModeZ=-1;
+int stepModeA=-1;
+int stepModeB=-1;
+int stepModeC=-1;
+int stepModeU=-1;
+int stepModeV=-1;
+int stepModeW=-1;
+
+boolean xMinState=false;
+boolean yMinState=false;
+boolean zMinState=false;
+boolean aMinState=false;
+boolean bMinState=false;
+boolean cMinState=false;
+boolean uMinState=false;
+boolean vMinState=false;
+boolean wMinState=false;
+boolean xMinStateOld=false;
+boolean yMinStateOld=false;
+boolean zMinStateOld=false;
+boolean aMinStateOld=false;
+boolean bMinStateOld=false;
+boolean cMinStateOld=false;
+boolean uMinStateOld=false;
+boolean vMinStateOld=false;
+boolean wMinStateOld=false;
+
+boolean xHomeState=false;
+boolean yHomeState=false;
+boolean zHomeState=false;
+boolean aHomeState=false;
+boolean bHomeState=false;
+boolean cHomeState=false;
+boolean uHomeState=false;
+boolean vHomeState=false;
+boolean wHomeState=false;
+boolean xHomeStateOld=false;
+boolean yHomeStateOld=false;
+boolean zHomeStateOld=false;
+boolean aHomeStateOld=false;
+boolean bHomeStateOld=false;
+boolean cHomeStateOld=false;
+boolean uHomeStateOld=false;
+boolean vHomeStateOld=false;
+boolean wHomeStateOld=false;
+
+boolean xMaxState=false;
+boolean yMaxState=false;
+boolean zMaxState=false;
+boolean aMaxState=false;
+boolean bMaxState=false;
+boolean cMaxState=false;
+boolean uMaxState=false;
+boolean vMaxState=false;
+boolean wMaxState=false;
+boolean xMaxStateOld=false;
+boolean yMaxStateOld=false;
+boolean zMaxStateOld=false;
+boolean aMaxStateOld=false;
+boolean bMaxStateOld=false;
+boolean cMaxStateOld=false;
+boolean uMaxStateOld=false;
+boolean vMaxStateOld=false;
+boolean wMaxStateOld=false;
+
+boolean eStopStateOld=false;
+boolean powerStateOld=false;
+boolean probeStateOld=false;
+boolean startStateOld=false;
+boolean stopStateOld=false;
+boolean pauseStateOld=false;
+boolean resumeStateOld=false;
+boolean stepStateOld=false;
+int globalBusy=0;
+
+long divisor=1000000; // input divisor. Our HAL script wont send the six decimal place floats that EMC cranks out.
+ // A simple workaround is to multply it by 1000000 before sending it over the wire.
+ // So here we have to put the decimal back to get the real numbers.
+ // Used in: processCommand()
+
+boolean psuState=powerSupplyInverted;
+boolean spindleState=spindleDirectionInverted;
+
+float fbx=1;
+float fby=1;
+float fbz=1;
+float fba=1;
+float fbb=1;
+float fbc=1;
+float fbu=1;
+float fbv=1;
+float fbw=1;
+
+float fbxOld=0;
+float fbyOld=0;
+float fbzOld=0;
+float fbaOld=0;
+float fbbOld=0;
+float fbcOld=0;
+float fbuOld=0;
+float fbvOld=0;
+float fbwOld=0;
+
+
+void jog(float x, float y, float z, float a, float b, float c, float u, float v, float w)
+{
+ pos_x=x;
+ pos_y=y;
+ pos_z=z;
+ pos_a=a;
+ pos_b=b;
+ pos_c=c;
+ pos_u=u;
+ pos_v=v;
+ pos_w=w;
+ // Handle our limit switches.
+ // Compressed to save visual space. Otherwise it would be several pages long!
+
+ if(!useRealMinX){if(pos_x > xMin){xMinState=true;}else{xMinState=false;}}else{xMinState=digitalReadFast2(xMinPin);if(xMinPinInverted)xMinState=!xMinState;}
+ if(!useRealMinY){if(pos_y > yMin){yMinState=true;}else{yMinState=false;}}else{yMinState=digitalReadFast2(yMinPin);if(yMinPinInverted)yMinState=!yMinState;}
+ if(!useRealMinZ){if(pos_z > zMin){zMinState=true;}else{zMinState=false;}}else{zMinState=digitalReadFast2(zMinPin);if(zMinPinInverted)zMinState=!zMinState;}
+ if(!useRealMinA){if(pos_a > aMin){aMinState=true;}else{aMinState=false;}}else{aMinState=digitalReadFast2(aMinPin);if(aMinPinInverted)aMinState=!aMinState;}
+ if(!useRealMinB){if(pos_b > bMin){bMinState=true;}else{bMinState=false;}}else{bMinState=digitalReadFast2(bMinPin);if(bMinPinInverted)bMinState=!bMinState;}
+ if(!useRealMinC){if(pos_c > cMin){cMinState=true;}else{cMinState=false;}}else{cMinState=digitalReadFast2(cMinPin);if(cMinPinInverted)cMinState=!cMinState;}
+ if(!useRealMinU){if(pos_u > uMin){uMinState=true;}else{uMinState=false;}}else{uMinState=digitalReadFast2(uMinPin);if(uMinPinInverted)uMinState=!uMinState;}
+ if(!useRealMinV){if(pos_v > vMin){vMinState=true;}else{vMinState=false;}}else{vMinState=digitalReadFast2(vMinPin);if(vMinPinInverted)vMinState=!vMinState;}
+ if(!useRealMinW){if(pos_w > wMin){wMinState=true;}else{wMinState=false;}}else{wMinState=digitalReadFast2(wMinPin);if(wMinPinInverted)wMinState=!wMinState;}
+
+ if(!useRealMaxX){if(pos_x > xMax){xMaxState=true;}else{xMaxState=false;}}else{xMaxState=digitalReadFast2(xMaxPin);if(xMaxPinInverted)xMaxState=!xMaxState;}
+ if(!useRealMaxY){if(pos_y > yMax){yMaxState=true;}else{yMaxState=false;}}else{yMaxState=digitalReadFast2(yMaxPin);if(yMaxPinInverted)yMaxState=!yMaxState;}
+ if(!useRealMaxZ){if(pos_z > zMax){zMaxState=true;}else{zMaxState=false;}}else{zMaxState=digitalReadFast2(zMaxPin);if(zMaxPinInverted)zMaxState=!zMaxState;}
+ if(!useRealMaxA){if(pos_a > aMax){aMaxState=true;}else{aMaxState=false;}}else{aMaxState=digitalReadFast2(aMaxPin);if(aMaxPinInverted)aMaxState=!aMaxState;}
+ if(!useRealMaxB){if(pos_b > bMax){bMaxState=true;}else{bMaxState=false;}}else{bMaxState=digitalReadFast2(bMaxPin);if(bMaxPinInverted)bMaxState=!bMaxState;}
+ if(!useRealMaxC){if(pos_c > cMax){cMaxState=true;}else{cMaxState=false;}}else{cMaxState=digitalReadFast2(cMaxPin);if(cMaxPinInverted)cMaxState=!cMaxState;}
+ if(!useRealMaxU){if(pos_u > uMax){uMaxState=true;}else{uMaxState=false;}}else{uMaxState=digitalReadFast2(uMaxPin);if(uMaxPinInverted)uMaxState=!uMaxState;}
+ if(!useRealMaxV){if(pos_v > vMax){vMaxState=true;}else{vMaxState=false;}}else{vMaxState=digitalReadFast2(vMaxPin);if(vMaxPinInverted)vMaxState=!vMaxState;}
+ if(!useRealMaxW){if(pos_w > wMax){wMaxState=true;}else{wMaxState=false;}}else{wMaxState=digitalReadFast2(wMaxPin);if(wMaxPinInverted)wMaxState=!wMaxState;}
+
+ if(!useRealHomeX){if(pos_x > xHome){xHomeState=true;}else{xHomeState=false;}}else{xHomeState=digitalReadFast2(xHomePin);if(xHomePinInverted)xHomeState=!xHomeState;}
+ if(!useRealHomeY){if(pos_y > yHome){yHomeState=true;}else{yHomeState=false;}}else{yHomeState=digitalReadFast2(yHomePin);if(yHomePinInverted)yHomeState=!yHomeState;}
+ if(!useRealHomeZ){if(pos_z > zHome){zHomeState=true;}else{zHomeState=false;}}else{zHomeState=digitalReadFast2(zHomePin);if(zHomePinInverted)zHomeState=!zHomeState;}
+ if(!useRealHomeA){if(pos_a > aHome){aHomeState=true;}else{aHomeState=false;}}else{aHomeState=digitalReadFast2(aHomePin);if(aHomePinInverted)aHomeState=!aHomeState;}
+ if(!useRealHomeB){if(pos_b > bHome){bHomeState=true;}else{bHomeState=false;}}else{bHomeState=digitalReadFast2(bHomePin);if(bHomePinInverted)bHomeState=!bHomeState;}
+ if(!useRealHomeC){if(pos_c > cHome){cHomeState=true;}else{cHomeState=false;}}else{cHomeState=digitalReadFast2(cHomePin);if(cHomePinInverted)cHomeState=!cHomeState;}
+ if(!useRealHomeU){if(pos_u > uHome){uHomeState=true;}else{uHomeState=false;}}else{uHomeState=digitalReadFast2(uHomePin);if(uHomePinInverted)uHomeState=!uHomeState;}
+ if(!useRealHomeV){if(pos_v > vHome){vHomeState=true;}else{vHomeState=false;}}else{vHomeState=digitalReadFast2(vHomePin);if(vHomePinInverted)vHomeState=!vHomeState;}
+ if(!useRealHomeW){if(pos_w > wHome){wHomeState=true;}else{wHomeState=false;}}else{wHomeState=digitalReadFast2(wHomePin);if(wHomePinInverted)wHomeState=!wHomeState;}
+
+ if(xMinState != xMinStateOld){xMinStateOld=xMinState;Serial.print("x");Serial.print(xMinState);}
+ if(yMinState != yMinStateOld){yMinStateOld=yMinState;Serial.print("y");Serial.print(yMinState);}
+ if(zMinState != zMinStateOld){zMinStateOld=zMinState;Serial.print("z");Serial.print(zMinState);}
+ if(aMinState != aMinStateOld){aMinStateOld=aMinState;Serial.print("a");Serial.print(aMinState);}
+ if(bMinState != bMinStateOld){bMinStateOld=bMinState;Serial.print("b");Serial.print(bMinState);}
+ if(cMinState != cMinStateOld){cMinStateOld=cMinState;Serial.print("c");Serial.print(cMinState);}
+ if(uMinState != uMinStateOld){uMinStateOld=uMinState;Serial.print("u");Serial.print(uMinState);}
+ if(vMinState != vMinStateOld){vMinStateOld=vMinState;Serial.print("v");Serial.print(vMinState);}
+ if(wMinState != wMinStateOld){wMinStateOld=wMinState;Serial.print("w");Serial.print(wMinState);}
+
+ if(xHomeState != xHomeStateOld){xHomeStateOld=xHomeState;Serial.print("x");Serial.print(xHomeState+4);}
+ if(yHomeState != yHomeStateOld){yHomeStateOld=yHomeState;Serial.print("y");Serial.print(yHomeState+4);}
+ if(zHomeState != zHomeStateOld){zHomeStateOld=zHomeState;Serial.print("z");Serial.print(zHomeState+4);}
+ if(aHomeState != aHomeStateOld){aHomeStateOld=aHomeState;Serial.print("a");Serial.print(aHomeState+4);}
+ if(bHomeState != bHomeStateOld){bHomeStateOld=bHomeState;Serial.print("b");Serial.print(bHomeState+4);}
+ if(cHomeState != cHomeStateOld){cHomeStateOld=cHomeState;Serial.print("c");Serial.print(cHomeState+4);}
+ if(uHomeState != uHomeStateOld){uHomeStateOld=uHomeState;Serial.print("u");Serial.print(uHomeState+4);}
+ if(vHomeState != vHomeStateOld){vHomeStateOld=vHomeState;Serial.print("v");Serial.print(vHomeState+4);}
+ if(wHomeState != wHomeStateOld){wHomeStateOld=wHomeState;Serial.print("w");Serial.print(wHomeState+4);}
+
+ if(xMaxState != xMaxStateOld){xMaxStateOld=xMaxState;Serial.print("x");Serial.print(xMaxState+1);}
+ if(yMaxState != yMaxStateOld){yMaxStateOld=yMaxState;Serial.print("y");Serial.print(yMaxState+1);}
+ if(zMaxState != zMaxStateOld){zMaxStateOld=zMaxState;Serial.print("z");Serial.print(zMaxState+1);}
+ if(aMaxState != aMaxStateOld){aMaxStateOld=aMaxState;Serial.print("a");Serial.print(aMaxState+1);}
+ if(bMaxState != bMaxStateOld){bMaxStateOld=bMaxState;Serial.print("b");Serial.print(bMaxState+1);}
+ if(cMaxState != cMaxStateOld){cMaxStateOld=cMaxState;Serial.print("c");Serial.print(cMaxState+1);}
+ if(uMaxState != uMaxStateOld){uMaxStateOld=uMaxState;Serial.print("u");Serial.print(uMaxState+1);}
+ if(vMaxState != vMaxStateOld){vMaxStateOld=vMaxState;Serial.print("v");Serial.print(vMaxState+1);}
+ if(wMaxState != wMaxStateOld){wMaxStateOld=wMaxState;Serial.print("w");Serial.print(wMaxState+1);}
+
+ if(xMinState && !xMaxState)stepper0Goto=pos_x*stepsPerInchX*2;
+ if(yMinState && !yMaxState)stepper1Goto=pos_y*stepsPerInchY*2;
+ if(zMinState && !zMaxState)stepper2Goto=pos_z*stepsPerInchZ*2; // we need the *2 as we're driving a flip-flop routine (in stepLight function)
+ if(aMinState && !aMaxState)stepper3Goto=pos_a*stepsPerInchA*2;
+ if(bMinState && !bMaxState)stepper4Goto=pos_b*stepsPerInchB*2;
+ if(cMinState && !cMaxState)stepper5Goto=pos_c*stepsPerInchC*2;
+ if(uMinState && !uMaxState)stepper6Goto=pos_u*stepsPerInchU*2;
+ if(vMinState && !vMaxState)stepper7Goto=pos_v*stepsPerInchV*2;
+ if(wMinState && !wMaxState)stepper8Goto=pos_w*stepsPerInchW*2;
+
+}
+
+void processCommand()
+{
+ float xx=pos_x;
+ float yy=pos_y;
+ float zz=pos_z;
+ float aa=pos_a;
+ float bb=pos_b;
+ float cc=pos_c;
+ float uu=pos_u;
+ float vv=pos_v;
+ float ww=pos_w;
+
+ float ss=revs_in;
+
+ char *ptr=buffer;
+ while(ptr && ptr<buffer+sofar)
+ {
+ ptr=strchr(ptr,' ')+1;
+ switch(*ptr) {
+
+ // These are axis move commands
+ case 'x': case 'X': xx=atof(ptr+1); xx=xx/divisor; break;
+ case 'y': case 'Y': yy=atof(ptr+1); yy=yy/divisor; break;
+ case 'z': case 'Z': zz=atof(ptr+1); zz=zz/divisor; break;
+ case 'a': case 'A': aa=atof(ptr+1); aa=aa/divisor; break;
+ case 'b': case 'B': bb=atof(ptr+1); bb=bb/divisor; break;
+ case 'c': case 'C': cc=atof(ptr+1); cc=cc/divisor; break;
+ case 'u': case 'U': uu=atof(ptr+1); uu=uu/divisor; break;
+ case 'v': case 'V': vv=atof(ptr+1); vv=vv/divisor; break;
+ case 'w': case 'W': ww=atof(ptr+1); ww=ww/divisor; break;
+
+ // Spindle speed command. In revs per second
+ case 's': case 'S': ss=atof(ptr+1); spindleRPSin=ss; break;
+
+ default: ptr=0; break;
+ }
+ }
+ jog(xx,yy,zz,aa,bb,cc,uu,vv,ww);
+ if(globalBusy<15)
+ {
+ // Insert LCD call here. (Updated when mostly idle.) Future project.
+ }
+}
+
+void stepLight() // Set by jog() && Used by loop()
+{
+ unsigned long curTime=micros();
+ if(curTime - stepTimeOld >= minStepTime)
+ {
+ stepState=!stepState;
+ int busy=0;
+
+ if(stepper0Pos != stepper0Goto){busy++;if(stepper0Pos > stepper0Goto){digitalWriteFast2(dirPin0,!dirState0);digitalWriteFast2(stepPin0,stepState);stepper0Pos--;}else{digitalWriteFast2(dirPin0, dirState0);digitalWriteFast2(stepPin0,stepState);stepper0Pos++;}}
+ if(stepper1Pos != stepper1Goto){busy++;if(stepper1Pos > stepper1Goto){digitalWriteFast2(dirPin1,!dirState1);digitalWriteFast2(stepPin1,stepState);stepper1Pos--;}else{digitalWriteFast2(dirPin1, dirState1);digitalWriteFast2(stepPin1,stepState);stepper1Pos++;}}
+ if(stepper2Pos != stepper2Goto){busy++;if(stepper2Pos > stepper2Goto){digitalWriteFast2(dirPin2,!dirState2);digitalWriteFast2(stepPin2,stepState);stepper2Pos--;}else{digitalWriteFast2(dirPin2, dirState2);digitalWriteFast2(stepPin2,stepState);stepper2Pos++;}}
+ if(stepper3Pos != stepper3Goto){busy++;if(stepper3Pos > stepper3Goto){digitalWriteFast2(dirPin3,!dirState3);digitalWriteFast2(stepPin3,stepState);stepper3Pos--;}else{digitalWriteFast2(dirPin3, dirState3);digitalWriteFast2(stepPin3,stepState);stepper3Pos++;}}
+ if(stepper4Pos != stepper4Goto){busy++;if(stepper4Pos > stepper4Goto){digitalWriteFast2(dirPin4,!dirState4);digitalWriteFast2(stepPin4,stepState);stepper4Pos--;}else{digitalWriteFast2(dirPin4, dirState4);digitalWriteFast2(stepPin4,stepState);stepper4Pos++;}}
+ if(stepper5Pos != stepper5Goto){busy++;if(stepper5Pos > stepper5Goto){digitalWriteFast2(dirPin5,!dirState5);digitalWriteFast2(stepPin5,stepState);stepper5Pos--;}else{digitalWriteFast2(dirPin5, dirState5);digitalWriteFast2(stepPin5,stepState);stepper5Pos++;}}
+ if(stepper6Pos != stepper6Goto){busy++;if(stepper6Pos > stepper6Goto){digitalWriteFast2(dirPin6,!dirState6);digitalWriteFast2(stepPin6,stepState);stepper6Pos--;}else{digitalWriteFast2(dirPin6, dirState6);digitalWriteFast2(stepPin6,stepState);stepper6Pos++;}}
+ if(stepper7Pos != stepper7Goto){busy++;if(stepper7Pos > stepper7Goto){digitalWriteFast2(dirPin7,!dirState7);digitalWriteFast2(stepPin7,stepState);stepper7Pos--;}else{digitalWriteFast2(dirPin7, dirState7);digitalWriteFast2(stepPin7,stepState);stepper7Pos++;}}
+ if(stepper8Pos != stepper8Goto){busy++;if(stepper8Pos > stepper8Goto){digitalWriteFast2(dirPin8,!dirState8);digitalWriteFast2(stepPin8,stepState);stepper8Pos--;}else{digitalWriteFast2(dirPin8, dirState8);digitalWriteFast2(stepPin8,stepState);stepper8Pos++;}}
+ if(busy){digitalWriteFast2(idleIndicator,LOW);if(globalBusy<255){globalBusy++;}}else{digitalWriteFast2(idleIndicator,HIGH);if(globalBusy>0){globalBusy--;}
+ if(giveFeedBackX){fbx=stepper0Pos/4/(stepsPerInchX*0.5);if(!busy){if(fbx!=fbxOld){fbxOld=fbx;Serial.print("fx");Serial.println(fbx,6);}}}
+ if(giveFeedBackY){fby=stepper1Pos/4/(stepsPerInchY*0.5);if(!busy){if(fby!=fbyOld){fbyOld=fby;Serial.print("fy");Serial.println(fby,6);}}}
+ if(giveFeedBackZ){fbz=stepper2Pos/4/(stepsPerInchZ*0.5);if(!busy){if(fbz!=fbzOld){fbzOld=fbz;Serial.print("fz");Serial.println(fbz,6);}}}
+ if(giveFeedBackA){fba=stepper3Pos/4/(stepsPerInchA*0.5);if(!busy){if(fba!=fbaOld){fbaOld=fba;Serial.print("fa");Serial.println(fba,6);}}}
+ if(giveFeedBackB){fbb=stepper4Pos/4/(stepsPerInchB*0.5);if(!busy){if(fbb!=fbbOld){fbbOld=fbb;Serial.print("fb");Serial.println(fbb,6);}}}
+ if(giveFeedBackC){fbc=stepper5Pos/4/(stepsPerInchC*0.5);if(!busy){if(fbc!=fbcOld){fbcOld=fbc;Serial.print("fc");Serial.println(fbc,6);}}}
+ if(giveFeedBackU){fbu=stepper6Pos/4/(stepsPerInchU*0.5);if(!busy){if(fbu!=fbuOld){fbuOld=fbu;Serial.print("fu");Serial.println(fbu,6);}}}
+ if(giveFeedBackV){fbv=stepper7Pos/4/(stepsPerInchV*0.5);if(!busy){if(fbv!=fbvOld){fbvOld=fbv;Serial.print("fv");Serial.println(fbv,6);}}}
+ if(giveFeedBackW){fbw=stepper8Pos/4/(stepsPerInchW*0.5);if(!busy){if(fbw!=fbwOld){fbwOld=fbw;Serial.print("fw");Serial.println(fbw,6);}}}
+ }
+
+ stepTimeOld=curTime;
+ }
+}
+
+void stepMode(int axis, int mode) // May be omitted in the future. (Undecided)
+{
+ // called just once during setup()
+
+ // This works OPPOSITE of what you might think.
+ // Mode 1 = 1/16 step.
+ // Mode 2 = 1/8 step.
+ // Mode 4 = 1/4 step.
+ // Mode 8 = 1/2 step.
+ // Mode 16 = Full step.
+ // Its simular to a car's gearbox with gears from low to high as in 1,2,4,8,16
+
+ // Originally intended for dynamic microstepping control to reduce mpu overhead and speed steppers when moving large distances.
+ // Real world result: Increased overhead and slowed steppers while juggling unessessary math and pin commands.
+
+ boolean ms1;
+ boolean ms2;
+ boolean ms3;
+ int count;
+ if(mode>=16){ms1=LOW;ms2=LOW;ms3=LOW;count=16;}
+ if(mode>=8 && mode<=15){ms1=HIGH;ms2=LOW;ms3=LOW;count=8;}
+ if(mode>=4 && mode<=7){ms1=LOW;ms2=HIGH;ms3=LOW;count=4;}
+ if(mode>=2 && mode<=3){ms1=HIGH;ms2=HIGH;ms3=LOW;count=2;}
+ if(mode<=1){ms1=HIGH;ms2=HIGH;ms3=HIGH;count=1;}
+ if(axis == 0 || 9){if(mode!=stepModeX){digitalWriteFast2(chanXms1,ms1);digitalWriteFast2(chanXms2,ms2);digitalWriteFast2(chanXms3,ms3);stepModeX=count;}}
+ if(axis == 1 || 9){if(mode!=stepModeY){digitalWriteFast2(chanYms1,ms1);digitalWriteFast2(chanYms2,ms2);digitalWriteFast2(chanYms3,ms3);stepModeY=count;}}
+ if(axis == 2 || 9){if(mode!=stepModeZ){digitalWriteFast2(chanZms1,ms1);digitalWriteFast2(chanZms2,ms2);digitalWriteFast2(chanZms3,ms3);stepModeZ=count;}}
+ if(axis == 3 || 9){if(mode!=stepModeA){digitalWriteFast2(chanAms1,ms1);digitalWriteFast2(chanAms2,ms2);digitalWriteFast2(chanAms3,ms3);stepModeA=count;}}
+ if(axis == 4 || 9){if(mode!=stepModeB){digitalWriteFast2(chanBms1,ms1);digitalWriteFast2(chanBms2,ms2);digitalWriteFast2(chanBms3,ms3);stepModeB=count;}}
+ if(axis == 5 || 9){if(mode!=stepModeC){digitalWriteFast2(chanCms1,ms1);digitalWriteFast2(chanCms2,ms2);digitalWriteFast2(chanCms3,ms3);stepModeC=count;}}
+ if(axis == 6 || 9){if(mode!=stepModeU){digitalWriteFast2(chanUms1,ms1);digitalWriteFast2(chanUms2,ms2);digitalWriteFast2(chanUms3,ms3);stepModeU=count;}}
+ if(axis == 7 || 9){if(mode!=stepModeV){digitalWriteFast2(chanVms1,ms1);digitalWriteFast2(chanVms2,ms2);digitalWriteFast2(chanVms3,ms3);stepModeV=count;}}
+ if(axis == 8 || 9){if(mode!=stepModeW){digitalWriteFast2(chanWms1,ms1);digitalWriteFast2(chanWms2,ms2);digitalWriteFast2(chanWms3,ms3);stepModeW=count;}}
+}
+
+int determinInterrupt(int val)
+{
+ if(val<0) return -1;
+ if(val==2) return 0;
+ if(val==3) return 1;
+ if(val==18) return 5;
+ if(val==19) return 4;
+ if(val==20) return 3;
+ if(val==21) return 2;
+}
+
+volatile unsigned long spindleRevs=0;
+float spindleRPS=0;
+float spindleRPM=0;
+
+void countSpindleRevs()
+{
+ spindleRevs++;
+}
+
+float updateSpindleRevs()
+{
+ unsigned long spindleTime=millis();
+ if(spindleTime - spindleTimeOld >= 100)
+ {
+ spindleRPS=spindleRevs*10.0;
+ spindleRPM=spindleRPS*60.0;
+ spindleRevs=0;
+ }
+}
+
+boolean spindleEnabled=false;
+boolean spindleEnableState=spindleEnableInverted;
+
+boolean spindleAtSpeed()
+{
+ if(spindleTach>0)
+ {
+ if(spindleRPSin<spindleRPS)
+ {
+ if(spindleRPSin*1.05<spindleRPS || !spindleEnabled)
+ { /* Slow down. */
+ if(spindleEnablePin>0){digitalWriteFast2(spindleEnablePin,!spindleEnableState);}
+ }else{
+ if(spindleEnabled)
+ { /* Speed up. */
+ if(spindleEnablePin>0){digitalWriteFast2(spindleEnablePin,spindleEnableState);}
+ }
+ }
+ return true;
+ }else{
+ return false;
+ }
+ }else{
+ return spindleEnabled; // No tach? We'll fake it.
+ }
+}
+
+void setup()
+{
+ // If using a spindle tachometer, setup an interrupt for it.
+ if(spindleTach>0){int result=determinInterrupt(spindleTach);attachInterrupt(result,countSpindleRevs,FALLING);}
+
+ // Setup Min limit switches.
+ if(useRealMinX){pinMode(xMinPin,INPUT);if(!xMinPinInverted)digitalWriteFast2(xMinPin,HIGH);}
+ if(useRealMinY){pinMode(yMinPin,INPUT);if(!yMinPinInverted)digitalWriteFast2(yMinPin,HIGH);}
+ if(useRealMinZ){pinMode(zMinPin,INPUT);if(!zMinPinInverted)digitalWriteFast2(zMinPin,HIGH);}
+ if(useRealMinA){pinMode(aMinPin,INPUT);if(!aMinPinInverted)digitalWriteFast2(aMinPin,HIGH);}
+ if(useRealMinB){pinMode(bMinPin,INPUT);if(!bMinPinInverted)digitalWriteFast2(bMinPin,HIGH);}
+ if(useRealMinC){pinMode(cMinPin,INPUT);if(!cMinPinInverted)digitalWriteFast2(cMinPin,HIGH);}
+ if(useRealMinU){pinMode(uMinPin,INPUT);if(!uMinPinInverted)digitalWriteFast2(uMinPin,HIGH);}
+ if(useRealMinV){pinMode(vMinPin,INPUT);if(!vMinPinInverted)digitalWriteFast2(vMinPin,HIGH);}
+ if(useRealMinW){pinMode(wMinPin,INPUT);if(!wMinPinInverted)digitalWriteFast2(wMinPin,HIGH);}
+
+ // Setup Max limit switches.
+ if(useRealMaxX){pinMode(xMaxPin,INPUT);if(!xMaxPinInverted)digitalWriteFast2(xMaxPin,HIGH);}
+ if(useRealMaxY){pinMode(yMaxPin,INPUT);if(!yMaxPinInverted)digitalWriteFast2(yMaxPin,HIGH);}
+ if(useRealMaxZ){pinMode(zMaxPin,INPUT);if(!zMaxPinInverted)digitalWriteFast2(zMaxPin,HIGH);}
+ if(useRealMaxA){pinMode(aMaxPin,INPUT);if(!aMaxPinInverted)digitalWriteFast2(aMaxPin,HIGH);}
+ if(useRealMaxB){pinMode(bMaxPin,INPUT);if(!bMaxPinInverted)digitalWriteFast2(bMaxPin,HIGH);}
+ if(useRealMaxC){pinMode(cMaxPin,INPUT);if(!cMaxPinInverted)digitalWriteFast2(cMaxPin,HIGH);}
+ if(useRealMaxU){pinMode(uMaxPin,INPUT);if(!uMaxPinInverted)digitalWriteFast2(uMaxPin,HIGH);}
+ if(useRealMaxV){pinMode(vMaxPin,INPUT);if(!vMaxPinInverted)digitalWriteFast2(vMaxPin,HIGH);}
+ if(useRealMaxW){pinMode(wMaxPin,INPUT);if(!wMaxPinInverted)digitalWriteFast2(wMaxPin,HIGH);}
+
+ // Setup Homing switches.
+ if(useRealHomeX){pinMode(xHomePin,INPUT);if(!xHomePinInverted)digitalWriteFast2(xHomePin,HIGH);}
+ if(useRealHomeY){pinMode(yHomePin,INPUT);if(!yHomePinInverted)digitalWriteFast2(yHomePin,HIGH);}
+ if(useRealHomeZ){pinMode(zHomePin,INPUT);if(!zHomePinInverted)digitalWriteFast2(zHomePin,HIGH);}
+ if(useRealHomeA){pinMode(aHomePin,INPUT);if(!aHomePinInverted)digitalWriteFast2(aHomePin,HIGH);}
+ if(useRealHomeB){pinMode(bHomePin,INPUT);if(!bHomePinInverted)digitalWriteFast2(bHomePin,HIGH);}
+ if(useRealHomeC){pinMode(cHomePin,INPUT);if(!cHomePinInverted)digitalWriteFast2(cHomePin,HIGH);}
+ if(useRealHomeU){pinMode(uHomePin,INPUT);if(!uHomePinInverted)digitalWriteFast2(uHomePin,HIGH);}
+ if(useRealHomeV){pinMode(vHomePin,INPUT);if(!vHomePinInverted)digitalWriteFast2(vHomePin,HIGH);}
+ if(useRealHomeW){pinMode(wHomePin,INPUT);if(!wHomePinInverted)digitalWriteFast2(wHomePin,HIGH);}
+
+ // Enable stepper drivers.
+ pinMode(xEnablePin,OUTPUT);digitalWrite(xEnablePin,LOW);
+ pinMode(yEnablePin,OUTPUT);digitalWrite(yEnablePin,LOW);
+ pinMode(zEnablePin,OUTPUT);digitalWrite(zEnablePin,LOW);
+ pinMode(aEnablePin,OUTPUT);digitalWrite(aEnablePin,LOW);
+ pinMode(bEnablePin,OUTPUT);digitalWrite(bEnablePin,LOW);
+ pinMode(cEnablePin,OUTPUT);digitalWrite(cEnablePin,LOW);
+ pinMode(uEnablePin,OUTPUT);digitalWrite(uEnablePin,LOW);
+ pinMode(vEnablePin,OUTPUT);digitalWrite(vEnablePin,LOW);
+ pinMode(wEnablePin,OUTPUT);digitalWrite(wEnablePin,LOW);
+
+ // Setup step pins.
+ pinMode(stepPin0,OUTPUT);
+ pinMode(stepPin1,OUTPUT);
+ pinMode(stepPin2,OUTPUT);
+ pinMode(stepPin3,OUTPUT);
+ pinMode(stepPin4,OUTPUT);
+ pinMode(stepPin5,OUTPUT);
+ pinMode(stepPin6,OUTPUT);
+ pinMode(stepPin7,OUTPUT);
+ pinMode(stepPin8,OUTPUT);
+
+ // Setup dir pins.
+ pinMode(dirPin0,OUTPUT);
+ pinMode(dirPin1,OUTPUT);
+ pinMode(dirPin2,OUTPUT);
+ pinMode(dirPin3,OUTPUT);
+ pinMode(dirPin4,OUTPUT);
+ pinMode(dirPin5,OUTPUT);
+ pinMode(dirPin6,OUTPUT);
+ pinMode(dirPin7,OUTPUT);
+ pinMode(dirPin8,OUTPUT);
+
+ // Setup microStepping pins.
+ pinMode(chanXms1,OUTPUT);pinMode(chanXms2,OUTPUT);pinMode(chanXms3,OUTPUT);
+ pinMode(chanYms1,OUTPUT);pinMode(chanYms2,OUTPUT);pinMode(chanYms3,OUTPUT);
+ pinMode(chanZms1,OUTPUT);pinMode(chanZms2,OUTPUT);pinMode(chanZms3,OUTPUT);
+ pinMode(chanAms1,OUTPUT);pinMode(chanAms2,OUTPUT);pinMode(chanAms3,OUTPUT);
+ pinMode(chanBms1,OUTPUT);pinMode(chanBms2,OUTPUT);pinMode(chanBms3,OUTPUT);
+ pinMode(chanCms1,OUTPUT);pinMode(chanCms2,OUTPUT);pinMode(chanCms3,OUTPUT);
+ pinMode(chanUms1,OUTPUT);pinMode(chanUms2,OUTPUT);pinMode(chanUms3,OUTPUT);
+ pinMode(chanVms1,OUTPUT);pinMode(chanVms2,OUTPUT);pinMode(chanVms3,OUTPUT);
+ pinMode(chanWms1,OUTPUT);pinMode(chanWms2,OUTPUT);pinMode(chanWms3,OUTPUT);
+
+ // Setup eStop, power, start, stop, pause, resume, program step, spindle, coolant, LED and probe pins.
+ if(useEstopSwitch){pinMode(eStopPin,INPUT);if(!eStopPinInverted){digitalWriteFast2(eStopPin,HIGH);}}
+ if(usePowerSwitch){pinMode(powerPin,INPUT);if(!powerPinInverted){digitalWriteFast2(powerPin,HIGH);}}
+ if(useProbe){pinMode(probePin,INPUT);if(!probePinInverted){digitalWriteFast2(probePin,HIGH);}}
+ if(useStartSwitch){pinMode(startPin,INPUT);if(!startPinInverted){digitalWriteFast2(startPin,HIGH);}}
+ if(useStopSwitch){pinMode(stopPin,INPUT);if(!stopPinInverted){digitalWriteFast2(stopPin,HIGH);}}
+ if(usePauseSwitch){pinMode(pausePin,INPUT);if(!pausePinInverted){digitalWriteFast2(pausePin,HIGH);}}
+ if(useResumeSwitch){pinMode(resumePin,INPUT);if(!resumePinInverted){digitalWriteFast2(resumePin,HIGH);}}
+ if(useStepSwitch){pinMode(stepPin,INPUT);if(!stepPinInverted){digitalWriteFast2(stepPin,HIGH);}}
+ if(powerLedPin > 0){pinMode(powerLedPin,OUTPUT);digitalWriteFast2(powerLedPin,HIGH);}
+ if(eStopLedPin>0){pinMode(eStopLedPin,OUTPUT);digitalWriteFast2(eStopLedPin,LOW);}
+ if(spindleEnablePin>0){pinMode(spindleEnablePin,OUTPUT);digitalWriteFast2(spindleEnablePin,HIGH);}
+ if(spindleDirection>0){pinMode(spindleDirection,OUTPUT);digitalWriteFast2(spindleDirection,LOW);}
+ if(spindleTach>0){pinMode(spindleTach,INPUT);digitalWriteFast2(spindleTach,HIGH);}
+ if(coolantMistPin>0){pinMode(coolantMistPin,OUTPUT);digitalWriteFast2(coolantMistPin,LOW);}
+ if(coolantFloodPin>0){pinMode(coolantFloodPin,OUTPUT);digitalWriteFast2(coolantFloodPin,LOW);}
+ if(powerSupplyPin>0){pinMode(powerSupplyPin,OUTPUT);digitalWriteFast2(powerSupplyPin,psuState);}
+
+ // Setup idle indicator led.
+ pinMode(idleIndicator,OUTPUT);
+
+ // Actually SET our microStepping mode. (If you must change this, re-adjust your stepsPerInch settings.)
+ stepMode(9,1);
+
+ // Setup serial link.
+ Serial.begin(BAUD);
+
+ // Initialize serial command buffer.
+ sofar=0;
+}
+
+boolean spindleAtSpeedStateOld;
+
+void loop()
+{
+ if(useEstopSwitch==true){boolean eStopState=digitalReadFast2(eStopPin);if(eStopPinInverted){eStopState=!eStopState;}if(eStopState != eStopStateOld || eStopStateOld){eStopStateOld=eStopState;Serial.print("e");Serial.println(eStopState);delay(500);}}
+ if(usePowerSwitch==true){boolean powerState=digitalReadFast2(powerPin);if(powerPinInverted){powerState=!powerState;}if(powerState != powerStateOld){powerStateOld=powerState;if(powerSwitchIsMomentary){Serial.println("pt");}else{Serial.print("p");Serial.println(powerState);}}}
+ if(useProbe==true){boolean probeState=digitalReadFast2(probePin);if(probePinInverted){probeState=!probeState;}if(probeState != probeStateOld){probeStateOld=probeState;Serial.print("P");Serial.println(probeState);}}
+ if(useStartSwitch==true){boolean startState=digitalReadFast2(startPin);if(startPinInverted){startState=!startState;}if(startState != startStateOld){startStateOld=startState;Serial.print("h");Serial.println(startState);}}
+ if(useStopSwitch==true){boolean stopState=digitalReadFast2(stopPin);if(stopPinInverted){stopState=!stopState;}if(stopState != stopStateOld){stopStateOld=stopState;Serial.print("h");Serial.println(stopState+2);}}
+ if(usePauseSwitch==true){boolean pauseState=digitalReadFast2(pausePin);if(pausePinInverted){pauseState=!pauseState;}if(pauseState != pauseStateOld){pauseStateOld=pauseState;Serial.print("h");Serial.println(pauseState+4);}}
+ if(useResumeSwitch==true){boolean resumeState=digitalReadFast2(resumePin);if(resumePinInverted){resumeState=!resumeState;}if(resumeState != resumeStateOld){resumeStateOld=resumeState;Serial.print("h");Serial.println(resumeState+6);}}
+ if(useStepSwitch==true){boolean stepState=digitalReadFast2(stepPin);if(stepPinInverted){stepState=!stepState;}if(stepState != stepStateOld){stepStateOld=stepState;Serial.print("h");Serial.println(stepState+8);}}
+
+
+ // listen for serial commands
+ while(Serial.available() > 0) {
+ buffer[sofar++]=Serial.read();
+ if(buffer[sofar-1]==';') break; // in case there are multiple instructions
+
+ }
+ // Received a "+" turn something on.
+ if(sofar>0 && buffer[sofar-3]=='+') {
+ if(sofar>0 && buffer[sofar-2]=='P') { /* Power LED & PSU ON */ if(powerLedPin>0){digitalWriteFast2(powerLedPin,HIGH);}if(powerSupplyPin>0){digitalWriteFast2(powerSupplyPin,psuState);}}
+ if(sofar>0 && buffer[sofar-2]=='E') { /* E-Stop Indicator ON */ if(eStopLedPin>0){digitalWriteFast2(eStopLedPin,HIGH);}}
+ if(sofar>0 && buffer[sofar-2]=='S') { /* Spindle power ON */ spindleEnabled=true;}
+ if(sofar>0 && buffer[sofar-2]=='D') { /* Spindle direction CW */ if(spindleDirection>0){digitalWriteFast2(spindleDirection,spindleState);}}
+ if(sofar>0 && buffer[sofar-2]=='M') { /* Coolant Mist ON */ if(coolantMistPin>0){digitalWriteFast2(coolantMistPin,HIGH);}}
+ if(sofar>0 && buffer[sofar-2]=='F') { /* Coolant Flood ON */ if(coolantFloodPin>0){digitalWriteFast2(coolantFloodPin,HIGH);}}
+ }
+
+ // Received a "-" turn something off.
+ if(sofar>0 && buffer[sofar-3]=='-') {
+ if(sofar>0 && buffer[sofar-2]=='P') { /* Power LED & PSU OFF */ if(powerLedPin>0){digitalWriteFast2(powerLedPin,LOW);}if(powerSupplyPin>0){digitalWriteFast2(powerSupplyPin,!psuState);}}
+ if(sofar>0 && buffer[sofar-2]=='E') { /* E-Stop Indicator OFF */ if(eStopLedPin>0){digitalWriteFast2(eStopLedPin,LOW);}}
+ if(sofar>0 && buffer[sofar-2]=='S') { /* Spindle power OFF */ spindleEnabled=false;}
+ if(sofar>0 && buffer[sofar-2]=='D') { /* Spindle direction CCW */ if(spindleDirection>0){digitalWriteFast2(spindleDirection,!spindleState);}}
+ if(sofar>0 && buffer[sofar-2]=='M') { /* Coolant Mist OFF */ if(coolantMistPin>0){digitalWriteFast2(coolantMistPin,LOW);}}
+ if(sofar>0 && buffer[sofar-2]=='F') { /* Coolant Flood OFF */ if(coolantFloodPin>0){digitalWriteFast2(coolantFloodPin,LOW);}}
+ }
+
+ // Received a "?" about something give an answer.
+ if(sofar>0 && buffer[sofar-3]=='?') {
+ if(sofar>0 && buffer[sofar-2]=='V') { /* Report version */ Serial.println(VERSION);}
+ if(sofar>0 && buffer[sofar-2]=='R') { /* Report role */ Serial.println(ROLE);}
+ }
+
+ // if we hit a semi-colon, assume end of instruction.
+ if(sofar>0 && buffer[sofar-1]==';') {
+
+ buffer[sofar]=0;
+
+ // do something with the command
+ processCommand();
+
+ // reset the buffer
+ sofar=0;
+ }
+ updateSpindleRevs();
+ if(!globalBusy){boolean spindleAtSpeedState=spindleAtSpeed();if(spindleAtSpeedState != spindleAtSpeedStateOld){spindleAtSpeedStateOld=spindleAtSpeedState;Serial.print("S");Serial.println(spindleAtSpeedState);}}
+ stepLight(); // call every loop cycle to update stepper motion.
+}
View
38 pre-release/EmcArduino_07b/Instructions.txt
@@ -0,0 +1,38 @@
+These files are public domain. Do with them as you wish.
+
+The following instructions apply to CNC users. ( Robotics enthusiasts can just upload the sketch and play. :)
+How to going in 7 easy steps:
+
+1. Install linuxCNC on an old (USB enabled) computer.
+ You can get it for free at http://linuxcnc.org/
+
+2. Open the EmcArduino firmware adjust to suit, upload it to your UNO/Arduino Mega 2560.
+ It can be done with an UNO if your really clever about pin managment.
+
+3. Run the "EMC2 Stepconf Wizard" to setup your machine, it will by default create a folder
+ on your desktop called "my-mill".
+
+4. Edit the included custom.hal file, adjust to suit your needs.
+
+5. Copy this included custom.hal file into said "my-mill" folder.
+
+6. Open a Terminal window and do the following:
+
+ copy the 9axis_arduino file to /usr/bin/ with something like:
+ sudo cp ~/Downloads/EmcArduino_07b/9axis_arduino /usr/bin
+
+ then make it executable with,
+ sudo chmod +x /usr/bin/9axis_arduino
+
+7. Play with your CNC for a while tuning and tweaking it by editing the my-mill.ini file
+ in the my-mill folder. (Use tuning LED to help determine max speed.)
+
+
+
+Tips: Inside the my-mill.ini the following settings <> to english are:
+
+MAX_VELOCITY = 2.33 <> Maximum speed limit for an axis (in feet/meters per second).
+HOME = 2.0 <> Where your home SWITCH is located for that axis. (in inches/mm)
+HOME_OFFSET = 0.0 <> Where to move to after it finds the switch. (in inches/mm)
+
+Further reading on these and more settings are found at http://linuxcnc.org/index.php/english/documentation
View
13 pre-release/EmcArduino_07b/README
@@ -0,0 +1,13 @@
+EMC 2 Arduino is a small collection of files intended to allow a person with
+an Arduino Mega2560 (or UNO), an old USB enabled computer and a free copy of
+LinuxCNC to begin building their very own CNC machine cheaply and quickly with
+a variety of components, ranging from low-end inexpensive (even salvaged)
+components such as old steppers and computer parts to high-end linear servos.
+
+Right now We're just beginning, accepting advice, contributions of code and skill
+as well as learning too.
+
+ Got ideas, tips, suggestions?
+ Would you like to be part of our team?
+ Just let us know.
+ http://emc2arduino.wordpress.com
View
204 pre-release/EmcArduino_07b/custom.hal
@@ -0,0 +1,204 @@
+# Include your customized HAL commands here
+# This file will not be overwritten when you run stepconf again
+
+#First load the Python user module named /user/bin/9axis_arduino with the name 'arduino'
+loadusr -Wn arduino /usr/bin/9axis_arduino
+
+#Second 'unlinkp' our pins to make them available for use.
+# Then use 'net' to recreate/hook into them.
+# Comment out any sections that your machine doesn't have.
+
+# Control panel switches
+ # E-Stop
+unlinkp iocontrol.0.user-enable-out
+unlinkp iocontrol.0.emc-enable-in
+net eStop iocontrol.0.user-enable-out => arduino.eStop
+net eStop iocontrol.0.emc-enable-in <= arduino.eStop
+
+ # Power
+unlinkp halui.machine.is-on
+unlinkp halui.machine.on
+unlinkp halui.machine.off
+net machstate arduino.machine-state <= halui.machine.is-on
+net turnoff halui.machine.off <= arduino.switch-off
+net turnon halui.machine.on <= arduino.switch-on
+
+ # Start/Run
+unlinkp halui.program.run
+net ProgramStart arduino.start => halui.program.run
+
+ # Stop
+unlinkp halui.program.stop
+net ProgramStop arduino.stop => halui.program.stop
+
+ # Pause
+unlinkp halui.program.pause
+net ProgramPause arduino.pause => halui.program.pause
+
+ # Resume
+unlinkp halui.program.resume
+net ProgramResume arduino.resume => halui.program.resume
+
+ # Program step
+unlinkp halui.program.step
+net ProgramStep arduino.step => halui.program.step
+
+ # Touch probe
+unlinkp motion.probe-input
+net probe arduino.probe => motion.probe-input
+
+ # Spindle power
+unlinkp motion.spindle-on
+net spindle-On arduino.spindleEnable <= motion.spindle-on
+
+ # Spindle direction
+unlinkp motion.spindle-reverse
+net spindle-Dir arduino.spindleDirection <= motion.spindle-reverse
+
+ # Spindle at speed
+unlinkp motion.spindle-at-speed
+net spindle-at-speed arduino.spindleAtSpeed => motion.spindle-at-speed
+
+ # Coolant flood
+unlinkp iocontrol.0.coolant-flood
+net flood arduino.coolantFlood <= iocontrol.0.coolant-flood
+
+ # Coolant mist
+unlinkp iocontrol.0.coolant-mist
+net mist arduino.coolantMist <= iocontrol.0.coolant-mist
+
+
+###############################################################################
+##### Now on to the axies, if you don't have a given axis comment out the #####
+##### related section entirely. If you don't, EMC will crash on load. #####
+##### (But will give you useful debug info to guide you.) #####
+###############################################################################
+##### If you just want the switches for an axis, comment out the lines #####
+##### containing the word 'motor' in them for the axies you DO have. #####
+###############################################################################
+
+### X-axis (axis.0) ###
+unlinkp axis.0.home-sw-in
+net home-X arduino.xHome => axis.0.home-sw-in
+
+unlinkp axis.0.neg-lim-sw-in
+net min-X => arduino.xMinLmt => axis.0.neg-lim-sw-in
+
+unlinkp axis.0.pos-lim-sw-in
+net max-X => arduino.xMaxLmt => axis.0.pos-lim-sw-in
+
+unlinkp axis.0.motor-pos-cmd
+net xpos-cmd axis.0.motor-pos-cmd => arduino.axis0-cmd
+
+
+### Y-axis (axis.1) ###
+unlinkp axis.1.home-sw-in
+net home-Y arduino.yHome => axis.1.home-sw-in
+
+unlinkp axis.1.neg-lim-sw-in
+net min-Y => arduino.yMinLmt => axis.1.neg-lim-sw-in
+
+unlinkp axis.1.pos-lim-sw-in
+net max-Y => arduino.yMaxLmt => axis.1.pos-lim-sw-in
+
+unlinkp axis.1.motor-pos-cmd
+net ypos-cmd axis.1.motor-pos-cmd => arduino.axis1-cmd
+
+
+### Z-axis (axis.2) ###
+unlinkp axis.2.home-sw-in
+net home-Z arduino.zHome => axis.2.home-sw-in
+
+unlinkp axis.2.neg-lim-sw-in
+net min-Z => arduino.zMinLmt => axis.2.neg-lim-sw-in
+
+unlinkp axis.2.pos-lim-sw-in
+net max-Z => arduino.zMaxLmt => axis.2.pos-lim-sw-in
+
+unlinkp axis.2.motor-pos-cmd
+net zpos-cmd axis.2.motor-pos-cmd => arduino.axis2-cmd
+
+
+### A-axis (axis.3) ###
+unlinkp axis.3.home-sw-in
+net home-A arduino.aHome => axis.3.home-sw-in
+
+unlinkp axis.3.neg-lim-sw-in
+net min-A => arduino.aMinLmt => axis.3.neg-lim-sw-in
+
+unlinkp axis.3.pos-lim-sw-in
+net max-A => arduino.aMaxLmt => axis.3.pos-lim-sw-in
+
+unlinkp axis.3.motor-pos-cmd
+net apos-cmd axis.3.motor-pos-cmd => arduino.axis3-cmd
+
+
+### B-axis (axis.4) ###
+#unlinkp axis.4.home-sw-in
+#net home-B arduino.bHome => axis.4.home-sw-in
+
+#unlinkp axis.4.neg-lim-sw-in
+#net min-B => arduino.bMinLmt => axis.4.neg-lim-sw-in
+
+#unlinkp axis.4.pos-lim-sw-in
+#net max-B => arduino.bMaxLmt => axis.4.pos-lim-sw-in
+
+#unlinkp axis.4.motor-pos-cmd
+#net bpos-cmd axis.4.motor-pos-cmd => arduino.axis4-cmd
+
+
+### C-axis (axis.5) ###
+#unlinkp axis.5.home-sw-in
+#net home-C arduino.cHome => axis.5.home-sw-in
+
+#unlinkp axis.5.neg-lim-sw-in
+#net min-C => arduino.cMinLmt => axis.5.neg-lim-sw-in
+
+#unlinkp axis.5.pos-lim-sw-in
+#net max-C => arduino.cMaxLmt => axis.5.pos-lim-sw-in
+
+#unlinkp axis.5.motor-pos-cmd
+#net cpos-cmd axis.5.motor-pos-cmd => arduino.axis5-cmd
+
+
+### U-axis (axis.6) ###
+#unlinkp axis.6.home-sw-in
+#net home-U arduino.uHome => axis.6.home-sw-in
+
+#unlinkp axis.6.neg-lim-sw-in
+#net min-U => arduino.uMinLmt => axis.6.neg-lim-sw-in
+
+#unlinkp axis.6.pos-lim-sw-in
+#net max-U => arduino.uMaxLmt => axis.6.pos-lim-sw-in
+
+#unlinkp axis.6.motor-pos-cmd
+#net upos-cmd axis.6.motor-pos-cmd => arduino.axis6-cmd
+
+
+### V-axis (axis.7) ###
+#unlinkp axis.7.home-sw-in
+#net home-V arduino.vHome => axis.7.home-sw-in
+
+#unlinkp axis.7.neg-lim-sw-in
+#net min-V => arduino.vMinLmt => axis.7.neg-lim-sw-in
+
+#unlinkp axis.7.pos-lim-sw-in
+#net max-V => arduino.vMaxLmt => axis.7.pos-lim-sw-in
+
+#unlinkp axis.7.motor-pos-cmd
+#net vpos-cmd axis.7.motor-pos-cmd => arduino.axis7-cmd
+
+
+### W-axis (axis.8) ###
+#unlinkp axis.8.home-sw-in
+#net home-W arduino.wHome => axis.8.home-sw-in
+
+#unlinkp axis.8.neg-lim-sw-in
+#net min-W => arduino.wMinLmt => axis.8.neg-lim-sw-in
+
+#unlinkp axis.8.pos-lim-sw-in
+#net max-W => arduino.wMaxLmt => axis.8.pos-lim-sw-in
+
+#unlinkp axis.7.motor-pos-cmd
+#net wpos-cmd axis.8.motor-pos-cmd => arduino.axis8-cmd
+
View
136 pre-release/EmcArduino_07b/example.my-mill.ini
@@ -0,0 +1,136 @@
+# This is an example .ini for a 20in x 20in x 20in CNC w/rotary chuck.
+# I do not intend for you to use this file as a working config, though
+# you may if you so chose. Its just a copy of my actual current config.
+
+# Generated by stepconf at Sun Mar 18 14:52:32 2012
+# If you make changes to this file, they will be
+# overwritten when you run stepconf again
+
+[EMC]
+MACHINE = my-mill
+DEBUG = 0
+
+[DISPLAY]
+DISPLAY = axis
+# DISPLAY = mini
+EDITOR = gedit
+POSITION_OFFSET = RELATIVE
+POSITION_FEEDBACK = ACTUAL
+MAX_FEED_OVERRIDE = 2.5
+INTRO_GRAPHIC = emc2.gif
+INTRO_TIME = 1
+PROGRAM_PREFIX = /home/dewy/emc2/nc_files
+INCREMENTS = .1in .05in .01in .005in .001in .0005in .0001in
+PYVCP = custompanel.xml
+
+[FILTER]
+PROGRAM_EXTENSION = .png,.gif,.jpg Greyscale Depth Image
+PROGRAM_EXTENSION = .py Python Script
+png = image-to-gcode
+gif = image-to-gcode
+jpg = image-to-gcode
+py = python
+
+[TASK]
+TASK = milltask
+CYCLE_TIME = 0.010
+
+[RS274NGC]
+PARAMETER_FILE = emc.var
+
+[EMCMOT]
+EMCMOT = motmod
+COMM_TIMEOUT = 1.0
+COMM_WAIT = 0.010
+BASE_PERIOD = 24000
+SERVO_PERIOD = 1000000
+
+[HAL]
+HALUI = halui
+HALFILE = my-mill.hal
+HALFILE = custom.hal
+POSTGUI_HALFILE = custom_postgui.hal
+
+[HALUI]
+# add halui MDI commands here (max 64)
+
+[TRAJ]
+AXES = 4
+COORDINATES = X Y Z A
+MAX_ANGULAR_VELOCITY = 2.166666667
+DEFAULT_ANGULAR_VELOCITY = 1.8
+LINEAR_UNITS = inch
+ANGULAR_UNITS = degree
+CYCLE_TIME = 0.001
+DEFAULT_VELOCITY = 1.8
+MAX_LINEAR_VELOCITY = 2.166666667
+
+[EMCIO]
+EMCIO = io
+CYCLE_TIME = 0.100
+TOOL_TABLE = tool.tbl
+
+[AXIS_0]
+TYPE = LINEAR
+HOME = 2.0
+MAX_VELOCITY = 2.166666667
+MAX_ACCELERATION = 5
+STEPGEN_MAXACCEL = 9
+SCALE = 1.0
+FERROR = 0.05
+MIN_FERROR = 0.01
+MIN_LIMIT = -5.0
+MAX_LIMIT = 15.0
+HOME_OFFSET = 0.0
+HOME_SEARCH_VEL = .5
+HOME_LATCH_VEL = .1
+HOME_SEQUENCE = 1
+
+[AXIS_1]
+TYPE = LINEAR
+HOME = 2.0
+MAX_VELOCITY = 2.166666667
+MAX_ACCELERATION = 5
+STEPGEN_MAXACCEL = 9
+SCALE = 1.0
+FERROR = 0.05
+MIN_FERROR = 0.01
+MIN_LIMIT = -5.0
+MAX_LIMIT = 15.0
+HOME_OFFSET = 0.0
+HOME_SEARCH_VEL = .5
+HOME_LATCH_VEL = .1
+HOME_SEQUENCE = 2
+
+[AXIS_2]
+TYPE = LINEAR
+HOME = 2.0
+MAX_VELOCITY = 2.166666667
+MAX_ACCELERATION = 5
+STEPGEN_MAXACCEL = 9
+SCALE = 1.0
+FERROR = 0.05
+MIN_FERROR = 0.01
+MIN_LIMIT = -5.0
+MAX_LIMIT = 15.0
+HOME_OFFSET = 0.0
+HOME_SEARCH_VEL = .5
+HOME_LATCH_VEL = .1
+HOME_SEQUENCE = 0
+
+[AXIS_3]
+#TYPE = LINEAR
+TYPE = ANGULAR
+HOME = 2.0
+MAX_VELOCITY = 2.166666667
+MAX_ACCELERATION = 5
+STEPGEN_MAXACCEL = 9
+SCALE = 1.0
+FERROR = 1
+MIN_FERROR = .25
+MIN_LIMIT = -9999.0
+MAX_LIMIT = 9999.0
+HOME_OFFSET = 0.000000
+HOME_SEARCH_VEL = .5
+HOME_LATCH_VEL = 0.10000
+HOME_SEQUENCE = 3
Please sign in to comment.
Something went wrong with that request. Please try again.