Permalink
Browse files

Version 0.07b Released.

  • Loading branch information...
1 parent df22984 commit f71e2ae8426c99799cb1cac09e5675b543b31dce @dewy721 committed Apr 10, 2012
Binary file not shown.
View
@@ -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
@@ -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.
}
Oops, something went wrong.

0 comments on commit f71e2ae

Please sign in to comment.