// see https://www.youtube.com/edit?o=U&video_id=7-xpBwuLIoI
#include <AccelStepper.h>
#include <LiquidCrystal.h>
#include <stdlib.h>
/* V 3.1 - incl acceleration and threading */
/* V 3.2 - incl cross slide power button
/* V 3.3 - change (Q)uiet to (S)erial feedback, add debug level 2 for feedback
/* V 3.4 - threading performance changes
/* V 3.5 - changed code order for performance and threading speed to 2200
/* V 3.6 - send back RPM
/* V 3.7 - add abort routine
/* blue wire is gnd */
// leadscrew
#define DIR_L_PIN 9 // to direction+ on DQ542MA green
#define STEP_L_PIN 10 // to pulse+ on DQ542MA orange
// crosslide
#define DIR_C_PIN 7 // to direction+ on DQ542MA green
#define STEP_C_PIN 6 // to pulse+ on DQ542MA orange
#define ENABLE_L_PIN 8 // to enable+ on DQ542MA white
#define ENABLE_C_PIN 16 // to enable+ on DQ542MA white
#define waitPin 11 //q to switch
#define joyLeft 12
#define joyRight 5
#define joyIn 3
#define joyOut 4
#define powerpin 14
/*
cross and lead directions - facing lathe
+
+
----- ++++++
-
-
Commands :
Can use
R Query RPM
M1, M0 Mains power on SSR on or off - controls stepper power supply and lathe power
E1, E0 Enable/disable cross slide stepper (1 = enable)
Lxxx;Syyy; leadscrew xxx steps, at yyy per second - positive is right, negative left
Cxx;Syyy; crosslide xxx steps, at yyy per second - positive is in, negative out
Dxxx; at string start, repeat entire string xxx times (duplicate)
Bxxx;Syyy;Szzz go leadscrew left xxx steps at yyy per second, crossslide in at zzz per second (tied to leadscrew)
(if xxx is negative then right - if zzz is negative then out)
Tx;Pyyy;Lzzz; x=0 means turn thread starting at left, 1=from right
yyy=length of one thread in mm (eg 8 per inch = 3.175) - tied to one turn of spindle as measured by encoder
zzz=distance in mm to thread
K cut motors
Wxxx; wait xxx milliseconds
W0; wait for button on waitPin to be pressed
Xn.n; Set X
Zn.n; Set Z
Ax;Sy; Acceleration : x1=leadscrew x2=crosslide y=factor to multiply speed by
B command will not use acceleration
S0 means do not use acceleration
S Serial feedback 0=disable 1=enable, 2=debug as well (0 means qonly sends response after full command string is consumed
J joystick 0=disable 1=enable
# end of command string
*/
AccelStepper stepper1(1,STEP_L_PIN, DIR_L_PIN); // leadscrew
AccelStepper stepper2(1,STEP_C_PIN, DIR_C_PIN); // crosslide
const int quad_A = 2;
const int quad_B = 13;
const unsigned int mask_quad_A = digitalPinToBitMask(quad_A);
const unsigned int mask_quad_B = digitalPinToBitMask(quad_B);
const int enc_z = 15;
char commandin[2550];
char rest[2550];
char str[2550];
char comm[2550];
float commVal[2550];
char floatval[2550];
char output1[100];
char output2[40];
int x;
unsigned long d;
int charreader;
int commandstringlength=0;
unsigned long timerStart;
unsigned long timerEnd;
float commVal1;
float commVal2;
float commVal3;
float LAccel=0.0F;
float CAccel=0.0F;
char currentProcess;
int currentCommand=0;
int numCommands=0;
int numDups=0;
int currentDup=0;
boolean processing=false;
boolean allDone=true;
boolean joystick=false;
int serialFeedback=0;
int globalLeadSteps=0;
int globalCrossSteps=0;
unsigned long currMillis=millis();
unsigned long prevMillis=millis();
unsigned long debugMillis=millis();
unsigned long prev_z_millis=millis();
unsigned long z_millis=millis();
int enc_at_z=0;
int waiting_first_z=1;
unsigned int partticks;
unsigned int enc_ticks;
unsigned int enc_steps;
unsigned int enc_steps_req;
unsigned int enc_total_steps_req;
unsigned int enc_ticks_to_steps_x_10000;
// lcd pins are 4 6 11 12 13 14
// connected to the following pins on the Due
LiquidCrystal lcd(45, 43, 53, 51, 49, 47);
void setup() {
pinMode(powerpin,OUTPUT);
digitalWrite(powerpin, LOW);
Serial.begin(115200);
// lcd.begin(20, 4);
//lcdprint1("12345678901234567890");
// lcdprint1("Initialising ");
pinMode(ENABLE_L_PIN,OUTPUT);
digitalWrite(ENABLE_L_PIN, LOW);
pinMode(ENABLE_C_PIN,OUTPUT);
digitalWrite(ENABLE_C_PIN, LOW);
pinMode(waitPin,INPUT);
digitalWrite(waitPin, HIGH); // enable internal resistor
pinMode(joyLeft,INPUT);
digitalWrite(joyLeft, HIGH);
pinMode(joyRight,INPUT);
digitalWrite(joyRight, HIGH);
pinMode(joyIn,INPUT);
digitalWrite(joyIn, HIGH);
pinMode(joyOut,INPUT);
digitalWrite(joyOut, HIGH);
stepper1.setMaxSpeed(3000);
stepper2.setMaxSpeed(3000);
stepper1.setAcceleration(0);
stepper2.setAcceleration(0);
// encoder
// activate peripheral functions for quad pins
REG_PIOB_PDR = mask_quad_A; // activate peripheral function (disables all PIO functionality)
REG_PIOB_ABSR |= mask_quad_A; // choose peripheral option B
REG_PIOB_PDR = mask_quad_B; // activate peripheral function (disables all PIO functionality)
REG_PIOB_ABSR |= mask_quad_B; // choose peripheral option B
// activate clock for TC0
REG_PMC_PCER0 = (1<<27);
// select XC0 as clock source and set capture mode
REG_TC0_CMR0 = 5;
// activate quadrature encoder and position measure mode, no filters
REG_TC0_BMR = (1<<9)|(1<<8)|(1<<12);
// enable the clock (CLKEN=1) and reset the counter (SWTRG=1)
// SWTRG = 1 necessary to start the clock!!
REG_TC0_CCR0 = 5;
pinMode(enc_z, INPUT);
attachInterrupt(enc_z, enc_zrising, RISING);
enc_at_z=0;
// end encoder
delay(1000);
lcd.clear();
//lcdprint1("12345678901234567890");
lcdprint1("Ready ");
}
void loop() {
currMillis=millis();
if(Serial.available()>0) {
charreader = Serial.read();
switch (char(charreader)) {
case 'K':
stepper1.stop();
stepper2.stop();
sendXZImmediate();
Serial.write("PC #"); // command sequence completed
//lcdprint1("12345678901234567890");
lcdprint1("Killed ");
commandstringlength=0;
currentCommand=0;
numDups=0;
currentDup=0;
allDone=true;
processing=false;
break;
case '#':
commandin[commandstringlength] = charreader;
parse();
commandstringlength=0;
break;
default :
commandin[commandstringlength] = charreader;
commandstringlength++;
break;
} // switch
} // Serial.available
// -------------------------------------------------------------------------------------------------
if (processing) {
if (currentProcess=='T') {
enc_ticks=REG_TC0_CV0; /// swap with next line after debugging
// enc_ticks=millis() - debugMillis;
if (waiting_first_z==0) {
enc_steps_req=((enc_ticks-partticks)*10000) / enc_ticks_to_steps_x_10000;
if (enc_steps>=enc_total_steps_req) {
respond_to_pc ("CC #");
incrementCommands();
return;
} // threading completed
if (enc_steps<enc_steps_req) {
if ((enc_steps_req-enc_steps)>1) {
Serial.print("Aborting - threading step backlog>1, steps required=");
Serial.print(enc_steps_req);
Serial.print(" Steps made=");
Serial.println(enc_steps);
abort();
}
stepLeadScrew(commVal1);
return;
} // need to step
} // waiting_first_z==0
if (waiting_first_z==1) {
// enc_at_z=1; /////////////////// take out after debugging
if (enc_at_z==1) {
waiting_first_z=0;
partticks=enc_ticks; // partticks will be a partial revolution until z was reached
} // at z
} // waiting first z, still not arrived
return;
} // currentProcess=='T'
if (currentProcess=='L') {
if (commVal1==0 || (stepper1.distanceToGo()<=0 && commVal1>0) || (stepper1.distanceToGo()>=0 && commVal1<0)) {
respond_to_pc ("CC #");
incrementCommands();
return;
} // done
runMotor(1);
return;
} // L
if (currentProcess=='C') {
if (commVal1==0 || (stepper2.distanceToGo()<=0 && commVal1>0) || (stepper2.distanceToGo()>=0 && commVal1<0)) {
respond_to_pc ("CC #");
incrementCommands();
return;
} // done
runMotor(2);
return;
} // C
if (currentProcess=='B') {
if (commVal1==0 || (stepper1.distanceToGo()<=0 && commVal1>0) || (stepper1.distanceToGo()>=0 && commVal1<0)) {
respond_to_pc("CC #");
incrementCommands();
return;
} // done
runMotor(1);
runMotor(2);
return;
} // B
if (currentProcess=='S') {
serialFeedback=commVal1;
respond_to_pc("CC #");
incrementCommands();
return;
}
if (currentProcess=='R') {
numCommands=0;
return;
}
if (currentProcess=='M') {
if (commVal1==1) {
digitalWrite(powerpin,HIGH);
}
else {
digitalWrite(powerpin,LOW);
}
respond_to_pc("CC #");
incrementCommands();
return;
}
if (currentProcess=='E') {
if (commVal1==1) {
digitalWrite(ENABLE_C_PIN,HIGH);
}
else {
digitalWrite(ENABLE_C_PIN,LOW);
}
respond_to_pc("CC #");
incrementCommands();
return;
}
if (currentProcess=='X') {
respond_to_pc("CC #");
incrementCommands();
return;
}
if (currentProcess=='Z') {
respond_to_pc("CC #");
incrementCommands();
return;
}
if (currentProcess=='J') {
respond_to_pc("CC #");
incrementCommands();
return;
}
if (currentProcess=='A') {
respond_to_pc("CC #");
incrementCommands();
return;
}
if (currentProcess=='W') {
if (commVal1==0) {
if (digitalRead(waitPin)==LOW) {
respond_to_pc("CC #");
incrementCommands();
} // LOW
return;
} // Val1=0
timerEnd=millis();
if ((timerEnd-timerStart)>=commVal1) {
respond_to_pc("CC #");
incrementCommands();
return;
} // done
} // W
} // processing
// -------------------------------------------------------------------------------------------------
if (!processing) {
if (joystick==true) {
if (digitalRead(joyLeft)==LOW) {
stepper1.move(-99999999L);
if (digitalRead(waitPin)==LOW) {
stepper1.setSpeed(-1500);
stepper1.setAcceleration(-1500*LAccel);
}
else {
stepper1.setSpeed(-120);
stepper1.setAcceleration(-120*LAccel);
}
runMotor(1);
}
else if (digitalRead(joyRight)==LOW) {
stepper1.move(99999999L);
if (digitalRead(waitPin)==LOW) {
stepper1.setSpeed(1500);
stepper1.setAcceleration(1500*LAccel);
}
else {
stepper1.setSpeed(120);
stepper1.setAcceleration(120*LAccel);
}
runMotor(1);
}
else if (digitalRead(joyIn)==LOW) {
stepper2.move(99999999L);
if (digitalRead(waitPin)==LOW) {
stepper2.setSpeed(2000);
stepper2.setAcceleration(2000*CAccel);
}
else {
stepper2.setSpeed(100);
stepper2.setAcceleration(100*CAccel);
}
runMotor(2);
}
else if (digitalRead(joyOut)==LOW) {
stepper2.move(-99999999L);
if (digitalRead(waitPin)==LOW) {
stepper2.setSpeed(-2000);
stepper2.setAcceleration(-2000*CAccel);
}
else {
stepper2.setSpeed(-100);
stepper2.setAcceleration(-100*CAccel);
}
runMotor(2);
}
else {
//digitalWrite(13, HIGH);
// sendXZJoystick();
}
} // joystick enabled
// else
// digitalWrite(13, LOW);
if (allDone) {
return;
} // allDone
// -------------------------------------------------------------------------------------------------
processing=true; // not allDone, and not processing - so process next command
currentProcess=comm[currentCommand];
//Serial.println(currentProcess);
if (currentProcess=='W') {
commVal1=commVal[currentCommand];
if (commVal1>0) {
if (serialFeedback>0) {
//lcdprint1("12345678901234567890");
lcdprint1("Wait time ");
Serial.write("WT ");
Serial.print(commVal1,0);
Serial.write(" #");
}
timerStart=millis();
}
else {
if (serialFeedback>0) {
//lcdprint1("12345678901234567890");
lcdprint1("Wait button ");
Serial.write("WB ");
Serial.write("#");
}
}
return;
} // W
if (currentProcess=='R') {
Serial.write("R ");
Serial.print(z_millis - prev_z_millis);
Serial.write(" ");
Serial.print(prev_z_millis);
Serial.write(" ");
Serial.print(z_millis);
Serial.write (" #");
processing=false;
currentProcess=' ';
numCommands=0;
allDone=true;
return;
} // rpm
if (currentProcess=='S') {
commVal1=commVal[currentCommand];
respond_to_pc("S #");
return;
} // set X
if (currentProcess=='M') {
commVal1=commVal[currentCommand];
globalLeadSteps=commVal1;
if (commVal1==0)
Serial.write("M0 #");
else
Serial.write("M1 #");
return;
} // set mains
if (currentProcess=='E') {
commVal1=commVal[currentCommand];
globalLeadSteps=commVal1;
if (commVal1==0)
Serial.write("E0 #");
else
Serial.write("E1 #");
return;
} // set cross slide enabled
if (currentProcess=='Z') {
commVal1=commVal[currentCommand];
globalLeadSteps=commVal1;
respond_to_pc("Z #");
return;
} // set Z
if (currentProcess=='X') {
commVal1=commVal[currentCommand];
globalCrossSteps=commVal1;
respond_to_pc("X #");
return;
} // set X
if (currentProcess=='J') {
commVal1=commVal[currentCommand];
if (commVal1==1) {
joystick=true;
}
else {
joystick=false;
}
Serial.write("J ");
Serial.write("#");
Serial.flush();
return;
} // set joystick on/off
if (currentProcess=='A') {
if (commVal[currentCommand]==1) {
currentCommand++;
LAccel=commVal[currentCommand];
}
if (commVal[currentCommand]==2) {
currentCommand++;
CAccel=commVal[currentCommand];
}
respond_to_pc("A #");
}
if (currentProcess=='T') {
commVal1=commVal[currentCommand];
currentCommand++;
commVal2=commVal[currentCommand];
currentCommand++;
commVal3=commVal[currentCommand];
// ?Multiply encoding factor by 10000, to use integer instead of float for performance
// ?It is divided again when calculating whether a step is needed
enc_total_steps_req=commVal3*214.1732;
// Ticks to steps conversion is calculated as follows:
// 680 = steps per mm (214.1732) x 3.175 mm (1/8 inch or one rev) - so 680 = steps for 1 leadscrew rev
// 2.1176 = ticks per rev (1440 / 680 )
// 21176 is 10000 x conversion factor from ticks to steps
// commVal2 is pitch in mm (3.175=1/8 inch) - larger pitch needs fewer ticks - so dividing by this gives a faster step rate for coarser pitches
enc_ticks_to_steps_x_10000=21176 / commVal2;
sprintf(output1,"Threading enc_ticks_to_steps_x_10000 is %u , total steps required is %u #",enc_ticks_to_steps_x_10000,enc_total_steps_req);
Serial.println(output1);
enc_steps=0;
waiting_first_z=1;
if (commVal1==1) {
digitalWrite(DIR_L_PIN,LOW);
}
else {
digitalWrite(DIR_L_PIN,HIGH);
}
debugMillis=millis();
enc_at_z=0;
} // T
if (currentProcess=='L') {
commVal1=commVal[currentCommand];
currentCommand++;
commVal2=commVal[currentCommand];
stepper1.setCurrentPosition(0);
stepper1.move(commVal1);
stepper1.setSpeed(commVal2);
stepper1.setAcceleration(commVal2*LAccel);
if (commVal1 >0.0) {
//lcdprint1("12345678901234567890");
lcdprint1("Right ");
if (serialFeedback>1) {
sprintf(output1,"LR %0.0f %0.0f #",commVal1,commVal2);
Serial.print(output1);
}
}
else {
//lcdprint1("12345678901234567890");
lcdprint1("Left ");
if (serialFeedback>1) {
sprintf(output1,"LL %0.0f %0.0f #",commVal1*-1.0,commVal2*-1.0);
Serial.print(output1);
}
}
} // L
if (currentProcess=='C') {
commVal1=commVal[currentCommand];
currentCommand++;
commVal2=commVal[currentCommand];
stepper2.setCurrentPosition(0);
stepper2.move(commVal1);
stepper2.setSpeed(commVal2);
stepper2.setAcceleration(commVal2*CAccel);
if (commVal1 >0.0) {
//lcdprint1("12345678901234567890");
lcdprint1("In ");
if (serialFeedback>1) {
sprintf(output1,"CI %0.0f %0.0f #",commVal1,commVal2);
Serial.print(output1);
}
}
else {
//lcdprint1("12345678901234567890");
lcdprint1("Out ");
if (serialFeedback>1) {
sprintf(output1,"CO %0.0f %0.0f #",commVal1*-1.0,commVal2*-1.0);
Serial.print(output1);
}
}
} // C
if (currentProcess=='B') { // both, leadscrew controls the stop
commVal1=commVal[currentCommand];
currentCommand++;
commVal2=commVal[currentCommand];
currentCommand++;
commVal3=commVal[currentCommand];
stepper1.setCurrentPosition(0);
stepper2.setCurrentPosition(0);
stepper1.move(commVal1);
stepper1.setSpeed(commVal2);
stepper1.setAcceleration(0);
if (commVal3>0) {
stepper2.move(99999999L);
}
else {
stepper2.move(-99999999L);
}
stepper2.setSpeed(commVal3);
stepper2.setAcceleration(0);
if (commVal2 >0.0 && commVal3 >0.0) {
//lcdprint1("12345678901234567890");
lcdprint1("In right ");
if (serialFeedback>1) {
sprintf(output1,"BRI %0.0f %0.0f %0.0f#",commVal1,commVal2,commVal3);
Serial.print(output1);
lcdprint2(output1);
}
} // BRI
if (commVal2 >=0.0 && commVal3 <0.0) {
//lcdprint1("12345678901234567890");
lcdprint1("Out right ");
if (serialFeedback>1) {
sprintf(output1,"BRO %0.0f %0.0f %0.0f#",commVal1,commVal2,commVal3*-1.0);
Serial.print(output1);
lcdprint2(output1);
}
} // BRO
if (commVal2 <0.0 && commVal3 >=0.0) {
//lcdprint1("12345678901234567890");
lcdprint1("In left ");
if (serialFeedback>1) {
sprintf(output1,"BLI %0.0f %0.0f %0.0f#",commVal1*-1.0,commVal2*-1.0,commVal3);
Serial.print(output1);
lcdprint2(output1);
}
} // BLI
if (commVal2 <0.0 && commVal3 <0.0) {
//lcdprint1("12345678901234567890");
lcdprint1("Out left ");
if (serialFeedback>1) {
sprintf(output1,"BLO %0.0f %0.0f %0.0f#",commVal1*-1.0,commVal2*-1.0,commVal3*-1.0);
Serial.print(output1);
lcdprint2(output1);
}
} // BLO
} // B
} // !processing
// -----------------------------------------------------------------------------------------------------------------
} // loop
void parse() {
numCommands=0;
currentDup=1;
allDone=false;
processing=false;
while (char(commandin[0])!='#') {
sscanf(commandin,"%[^';']%[^\n]",str,rest);
x=sscanf(str,"%c%s",&comm[numCommands],floatval);
commVal[numCommands]=atof(floatval);
numCommands++;
strcpy(commandin,rest+1);
}
if (comm[0]=='D') {
// Serial.println("Received D");
currentCommand=1;
numDups=commVal[0];
if (numDups==0) {
numDups=1;
}
Serial.write("D 1 #");
}
else {
currentCommand=0;
numDups=1;
}
// Serial.print("numDups=");
// Serial.println(numDups);
// Serial.print("numCommands=");
// Serial.println(numCommands);
} // parse
void incrementCommands() {
processing=false;
currentProcess=' ';
// Serial.print("currentCommand=");
// Serial.println(currentCommand);
// Serial.print("numCommands=");
// Serial.println(numCommands);
if (currentCommand<numCommands) {
currentCommand++;
}
if (currentCommand==numCommands) {
// Serial.print("currentDup=");
// Serial.println(currentDup);
// Serial.print("numDups=");
// Serial.println(numDups);
if (currentDup==numDups) {
allDone=true;
//lcdprint1("12345678901234567890");
// lcdprint1("Ready ");
// lcdprint2(" ");
Serial.write("PC "); // command completed
Serial.print(globalCrossSteps);
Serial.write(" ");
Serial.print(globalLeadSteps);
Serial.write(" #");
return;
}
currentDup++;
if (serialFeedback>0) {
Serial.write("D ");
Serial.print(currentDup);
Serial.write(" #");
}
if (comm[0]=='D') {
currentCommand=1;
}
else {
currentCommand=0;
}
}
}
void lcdprint1(char * msg) {
lcd.setCursor(0,0);
lcd.print(msg);
}
void lcdprint2(char * msg) {
lcd.setCursor(0,1);
lcd.print(" ");
lcd.setCursor(0,1);
lcd.print(msg);
}
void stepLeadScrew(int dir) {
digitalWrite(STEP_L_PIN,HIGH);
delayMicroseconds(1);
digitalWrite(STEP_L_PIN,LOW);
enc_steps++;
if (dir==1) {
globalLeadSteps--;
}
else {
globalLeadSteps++;
}
}
void runMotor(int motor) {
if (motor==1) {
if (stepper1.runSpeed()) {
if (stepper1.speed()>=0) {
globalLeadSteps++;
enc_steps++; }
else {
globalLeadSteps--;
enc_steps++;
}
}
}
if (motor==2) {
if (stepper2.runSpeed()) {
if (stepper2.speed()>=0) {
globalCrossSteps++;
}
else {
globalCrossSteps--;
}
}
}
}
void respond_to_pc (char * msg) {
if (serialFeedback>0) {
Serial.write(msg);
}
}
void respond_to_pc_debug (char * msg) {
if (serialFeedback==2) {
Serial.write(msg);
}
}
void sendXZJoystick() {
if ((currMillis-prevMillis)>100) {
prevMillis=currMillis;
Serial.write("P ");
Serial.print(globalCrossSteps);
Serial.write(" ");
Serial.print(globalLeadSteps);
Serial.write(" #");
}
}
void sendXZImmediate() {
Serial.write("P ");
Serial.print(globalCrossSteps);
Serial.write(" ");
Serial.print(globalLeadSteps);
Serial.write(" #");
}
void enc_zrising() {
enc_at_z=1;
prev_z_millis=z_millis;
z_millis=currMillis;
respond_to_pc_debug ("Z");
}
void abort () {
stepper1.stop();
stepper2.stop();
sendXZImmediate();
Serial.write("PC #"); // command sequence completed
commandstringlength=0;
currentCommand=0;
numDups=0;
currentDup=0;
allDone=true;
processing=false;
}
