// https://wokwi.com/projects/381581240931217409
// David Forrest 2023-11-16
//------------------------------------------------------------------------------
// 4 Axis CNC Demo - supports CNCShieldV3 on arduino UNO
// [email protected] 2013-10-28
// Modified by Søren Vedel
// [email protected] 2015-06-19
// Modified by Cristian Leiva
// [email protected] 2016-04-28
// add Support cncshield - feed rate in mm/min
//------------------------------------------------------------------------------
// Copyright at end of file.
// please see http://www.github.com/MarginallyClever/GcodeCNCDemo for more information.
//------------------------------------------------------------------------------
// CONSTANTS
//------------------------------------------------------------------------------
//#define VERBOSE (1) // add to get a lot more serial output.
#define VERSION (2) // firmware version
#define BAUD (115200) // How fast is the Arduino talking?(BAUD Rate of Arduino)
#define MAX_BUF (64) // What is the longest message Arduino can store?
#define STEPS_PER_TURN (200) // depends on your stepper motor. most are 200.
#define STEPS_PER_MM (200+0*STEPS_PER_TURN*1/0.8) // (400*16)/0.8 with a M5 spindle
#define MAX_FEEDRATE (10000) // steps/sec
#define MIN_FEEDRATE (1) // steps/sec
#define NUM_AXIES (4)
#define US_PER_SEC (1000000UL) // microsecs/sec
#define MIN_US_PER_STEP (US_PER_SEC/MAX_FEEDRATE) //
#define INITIAL_STEP_US (12000)
#define ACCEL_PER_STEP (40)
//------------------------------------------------------------------------------
// STRUCTS
//------------------------------------------------------------------------------
// for line()
typedef struct {
long delta; // number of steps to move
long absdelta;
long over; // for dx/dy bresenham calculations
} Axis;
typedef struct {
int step_pin;
int dir_pin;
int enable_pin;
int limit_switch_pin;
} Motor;
//------------------------------------------------------------------------------
// GLOBALS
//------------------------------------------------------------------------------
Axis a[NUM_AXIES]; // for line()
Axis atemp; // for line()
Motor motors[NUM_AXIES];
char buffer[MAX_BUF]; // where we store the message until we get a ';'
int sofar; // how much is in the buffer
// speeds
float fr=0; // human version
long step_delay; // machine version
float px,py,pz,pe; // position
// settings
char mode_abs=1; // absolute mode?
long line_number=0;
//------------------------------------------------------------------------------
// METHODS
//------------------------------------------------------------------------------
/**
* delay for the appropriate number of microseconds
* @input ms how many milliseconds to wait
*/
void pause(unsigned long us) {
delay(us/1000);
delayMicroseconds(us%1000); // delayMicroseconds doesn't work for values > ~16k.
}
/**
* Set the feedrate (speed motors will move)
* @input nfr the new speed in steps/second
*
*/
void feedrate(float nfr) {
// nfr = nfr*STEPS_PER_MM/60; // step/sec * step/mm / (60 sec/min)
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 = MAX_FEEDRATE/nfr;
fr=nfr;
Serial.print("step_delay:");
Serial.println(step_delay);
Serial.print("ms? nfr:");
Serial.println(nfr);
}
/**
* Set the logical position
* @input npx new position x
* @input npy new position y
*/
void position(float npx,float npy,float npz,float npe) {
// here is a good place to add sanity tests
px=npx;
py=npy;
pz=npz;
pe=npe;
}
/**
* Supports movement with both styles of Motor Shield
* @input newx the destination x position
* @input newy the destination y position
**/
void onestep(int motor) {
#ifdef VERBOSE
char *letter="XYZE";
Serial.print(letter[]);
#endif
digitalWrite(motors[motor].step_pin,HIGH);
digitalWrite(motors[motor].step_pin,LOW);
}
/**
* Uses bresenham's line algorithm to move both motors
* @input newx the destination x position
* @input newy the destination y position
**/
void line(float newx,float newy,float newz,float newe) {
a[0].delta = (newx-px)*STEPS_PER_MM;
a[1].delta = (newy-py)*STEPS_PER_MM;
a[2].delta = (newz-pz)*STEPS_PER_MM;
a[3].delta = (newe-pe)*STEPS_PER_MM;
long i,j,maxsteps=0;
for(i=0;i<NUM_AXIES;++i) {
a[i].absdelta = abs(a[i].delta);
a[i].over=0;
if( maxsteps < a[i].absdelta ) maxsteps = a[i].absdelta;
// set the direction once per movement
digitalWrite(motors[i].dir_pin,a[i].delta>0?HIGH:LOW);
}
// seems confused at https://github.com/MarginallyClever/GcodeCNCDemo/blob/3af2254eb5b550b015b14ec4eec294b16c306087/GcodeCNCDemo4AxisCNCShield/GcodeCNCDemo4AxisCNCShield.ino#L158
// Should determine dt_(us) between steps based on feedrate and number of steps
//unsigned long dt = MAX_FEEDRATE/5000; // us
unsigned long target_dt = US_PER_SEC/fr; //
unsigned long dt = INITIAL_STEP_US; // initial step delay us
long accel = ACCEL_PER_STEP; // us to decrease per step
// long steps_to_accel = (dt - step_delay)/accel;
long steps_to_accel = (dt - target_dt)/accel;
if(steps_to_accel < 1)
steps_to_accel = 1;
if(steps_to_accel > maxsteps/2 )
steps_to_accel = maxsteps/2;
long steps_to_decel = maxsteps - steps_to_accel;
Serial.print("maxsteps:");
Serial.println(maxsteps);
Serial.print("STARTdt (us):");
Serial.println(dt);
Serial.print("STOP ");
Serial.print(target_dt);
Serial.print("us -> ");
Serial.print(1.0e6/target_dt/STEPS_PER_TURN );
Serial.println(" Rev/sec");
Serial.print("accel until ");
Serial.println(steps_to_accel);
Serial.print("decel after ");
Serial.println(steps_to_decel);
Serial.print("total ");
Serial.println(maxsteps);
#ifdef VERBOSE
Serial.println(F("Start >"));
#endif
for( i=0; i<maxsteps; ++i ) {
for(j=0;j<NUM_AXIES;++j) {
a[j].over += a[j].absdelta;
if(a[j].over >= maxsteps) {
a[j].over -= maxsteps;
digitalWrite(motors[j].step_pin,HIGH);
digitalWrite(motors[j].step_pin,LOW);
}
}
if(i<steps_to_accel) {
dt -= accel;
if (dt < MIN_US_PER_STEP) dt = MIN_US_PER_STEP;
}
if(i>=steps_to_decel) {
dt += accel;
}
// delayMicroseconds(dt);
pause(dt);
}
#ifdef VERBOSE
Serial.println(F("< Done."));
#endif
position(newx,newy,newz,newe);
where();
}
// 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;
}
/**
* Look for character /code/ in the buffer and read the float that immediately follows it.
* @return the value found. If nothing is found, /val/ is returned.
* @input code the character to look for.
* @input val the return value if /code/ is not found.
**/
float parseNumber(char code,float def) {
char *ptr=buffer; // start at the beginning of buffer
while((long)ptr > 1 && (*ptr) && (long)ptr < (long)buffer+sofar) { // walk to the end
if(*ptr==code) { // if you find code on your walk,
return atof(ptr+1); // convert the digits that follow into a float and return it
}
ptr=strchr(ptr,' ')+1; // take a step from here to the letter after the next space
}
return def; // end reached, nothing found, return default def.
}
/**
* write a string followed by a float to the serial line. Convenient for debugging.
* @input code the string.
* @input val the float.
*/
void output(char *code,float val) {
Serial.print(code);
Serial.print(val);
Serial.print(" ");
}
/**
* print the current position, feedrate, and absolute mode.
*/
void where() {
output("X",px);
output("Y",py);
output("Z",pz);
output("E",pe);
output("F",fr/STEPS_PER_MM*60);
Serial.println(mode_abs?"ABS":"REL");
}
/**
* display helpful information
*/
void help() {
Serial.print(F("GcodeCNCDemo6AxisV2 "));
Serial.println(VERSION);
Serial.println(F("Commands:"));
Serial.println(F("G00/G01 [X/Y/Z/E(steps)] [F(feedrate)]; - linear move"));
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/E(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"));
Serial.println(F("All commands must end with a newline."));
Serial.println(F("Try G1 X10 Y20 Z30 E40 F60"));
}
/**
* Read the input buffer and find any recognized commands. One G or M command per line.
*/
void processCommand() {
int cmd = parseNumber('G',-1);
switch(cmd) {
case 0:
case 1: { // line
feedrate(parseNumber('F',fr)*STEPS_PER_MM/60); //
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),
parseNumber('E',(mode_abs?pe:0)) + (mode_abs?0:pe) );
break;
}
case 2:
case 4: pause(parseNumber('P',0)*1000); break; // dwell
case 90: mode_abs=1; break; // absolute mode
case 91: mode_abs=0; break; // relative mode
case 92: // set logical position
position( parseNumber('X',0),
parseNumber('Y',0),
parseNumber('Z',0),
parseNumber('E',0) );
break;
default: break;
}
cmd = parseNumber('M',-1);
switch(cmd) {
case 17: motor_enable(); break;
case 18: motor_disable(); break;
case 100: help(); break;
case 114: where(); break;
default: break;
}
}
/**
* prepares the input buffer to receive a new message and tells the serial connected device it is ready for more.
*/
void ready() {
sofar=0; // clear input buffer
Serial.print(F(">")); // signal ready to receive input
}
/**
* set up the pins for each motor
* Pins fits a CNCshieldV3.xx
*/
void motor_setup() {
motors[0].step_pin=2;
motors[0].dir_pin=5;
motors[0].enable_pin=8;
motors[0].limit_switch_pin=9;
motors[1].step_pin=3;
motors[1].dir_pin=6;
motors[1].enable_pin=8;
motors[1].limit_switch_pin=10;
motors[2].step_pin=4;
motors[2].dir_pin=7;
motors[2].enable_pin=8;
motors[2].limit_switch_pin=11;
motors[3].step_pin=12;
motors[3].dir_pin=13;
motors[3].enable_pin=8;
motors[3].limit_switch_pin=11;
int i;
for(i=0;i<NUM_AXIES;++i) {
// set the motor pin & scale
pinMode(motors[i].step_pin,OUTPUT);
pinMode(motors[i].dir_pin,OUTPUT);
pinMode(motors[i].enable_pin,OUTPUT);
}
}
void motor_enable() {
int i;
for(i=0;i<NUM_AXIES;++i) {
digitalWrite(motors[i].enable_pin,LOW);
}
}
void motor_disable() {
int i;
for(i=0;i<NUM_AXIES;++i) {
digitalWrite(motors[i].enable_pin,HIGH);
}
}
/**
* First thing this machine does on startup. Runs only once.
*/
void setup() {
Serial.begin(BAUD); // open coms
motor_setup();
motor_enable();
where(); // for debugging purposes
help(); // say hello
position(0,0,0,0); // set starting position
feedrate(1000); // set default speed
ready();
}
/**
* After setup() this machine will repeat loop() forever.
*/
void loop() {
// listen for serial commands
while(Serial.available() > 0) { // if something is available
char c=Serial.read(); // get it
Serial.print(c); // repeat it back so I know you got the message
if(sofar<MAX_BUF-1) buffer[sofar++]=c; // store it
if(c=='\n') {
// entire message received
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 file is part of GcodeCNCDemo.
*
* GcodeCNCDemo is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GcodeCNCDemo 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 Foobar. If not, see <http://www.gnu.org/licenses/>.
*/