#define Baud (115200)
#define Max_Buf (64)
#define MIN_STEP_DELAY (50)
#define MAX_FEEDRATE (1000000/MIN_STEP_DELAY)
#define MIN_FEEDRATE (0.01)
#define NUM_AXIES (2) //tengelyek száma
#define wire (1) //1 mm /rotarion //menet emelkedés
#define steps (200)//200 step/rotation lépésszám
// for arc directions
#define ARC_CW (1)
#define ARC_CCW (-1)
#define VERBOSE
// Arcs are split into many line segments. How long are the segments?
#define MM_PER_SEGMENT (20)
float unit=wire/steps;
float dt=0;
char buffer[Max_Buf];
int sofar;
float fr=0; // human version
long step_delay; // machine version
float px,py,pz; // position
char mode_abs=1; // absolute mode?
//------------------------------------------------------------------------------
// STRUCTS
//------------------------------------------------------------------------------
// for line()
typedef struct {
long delta;
long absdelta;
int dir;
long over;
} Axis;
Axis a[3]; // for line()
//Axis atemp; // for line()
class motor
{
public:
motor(int st_pin, int dir_pin);//,float _speed);
void begin();
void inc();
void dec();
//void sp();
bool status();
private:
int _st_pin;
int _dir_pin;
bool _status;
};
motor::motor(int st_pin, int dir_pin)
{
_st_pin=st_pin;
_dir_pin=dir_pin;
}
void motor::begin()
{
pinMode(_st_pin,OUTPUT);
pinMode(_dir_pin,OUTPUT);
}
void motor::inc()
{
digitalWrite(_dir_pin,HIGH);
digitalWrite(_st_pin,HIGH);//!digitalRead(_st_pin));
digitalWrite(_st_pin,LOW);//!digitalRead(_st_pin));
_status=1;
}
void motor::dec()
{
digitalWrite(_dir_pin,LOW);
digitalWrite(_st_pin,HIGH);//!digitalRead(_st_pin));
digitalWrite(_st_pin,LOW);//!digitalRead(_st_pin));
_status=0;
}
bool motor::status()
{
return _status;
}
void position(float _x, float _y, float _z)
{
px=_x;
py=_y;
pz=_z;
}
// motor Step_pins/Dir_pins
motor MX(8,9);
motor MY(10,11);
motor MZ(12,7);
void setup() {
// put your setup code here, to run once:
Serial.begin(Baud);
MX.begin();
MY.begin();
MZ.begin();
pinMode(13,OUTPUT);
help(); // say hello
position(0,0,0); // set staring position
feedrate(200); // set default speed
Serial.println("OK");
}
// put your main code here, to run repeatedly:
// listen for serial commands
void loop()
{
// listen for serial commands
while(Serial.available() > 0) { // if something is available
Serial.println("Se OK");
char c=Serial.read(); // get it
Serial.print(c); // repeat it back so I know you got the message
if(sofar<Max_Buf) buffer[sofar++]=c; // store it
if(buffer[sofar-1]=='\n') break; // entire message received
}
if(sofar>0 && buffer[sofar-1]=='\n') {
// we got a message and it ends with a semicolon
buffer[sofar]=0; // end the buffer so string functions work right
Serial.print(F("\r\n")); // echo a return character for humans
processCommand(); // do something with the command
ready();
}
}
// This method assumes the limits have already been checked.
// This method assumes the start and end radius match.
// This method assumes arcs are not >180 degrees (PI radians)
// cx/cy - center of circle
// x/y - end position
// dir - ARC_CW or ARC_CCW to control direction of arc
static void arc(float cx,float cy,float x,float y,float dir) {
// get radius
float dx = px - cx;
float dy = py - cy;
float radius=sqrt(dx*dx+dy*dy);
// find angle of arc (sweep)
float angle1=atan3(dy,dx);
float angle2=atan3(y-cy,x-cx);
float theta=angle2-angle1;
if(dir>0 && theta<0) angle2+=2*PI;
else if(dir<0 && theta>0) angle1+=2*PI;
theta=angle2-angle1;
// get length of arc
// float circ=PI*2.0*radius;
// float len=theta*circ/(PI*2.0);
// simplifies to
float len = abs(theta) * radius;
int i, segments = ceil( len * MM_PER_SEGMENT );
float nx, ny, angle3, scale;
for(i=0;i<segments;++i) {
// interpolate around the arc
scale = ((float)i)/((float)segments);
angle3 = ( theta * scale ) + angle1;
nx = cx + cos(angle3) * radius;
ny = cy + sin(angle3) * radius;
// send it to the planner
line(nx,ny,pz);
}
line(x,y,pz);
}
// returns angle of dy/dx as a value from 0...2PI
static float atan3(float dy,float dx) {
float a=atan2(dy,dx);
if(a<0) a=(PI*2.0)+a;
return a;
}
//Line void
void line(float newx,float newy,float newz) {
a[0].delta = (newx-px)/unit;
a[1].delta = (newy-py)/unit;
a[2].delta = (newz-pz)/unit;
Serial.print("Ide");
long i,j,maxsteps=0;
for(i=0;i<NUM_AXIES;i++) {
a[i].dir=a[i].delta > 0 ? 1:-1;
a[i].absdelta = abs(a[i].delta);
if( a[i].absdelta> maxsteps ) maxsteps = a[i].absdelta;
a[i].over=0;
Serial.println(NUM_AXIES);
}
a[0].dir=-a[0].dir; // because the motors are mounted in opposite directions
a[1].dir=-a[1].dir; // because the motors are mounted in opposite directions
a[2].dir=-a[2].dir; // because the motors are mounted in opposite directions
//#ifdef VERBOSE
Serial.println(F("Start >"));
//#endif
Serial.print(maxsteps);
for( i=0; i<maxsteps; i++ ) {
for(j=0;j<NUM_AXIES;j++) {
a[j].over += a[j].absdelta;
//Serial.println(maxsteps);
if(a[j].over >= maxsteps) {
a[j].over -= maxsteps;
onestep(j,a[j].dir);
digitalWrite(13,!digitalRead(13));
//Serial.print(j+" :"+a[j].dir);
}
}
pause(step_delay);
}
//macro
/*#ifdef VERBOSE
Serial.println(F("< Done."));
// Serial.println("< Done.");
#endif
*/
position(newx,newy,newz);
}
void pause(long ms) {
delay(ms/1000);
delayMicroseconds(ms%1000); // delayMicroseconds doesn't work for values > ~16k.
}
void onestep(int motor,int direction) {
#ifdef VERBOSE
char *letter="XYZ";
Serial.print(letter[motor]);
#endif
switch(motor)
{
case 0://x
if (direction==1) MX.inc();
else MX.dec();
break;
case 1://y
if (direction==1) MY.inc();
else MY.dec();
break;
case 2://Z
if (direction==1) MZ.inc();
else MZ.dec();
break;
default: break;
}
}
void help() {
Serial.print(F("Joe_CNC "));
//Serial.println(VERSION);
Serial.println(F("Commands:"));
Serial.println(F("G00/G01 [X/Y/Z(steps)] [F(feedrate)]; - linear move"));
Serial.println(F("G02 [X(steps)] [Y(steps)] [I(steps)] [J(steps)] [F(feedrate)]; - clockwise arc"));
Serial.println(F("G03 [X(steps)] [Y(steps)] [I(steps)] [J(steps)] [F(feedrate)]; - counter-clockwise arc"));
Serial.println(F("G04 P[seconds]; - delay"));
Serial.println(F("G90; - absolute mode"));
Serial.println(F("G91; - relative mode"));
Serial.println(F("G92 [X/Y/Z(steps)]; - change logical position"));
Serial.println(F("M18; - disable motors"));
Serial.println(F("M100; - this help message"));
Serial.println(F("M114; - report position and feedrate"));
//digitalWrite(13,!digitalRead(13));
}
float parsenumber(char code,float val) {
char *ptr=buffer;
while(ptr && *ptr && ptr<buffer+sofar) {
if(*ptr==code) {
return atof(ptr+1);
}
ptr=strchr(ptr,' ')+1;
}
return val;
}
void release() {
int i;
for(i=0;i<NUM_AXIES;++i) {
//m[i]->release();
}
}
void output(char *code,float val) {
Serial.print(code);
Serial.println(val);
}
// Set the feedrate (speed motors will move)
void feedrate(float nfr) {
if(fr==nfr) return; // same as last time? quit now.
if(nfr>MAX_FEEDRATE || nfr<MIN_FEEDRATE) { // don't allow crazy feed rates
Serial.print(F("New feedrate must be greater than "));
Serial.print(MIN_FEEDRATE);
Serial.print(F("steps/s and less than "));
Serial.print(MAX_FEEDRATE);
Serial.println(F("steps/s."));
return;
}
step_delay = 1000000.0/nfr;
fr=nfr;
}
void ready() {
sofar=0; // clear input buffer
Serial.print(F(">")); // signal ready to receive input
}
void where() {
output("X",px);
output("Y",py);
output("Z",pz);
output("F",fr);
Serial.println(mode_abs?"ABS":"REL");
}
float frtemp=0;
void processCommand()
{
int cmd=parsenumber('G',-1);
switch(cmd){
case 0:
frtemp=fr;
feedrate(MAX_FEEDRATE);
line( parsenumber('X',(mode_abs?px:0)) + (mode_abs?0:px),
parsenumber('Y',(mode_abs?py:0)) + (mode_abs?0:py),
parsenumber('Z',(mode_abs?pz:0)) + (mode_abs?0:pz));
feedrate(frtemp);
break;
case 1:
feedrate(parsenumber('F',fr));
line( parsenumber('X',(mode_abs?px:0)) + (mode_abs?0:px),
parsenumber('Y',(mode_abs?py:0)) + (mode_abs?0:py),
parsenumber('Z',(mode_abs?pz:0)) + (mode_abs?0:pz));
break;
case 2:
case 3: { // arc
feedrate(parsenumber('F',fr));
arc(parsenumber('I',(mode_abs?px:0)) + (mode_abs?0:px),
parsenumber('J',(mode_abs?py:0)) + (mode_abs?0:py),
parsenumber('X',(mode_abs?px:0)) + (mode_abs?0:px),
parsenumber('Y',(mode_abs?py:0)) + (mode_abs?0:py),
(cmd==2) ? -1 : 1);
break;
}
case 20: unit=wire/steps/25,4; break; // G20 unit(inch)
case 21: unit=wire/steps; break; // G21 unit(mm)
case 90: mode_abs=1; break; // absolute mode
case 91: mode_abs=0; break; // relative mode
case 4: pause(parsenumber('P',0)*1000); break; // dwell
case 92: // set logical position
position( parsenumber('X',0), parsenumber('Y',0),parsenumber('Z',0) );
break;
default:break;
}
cmd = parsenumber('M',-1);
switch(cmd) {
case 18: // disable motors
// release();
break;
case 100: help(); break;
case 114: where(); break;
default: break;
}
}