Motion Control project Attempt2

Hi All
Posting this up to get some community feedback and guidance on this project I am working on. This project has been pretty fragmented on the forum as I asked questions and tried to learn more about writing the code and figuring out the best components to use to make this all work. This is also a way of me talking the project through with myself to avoid the ID10T errors.

I am going to post it up in sections to keep the information load manageable, listed are sections I have in mind:

  • 1: Overview & Goals
  • 2: Hardware
  • 3: Code
    o Interrupt Functions
    o General Functions
    o Synched Pass Pulse Functions
    o Communication functions
  • 4: Components and connection to Arduino
    o Step and direction Outputs
    o Servo position inputs
    o Spindle Position inputs
    o Rs485 and Push Buttons

SECTION 1

Overview:

This started out with me wanting an ELS on my lathe, its an old lathe that did not have change gears or a gear box and I want to be able to cut both metric and standard threads with minimal fiddling around. I have a bit of experience designing and implementing control systems with PLCs and figured that it should be within my scope to get this done using an Arduino as I have had one lying on my desk for a few years and never got around to using it for something. I figured it was as simple as applying a function to the incoming spindle position and spitting out a synchronised pulse to the servo drive…

Goals:

  • System with selectable preloaded standard thread pitches for metric and standard threads.
  • Ability to do custom pitches
  • Set up done through touch screen
  • Tool position automatically adjusted to ensure correct cutting pressure
  • Stop tool once length of thread has been completed, return tool to new start position

SECTION 2

Hardware:

  • Lathe: Wyler (no idea on the brand nothing to be found online), Italian plates, probably from the 30s, still runs on a flat belt. 1m between centres, 6tpi lead screw. Spindle Speed from 20 RPM to ~800rpm (three speeds with back gear reduction)
  • Arduino Mega2560
  • Servo Drive: Delta ASDB series of drive, 750w, 10000ppr (SCALED TO 423ppr) closed loop drive connected to the lead screw with a 2:1 reduction.
  • Spindle Encoder: HONTKO INCREMENTAL OPTICAL ENCODER HTR-5B-1024A-P DC5-26VPUSH-PULL 10812A
  • HMI Delta DOP B07S515 industrial touch panel, will store all information for thread pitches as well as calculate tool positions for starting each cut.

Section 3 to follow, still new so can only post every 5 min.

SECTION 3
Code:

  • Interrupts
  • Positioning pulse Functions
  • General Functions
  • COMS

I am going to post up only the relevant sections of code for each subsection to start, the whole program exceeds the character limit, I also hope it will allow you to look at the bits that may interest you without having to search.

My goals for the code was to obviously have it run as fast as possible during a synchronised pass, ideally under 30micros.

SECTION 3.1:Interrupt functions
I should only need 3 interrupt pins to keep track of the position of my spindle and encoder.
All functions are triggered by rising edges.
A phase from Servo Drive on pin 19 (B phase checked during function to determine direction)
A phase from Spindle Encoder on pin 21 (B phase checked during function to determine direction)
Z phase from spindle encoder on pin 20 ( Trigger to start a synched pass)

I would like to use 4 interrupt functions:
void SPINDLE_TIME_POSITION() Monitors both TIME (for RPM) and the position during general functions
void SPINDLE_TIME () Monitors only spindle timing during a synched pass
void SPINDLE_ZERO() Starts synched pass at Spindles Zero position
Void DRIVE_POS() Always active when Arduino is on, monitoring the drive position

Interrupt functions set up and Variables.

 //SP-Spindle input
  const int SPEncoderA = 21;//Spindle encoder A pulse (1024ppr)
  const int SPEncoderB = 51;//Spindle encoder B Pulse (1024ppr)
  const int SPEncoderZ = 20;//Spindle encoder Z pulse (1ppr) 
//DR-Drive Encoder Input
  const int DREncoderA = 19;//Servo Drive feedback encoder (423ppr) Feed back resolution of 0.00500mm
  const int DREncoderB = 50;//Servo Drive feedback encoder (423ppr) 
//Spindle positioning and timing
  long SpindleRPM;
  int SPEncoderAstate;
  int SPEncoderBstate;
  int SPEncoderZstate;
  int SpindlePulseMicros; // time from A to A rising
  unsigned long SpindleMicros; //time for spindle to complete one full rotation. spindle pulse microseconds mul 1024
  long SPETimeA; //micros at A state change
  long SPETimeB; //micros at B state change
  unsigned int SpindlePositionABS;
  SpindlePostionABS = SpindlePositionABS & 0x3FF;//mask first ten bits so max value can only be between 0 and 1023
// Lead screw Drive positioning
  int DREncoderAstate;
  int DREncoderBstate;
  long DriveCurrentPos;
void setup() {

// Servo control set up  
  pinMode (SPEncoderA, INPUT);
  pinMode (SPEncoderB, INPUT);
  pinMode (SPEncoderZ, INPUT);
  pinMode (DREncoderA, INPUT);
  pinMode (DREncoderB, INPUT);
  attachInterrupt(digitalPinToInterrupt(SPEncoderA), SPINDLE_TIME, RISING);
  attachInterrupt(digitalPinToInterrupt(DREncoderA), DRIVE_POS, RISING);

A phase Servo drive interrupt. This interrupt should run at 47micros if the servo drive is running at 3000rpm (unlikely scenario) (INT.4)

 void DRIVE_POS(){
  DREncoderBstate = digitalRead(DREncoderB);
    if (DREncoderBstate != HIGH){
        DriveCurrentPos++;
      }else{
        DriveCurrentPos--;
      }  
}

Spindle A Phase interrupt. This interrupt should run every 58micros at a spindle speed of 1000rpm (again unlikely, max spindle speed on the lathe is 800rpm) (INT.2)

Code to switch between two interrupt functions depending on what functions are going to be executed.
(This code switches from SPINDLE_TIME_POSTION to SPINDLE_TIME)

 //GO to Start pos LEFT  
  if (holding15==1){
    if(DriveRightState == HIGH){
      RUNTOSTART = 1;
    }
//Start LEFT pass    
    if(holding13==1){
    if(DriveLeftState == HIGH){
    if(INPOSITIONL == 1){
     RUN_COMS = 0;
    //Shut down Modbus protocal here
     digitalWrite(DirPulse, HIGH);// Set Direction of drive for pass
     READY_FOR_PASSL = 1;
     detachInterrupt(digitalPinToInterrupt(SPEncoderA));//Detach SPINDLE_TIME_POSITION interrupt
     attachInterrupt(digitalPinToInterrupt(SPEncoderA), SPINDLE_TIME, RISING);//attach SPINDLE_TIME interrupt
     attachInterrupt(digitalPinToInterrupt(SPEncoderZ), SPINDLE_ZERO, RISING);
      }else{holding22=1;} // NOT IN LEFT POSTION ALARM
     }
    }
   }

This Code switches back to SPINDLE_TIME_POSTIONING

 void RESET(){
Millis2 = millis();
if( run1 == 0){
 RUN_SERVOL = 0;
 RUN_SERVOR = 0;
 READY_FOR_PASSR = 0;
 READY_FOR_PASSL = 0;
 INPOSITIONR = 0;
 INPOSITIONL = 0;
 holding13 = 0;
 holding21 = 0;
 holding29 = 0;
 detachInterrupt(digitalPinToInterrupt(SPEncoderA));//detach SPINDLE_TIME interrupt
 attachInterrupt(digitalPinToInterrupt(SPEncoderA), SPINDLE_TIME_POSITION, RISING);//attach SPINDLE_TIME_POSITION interrupt
 COMS_StartUp = 0; //Restart Serial
 RUN_COMS = 1;
 holding30++;
 Millis3 = Millis2;
 run1 = 1;
 }
 if(Millis2 - Millis3 > 1000){
 PassComplete = 0;
 run1 = 0 
 }  
}

Interupt code for monitoring spindle timing (only called during synched pass)

 void SPINDLE_TIME(){ 
      SPETimeA = SPETimeB;
      SPETimeB = micros();
      }
  SpindlePulseMicros = SPETimeB - SPETimeA;
  SpindleMicros = SpindlePulseMicros * 1024;
}

Interrupt code for monitoring spindle timing and positioning (called at any time other than synched pass)

 void SPINDLE_TIME_POSITION(){ 
      SPETimeA = SPETimeB;
      SPETimeB = micros();
      SPEncoderBstate = digitalRead(SPEncoderB);
      if (SPEncoderBstate == HIGH){
        SpindlePositionABS++;
        SpindlePositionABS = SpindlePositionABS & 0x3FF;//bitmask to allow max value 1023
      }else{
       SpindlePositionABS--;
       SpindlePositionABS = SpindlePositionABS & 0x3FF;
      }
  SpindlePulseMicros = SPETimeB - SPETimeA;
  SpindleMicros = SpindlePulseMicros * 1024;
}

Spindle Encoder Z pulse interrupt. Interrupt function from spindle Encoder Z pulse is only called when Set up is complete, tool is in position and command is given for the synched pass to start. It will run once and then detach itself. (INT.3)
Code calling the Interupt:

 // Start RIGHT pass   
    if(holding13==2){
    if(DriveRightState == HIGH){
    if(INPOSITIONR == 1){
     RUN_COMS = 0;
     //Shut down Modbus protocol here
     digitalWrite(DirPulse, LOW);//Set direction of drive for pass
     READY_FOR_PASSR = 1;
     detachInterrupt(digitalPinToInterrupt(SPEncoderA));//Detach SPINDLE_TIME_POSITION interrupt
     attachInterrupt(digitalPinToInterrupt(SPEncoderA), SPINDLE_TIME, RISING);//attach SPINDLE_TIME interrupt
     attachInterrupt(digitalPinToInterrupt(SPEncoderZ), SPINDLE_ZERO, RISING);
      }else{holding22=2;} // NOT IN RIGHT POSTION ALARM
     }
    }
   }

Zpulse interrupt Function

 void SPINDLE_ZERO(){
 if(READY_FOR_PASSR == HIGH){
  digitalWrite(RUN_SERVOR, HIGH);  
 }
 if(READY_FOR_PASSL == HIGH){
  digitalWrite(RUN_SERVOL, HIGH);  
 }
 detachInterrupt(digitalPinToInterrupt(SPEncoderZ));
}

SECTION 3.2: Synched Pass Pulse Functions

Besides The interrupt functions this will be the only function to run during a synched pass.
Variables and setup code

  const int StepPulse = 52; //200K pps = 3000rpm
  const int DirPulse = 53; //Set drive direction, High-FWD, Low-RVS
  int PulseState;
  unsigned long SpindleMicros; //time for spindle to complete one full rotation. spindle pulse microseconds mul 1024
  long DriveCurrentPos;
  long LeadScrewLeftMAX;
  long LeadScrewRightMAX;
  int SquareWaveHILO; //Time for square wave of step
  int SquareWave; //Half square wave time
  long PreviousMicros;
  long CurrentMicros;
  long sppsr; //step pulses per spindle revolution
void setup() {

// Servo control set up  
  pinMode (StepPulse, OUTPUT);
  pinMode (DirPulse, OUTPUT);

Function to drive servo either left or right synchronised to spindle, I am also including loop here as it contains the function to get the correct timing for the pulse square wave

 void loop() {
  SquareWaveHILO = SpindleMicros/sppsr;
  HalfSquareWave = SquareWaveHILO/2;

   //RUN ServoLEFT
 if (RUN_SERVOL == 1){
  SERVOPULSEFWD();
 }
  //RUN ServoRIGHT
 if (RUN_SERVOR == 1){
  SERVOPULSERVS();
 }

 if((RUN_SERVOL == 0) && (RUN_SERVOR == 0)){
  PROG_SEL();
 }

 if(PassComplete==1){
  RESET();
 }

} 
void SERVOPULSEFWD(){
  CurrentMicros = micros();
 if(LeadScrewLeftMAX > LeadScrewCurrentPos){
 if(PassComplete == 0){
  if(CurrentMicros - PreviousMicros >= SquareWave) {
    PreviousMicros = micros();
    if (PulseState == LOW){
      PulseState = HIGH;
    } else{
      PulseState = LOW;
    }
  }
 }
}
digitalWrite(StepPulse, PulseState);
if(LeadScrewLeftMAX <= LeadScrewCurrentPos){
  PassComplete = 1;
 }
}
void SERVOPULSERVS(){
 CurrentMicros = micros();
 if(LeadScrewRightMAX < LeadScrewCurrentPos){
 if(PassComplete == 1){
  if(CurrentMicros - PreviousMicros >= SquareWave) {
    PreviousMicros = micros();
    if (PulseState == LOW){
      PulseState = HIGH;
    } else{
      PulseState = LOW;
    }
  }
 }
}

There is not much to say here, How ever I would be open to any improvements to get this to run faster.

SECTION 3.3: General Functions
These functions allow for selecting which functions are run, getting the tool to position before starting a pass, checking backlash in the system. Jogging the drive left and right through the push button inputs. It also gives feed back on RPM and spindle positioning in degrees for general lathe operations, number of passes completed for current operation. Resets after a synched pass is complete.

Variables and setup.

//Control-Box input
  const int DriveLeft = 48; //Jog left,run pass in feed mode, start threading in threading mode/or if reverse threadng return to start.
  const int DriveRight = 49; //Jog right, run pass in feed mode, return to start in threading mode/or if reverse threading start threading.
  int DriveLeftState = LOW;
  int DriveRightState = LOW; 
//Spindle positioning and timing
  unsigned long SpindleRPM;
  long SpindlePositionABS100;
  int BackLash;
  int BackLash2;
  long LeadScrewCurrentPos;
  long LeadScrewReturnToLead;
  int PassNumber; //passes done
//prog ints
//Registers removed here due to character lims

void setup() {

// Servo control set up  
  pinMode (DriveLeft, INPUT);
  pinMode (DriveRight, INPUT);
  RUN_COMS = 1;
  PassComplete = 0;
  RUN_SERVOR = 0;
  RUN_SERVOL = 0;
  holding31 = 0;

Starting from the top as the code is written, Reset function, this is used after a pass is completed to bring general functions back online. (void loop also included here as it has the function to bring general function online again)

void loop() {
  SquareWaveHILO = SpindleMicros/sppsr;//SPPSR-Servo Pulse Per spindle Revolution. Source HMI
  HalfSquareWave = SquareWaveHILO/2;
   //RUN ServoLEFT
 if (RUN_SERVOL == 1){
  SERVOPULSEFWD();
 }
  //RUN ServoRIGHT
 if (RUN_SERVOR == 1){
  SERVOPULSERVS();
 }
 if((RUN_SERVOL == 0) && (RUN_SERVOR == 0)){
  PROG_SEL();
 }
 if(PassComplete==1){
  RESET();
 }
}
void RESET(){
Millis2 = millis();
if( run1 == 0){
 RUN_SERVOL = 0;
 RUN_SERVOR = 0;
 READY_FOR_PASSR = 0;
 READY_FOR_PASSL = 0;
 INPOSITIONR = 0;
 INPOSITIONL = 0;
 holding13 = 0;
 holding21 = 0;
 holding29 = 0;
 detachInterrupt(digitalPinToInterrupt(SPEncoderA));//detach SPINDLE_TIME interrupt
 attachInterrupt(digitalPinToInterrupt(SPEncoderA), SPINDLE_TIME_POSITION, RISING);//attach SPINDLE_TIME_POSITION interrupt
 COMS_StartUp = 0; //Restart Serial
 RUN_COMS = 1;
 holding30++;
 Millis3 = Millis2;
 run1 = 1;
 }
 if(Millis2 - Millis3 > 1000){
 PassComplete = 0;
 run1 = 0 
 }  
}

General functions are included in Prog_Sel, I may at a later stage optimise this section further to further improve loop speed. I think it is relatively well commented so I wont say much about it.

void PROG_SEL(){
  SpindleRPM = 60000000 / SpindleMicros;
  SpindlePositionABS100 = SpindlePositionABS * 100;//Improve degree accuracy
  BackLash2 = BackLash * 2;
  SpindleDeg = SpindlePositionABS100 / 284;//1 degree = 2.84 pulses from the spindle
  //JOG/RUN button inputs
  DriveLeftState = digitalRead(DriveLeft);
  DriveRightState = digitalRead(DriveRight);

  //SET HOME FROM HMI
  if(holding9==1){DriveCurrentPos = 0;}
  //SET LEFT MAX FROM HMI using drives current position
  if(holding9==2){LeadScrewLeftMAX = DriveCurrentPos;}
  //SET LEFT MAX FROM HMI using drives current position
  if(holding9==3){LeadScrewRightMAX = DriveCurrentPos;}
  //RUN BACKLASH TEST
  if((holding9==4)&&(BACKLASHTEST == 0)){BACKLASHTESTING = 1;}
  //Stop backlash testing from HMI
  if(holding9==5){BACKLASHTESTING = 0;BACKLASHRETURN = 0;BACKLASHTEST = 0;}
  //Reset after backlash testing
  if(BACKLASHTEST == 1){BACKLASHTEST = 0;}
  //ZERO SPINDLE POSITION FROM HMI
  if(holding9==6){SpindlePositionABS = 0;}
  //Further QOL to be implemented here



  //JOG L_R
  if((holding12==0)&&(DriveLeftState==HIGH)){holding31=1;}//HMI TO RUN DRIVE LEFT RW405=4999
  if((DriveRightState==HIGH)&&(holding12==0)){holding31=2;}//HMI TO RUN DRIVE RIGHT RW405=4998
  if((DriveRightState==LOW)&&(DriveLeftState==LOW)){holding31=0;}//HMI TO STOP DRIVE RW405=5000
//Go to Start pos RIGHT
  if (holding15==2){
    if(DriveLeftState == HIGH){
      RUNTOSTART = 1;
    }
// Start RIGHT pass   
    if(holding13==2){
    if(DriveRightState == HIGH){
    if(INPOSITIONR == 1){
     RUN_COMS = 0;
     //Shut down Modbus protocol here
     digitalWrite(DirPulse, LOW);//Set direction of drive for pass
     READY_FOR_PASSR = 1;
     detachInterrupt(digitalPinToInterrupt(SPEncoderA));//Detach SPINDLE_TIME_POSITION interrupt
     attachInterrupt(digitalPinToInterrupt(SPEncoderA), SPINDLE_TIME, RISING);//attach SPINDLE_TIME interrupt
     attachInterrupt(digitalPinToInterrupt(SPEncoderZ), SPINDLE_ZERO, RISING);
      }else{holding22=2;} // NOT IN RIGHT POSTION ALARM
     }
    }
   }
//GO to Start pos LEFT  
  if (holding15==1){
    if(DriveRightState == HIGH){
      RUNTOSTART = 1;
    }
//Start LEFT pass    
    if(holding13==1){
    if(DriveLeftState == HIGH){
    if(INPOSITIONL == 1){
     RUN_COMS = 0;
    //Shut down Modbus protocal here
     digitalWrite(DirPulse, HIGH);// Set Dricetion of drive for pass
     READY_FOR_PASSL = 1;
     detachInterrupt(digitalPinToInterrupt(SPEncoderA));//Detach SPINDLE_TIME_POSITION interrupt
     attachInterrupt(digitalPinToInterrupt(SPEncoderA), SPINDLE_TIME, RISING);//attach SPINDLE_TIME interrupt
     attachInterrupt(digitalPinToInterrupt(SPEncoderZ), SPINDLE_ZERO, RISING);
      }else{holding22=1;} // NOT IN LEFT POSTION ALARM
     }
    }
   }
  //RUN BACKLASH TESTING
  if(BACKLASHTESTING == 1){BACKLASHTESTER();}
  //RUN RS485
  if (RUN_COMS==1){COMS();}
  //SET START POSITION
  if(RUNTOSTART== 1){GO_TO_START();}
}

This code allows for the tool to be moved to the starting position first using the Jog function to get any macro movements completed followed by position-controlled function to set the tool ready for the start function. Start positions are pre calculated by the HMI.

void GO_TO_START(){
  
 if(holding14==0){//left pass
   if(LeadScrewCurrentPos>=0){
    holding31 = 1; 
   }else{
   holding31 = 0;
   SLOW_STEPL();
   }
  }
 if(holding14==1){//right pass
   if(LeadScrewCurrentPos<=0){
    holding31 = 2;
   }else{
   holding31 = 0;
   SLOW_STEPR();
   }
  }
}

void SLOW_STEPL(){//removes backlash and prepares for start possition for a right to left run
  if(DriveCurrentPos>=0+LeadScrewReturnToLead){
  if(Positioning2== HIGH){
    INPOSITIONL = 1;
    holding29= 1;
    RUNTOSTART = 0;
    Positioning1 = 0;
    Positioning2= 0;    
    } 
  }
  if(DriveCurrentPos>= LeadScrewReturnToLead-BackLash2){
  Positioning1 = 1;}
  if(DriveCurrentPos<= LeadScrewReturnToLead-BackLash2){
  Positioning2 = 1;
  Positioning1 = 0;}

  if(Positioning1==1){
    digitalWrite(DirPulse, LOW);}
  if(Positioning2==1){
    digitalWrite(DirPulse, HIGH);}
    
 if(INPOSITIONL==0){
  CurrentMicros = micros();     
  if(CurrentMicros - PreviousMicros >= 1600) {
    PreviousMicros = micros();
    if (PulseState == LOW){
      PulseState = HIGH;
    } else{
      PulseState = LOW;
    }
   }
  }
digitalWrite(StepPulse, PulseState);
}
void SLOW_STEPR(){//removes backlash and prepares for start possition for a left to right run. CODE REMOVED DUE TO CHRACTER LIMITATION MIRROR OF SLOW_STEPL

Backlash function, this function runs the drive back and fourth so backlash can be calculated using a DTI

void BACKLASHTESTER(){//BACKLASH TEST to be run from drive position 0
  CurrentMicros = micros();
  if((DriveCurrentPos< 4000)&&(BACKLASHTEST == 0)){//run forward 20mm
   digitalWrite(DirPulse, HIGH);
  }
  if((DriveCurrentPos>= 4000)&&(BACKLASHTEST == 0)){//reverse 20mm
   BACKLASHRETURN = 1; 
  }
  if(BACKLASHRETURN == 1){
    digitalWrite(DirPulse, LOW);
  }
  if((BACKLASHRETURN == 1) && (DriveCurrentPos <= 0)){
   digitalWrite(DirPulse, HIGH);
   BACKLASHRETURN = 2;
  }
  if((BACKLASHRETURN == 2) && (DriveCurrentPos >= 4000)){
   BACKLASHTEST = 1;
   BACKLASHRETURN = 0;
   BACKLASHTESTING = 0;
  }
   if(BACKLASHTESTING==1){     
    if(CurrentMicros - PreviousMicros >= holding8) {
     PreviousMicros = CurrentMicros;
      if (PulseState == LOW){
       PulseState = HIGH;
       }else{
        PulseState = LOW;
    }
   }
  }
digitalWrite(StepPulse, PulseState);
}

Again any thoughts and improvements are welcome.

SECTION 3.4: COMS

This all seems to be working so far should be mostly self explanatory. Made use of unsigned holding registers to make sure every thing was unsigned before being moved into modbus holding registers, seemed to have issue if that was not done. Refer to general code for Modbus functions being de/activated

Variables and set up

 #include "ModbusXT.h"

#define TIMEOUT 500   //Timeout for a failed packet. Timeout need to larger than polling
#define POLLING 2     //Wait time to next request

#define BAUD        57600  
#define RETRIES     10    //How many time to re-request packet frome slave if request is failed
#define BYTE_FORMAT SERIAL_8E1
#define TxEnablePin 2   //Arduino pin to enable transmission

#define print(x)  Serial.print(x)
#define println(x) Serial.println(x)


//Name for register in regs[]
enum {
 //READ PACKET 1
 PPR1,//W40101 DWLOW R1 0
 PPR2,//W40102 DWHIGH R2 1
 LEFTMAX1,//W40103 DWLOW R3 2
 LEFTMAX2,//W40104 DWHIGH R4 3
 RIGHTMAX1,//W40105 DWLOW R5 4
 RIGHTMAX2,//W40106 DWHIGH R6 5
 TOOLLEAD,//W40107 W R7 6
 PULSEWIDTH,//W40108 W R8 7
 HOMESETZERO,//W40109 W R9 8
 BACKLASH,//W40110 W R10 9
 SETSPINDLEZERO,//W40111 W R11 10
 SETUPCOMPLETE,//W40112 W R12 11
 READYFORPASS,//W40113 W R13 12
 PASSDIRECTION,//W40114 W R14 13 0=left pass 1=right pass
 GOTOSTART,//W40115 R15 14 0=not ready 1=ready to pass left 2=ready to pass right
 //WRITE PACKET 2
 LEADSCREWPOS1,//W40121 DWLOW R16 15
 LEADSCREWPOS2,//W40122 DWHIGH R17 16
 SPINDLE_DEG1,//W40123 W R18 17
 SPINDLE_DEG2,//W40124 W R19 18 
 SPINDLE_RPM1,//W40125 W R20 19
 SPINDLE_RPM2,//W40126 W R21 20
 PASSCOMPLETE,//W40127 W R22 21
 TOOLPOSITION1,//W40128 DWLOW R23 22
 TOOLPOSITION2,//W40129 DWHIGH R24 23
 LEFTMAXWRITE1,//W40130 DWLOW R25 24
 LEFTMAXWRITE2,//W40131 DWHIGH R26 25
 RIGHTMAXWRITE1,//W40132 DWLOW R27 26
 RIGHTMAXWRITE2,//W40133 DWHIGH R28 27
 INPOSITION, // W40134 W R29 28 0 = not in position 1 in position left 2 in pos right 3 backlash complete
 PASSNUMBER,//W40135 w R30 29
 //WRITE PACKET 3
 total_packets,//W40141 W R31 30
 total_failed,//W40142 W R32 31
 total_requests,//W40143 W R33 32
 transfer_rate,//W40144 W R34 33
 transfer_delay,//W40145 W R35 34
 DRIVEJOG,//W40146 W R36 35
 ALARM,//W40147 W R37 36
  TOTAL_REGS //=35 (double words covers two addresses)
};

unsigned int holding1, holding2, holding3, holding4, holding5, holding6, holding7, holding8, holding9, holding10, holding11, holding12, holding13, holding14, holding15;
unsigned int holding1l, holding2l, holding3l, holding4l, holding5l, holding6l, holding7l, holding8l, holding9l, holding10l, holding11l, holding12l, holding13l, holding14l, holding15l;
unsigned int holding16, holding17, holding18, holding19, holding20, holding21, holding22, holding23, holding24, holding25, holding26, holding27, holding28, holding29, holding30,holding31;

// This is the easiest way to create new packets
// Add as many as you want. TOTAL_NO_OF_PACKETS
// is automatically updated.
enum {
  PACKET1,
  PACKET2,
  PACKET3,
  NO_OF_PACKET  //=3
};

// Masters register array
uint16_t regs[TOTAL_REGS];

//Modbus packet
Packet packets[NO_OF_PACKET];

// Access individual packet parameter. Uncomment it if you know what you're doing
// packetPointer packet1 = &packets[PACKET1];
// packetPointer packet2 = &packets[PACKET2];

long sm,em,dm;
uint16_t temp;
const uint8_t hmiID = 1;  //ID of HMI. The ID need to match, unless program will not work

//Modbus Master class define
Modbus master;

Modbus Enable in its own function rather than setup as I want to able to activate and deactivate it separately. When first powered on screen needs to boot before coms are started.

 void COMS_BEGIN(){
  //Config packets and register
 master.configure(packets, NO_OF_PACKET, regs);

  //Config individual packet: (packet, ID, Function, Address, Number of register or data, start register in master register array)
  master.construct(&packets[PACKET1], hmiID, READ_HOLDING_REGISTERS, 100, 15, 0);
  master.construct(&packets[PACKET2], hmiID, PRESET_MULTIPLE_REGISTERS, 120, 15, 15);
  master.construct(&packets[PACKET3], hmiID, PRESET_MULTIPLE_REGISTERS, 140, 7, 30);
  master.begin(&Serial3, BAUD, BYTE_FORMAT, TIMEOUT, POLLING, RETRIES, TxEnablePin);
  COMS_StartUp = 2;
  
}

Coms functions. Here all variables are transferred to and from the mobus holding registers in the array in the setup. In some cases 32bit integers needed to be split or recombined into Hi and Lo 16 bit (or 32 bit registers if recombined) holding registers for transmission.

 void COMS(){
  if(COMS_StartUp < 2){
  ComsStartmillis = millis();
   if(COMS_StartUp == 0){
    ComsStartmillis1 = millis();
    COMS_StartUp = 1;
    }
   if((COMS_StartUp == 1) && (ComsStartmillis - ComsStartmillis1 > 30000)){//Allow HMI time to boot after power up before enabling coms
    COMS_BEGIN();
    }
  }
     
   master.update();  //polling
   sm = millis();
   //READS
   holding1 = regs[PPR1];
   holding2 = regs[PPR2];
   holding3 = regs[LEFTMAX1];
   holding4 = regs[LEFTMAX2];
   holding5 = regs[RIGHTMAX1];
   holding6 = regs[RIGHTMAX2];
   holding7 = regs[TOOLLEAD];
   holding8 = regs[PULSEWIDTH];
   holding9 = regs[HOMESETZERO];
   holding10 = regs[BACKLASH];
   holding11 = regs[SETSPINDLEZERO];
   holding12 = regs[SETUPCOMPLETE];
   holding13 = regs[READYFORPASS];
   holding14 = regs[PASSDIRECTION];
   holding15 = regs[GOTOSTART];
   sppsr = holding2;
   sppsr = (sppsr << 16) | holding1;
   BackLash = holding10;
   LeadScrewReturnToLead = holding7;
   LeadScrewLeftMAX = holding4;
   LeadScrewLeftMAX = (LeadScrewLeftMAX << 16) | holding3;
   LeadScrewRightMAX = holding6;
   LeadScrewRightMAX = (LeadScrewRightMAX << 16) | holding5;
   //WRITES
   holding16 = (int)DriveCurrentPos;
   holding17 = (int)(DriveCurrentPos >> 16);
   holding18 = (int)SpindleDeg;
   holding19 = (int)(SpindleDeg >> 16);
   holding20 = (int)SpindleRPM;
   holding21 = (int)(SpindleRPM >> 16);
   holding22 = PassComplete;
   holding23 = (int)DriveCurrentPos;
   holding24 = (int)(DriveCurrentPos >> 16);
   holding25 = (int)LeadScrewLeftMAX;
   holding26 = (int)(LeadScrewLeftMAX >> 16);
   holding27 = (int)LeadScrewRightMAX;
   holding28 = (int)(LeadScrewRightMAX >> 16);
   regs[LEADSCREWPOS1] = holding16;
   regs[LEADSCREWPOS2] = holding17;
   regs[SPINDLE_DEG1] = holding18;
   regs[SPINDLE_DEG2] = holding19;
   regs[SPINDLE_RPM1] = holding20;
   regs[SPINDLE_RPM2] = holding21;
   regs[PASSCOMPLETE] = holding22;
   regs[TOOLPOSITION1] = holding23;
   regs[TOOLPOSITION2] = holding24;
   regs[LEFTMAXWRITE1] = holding25;
   regs[LEFTMAXWRITE2] = holding26;
   regs[RIGHTMAXWRITE1] = holding27;
   regs[RIGHTMAXWRITE2] = holding28;
   regs[INPOSITION] = holding29;
   regs[PASSNUMBER] = holding30;
   regs[total_packets] = NO_OF_PACKET;            
   regs[total_requests] = master.total_requests();
   regs[total_failed] = master.total_failed(); 
   regs[DRIVEJOG] = holding31;    
  if ( (sm-dm) > 1000) //update 1s
   {
     dm = sm;
     regs[transfer_rate] = regs[total_requests] - temp;
     temp = regs[total_requests];
     regs[transfer_delay] = (unsigned int) ((NO_OF_PACKET*100000UL)/regs[transfer_rate]);
   }
}

As Always any improvements are welcome. I am still trying to work out how to completely deactivate the Modbus protocol to free up the processor during a synched pass.

SECTION 4

Will still add full component list but all is labelled in circuit diagrams.

SECTION 4.1: Step Direction OUTPUTS

Currently I have tested the system using IRL540N Mosfets how ever the gate to source voltage is 16v. I need it to be 20 v as the Drive needs a 24v signal and Arduino pins are obviously only outputting 5v, I will change to IRF540N Mosfets as shown in the diagram. I will also explain how I came up with my resistor values in all cases. (Hopefully I got them right).

SECTION4.2: Servo Position INPUTS
Pretty much just hope I got this right. Still untested. Anything that can be improved will be greatly appreciated. Or even pointing out where I have made stupid mistakes.

SECTION4.3: Spindle Position INPUTS

I am not sure if I am handling this right, Encoder is a push pull type. I am assuming that the output on A,B and Z are +5v. I have been testing with some 4N35 opto isolators and get no input. Did the same test with the Servo Position inputs using the 4N35s and I was getting a pulse. (Connecting the encoder outputs directly to the Arduino pins with pin set as INPUT_PULLUP was working fine but picked interference from the drive when it was on)

SECTION4.4: Push Buttons and RS485

This Is pretty straight forward…. I think

SECTION 5:
All my follow up comments/questions to go here.
Full sketch and circuit diagram pdf attached here

And github link…

Have at it, Tomorrow I hope to have all the correct components (6N137s being the most critical) in hand and do some more testing.

ARDUINO SERVO CONTROL V3.pdf (340 KB)

servo_control_v3.ino (19.1 KB)

Nycticoraci:
Have at it,

I hope you find a Forum member who is more generous with their time, but I’m sorry, what you have posted is far too long for me.

…R

I can quite appreciate that its very long. Not expecting anybody to analyse every aspect of it. Kind of why I chunked it up. There may be something that catches somebodies interest that they can then comment on. My one beef with the forum is that you see so many questions answered but never the see the answer being put into practice(this is a generalisation). I am pretty sure that there are others that will struggle with the same issues I am struggling with and I hope that I can actually provide some useful information on how I solved my issues...

Nycticoraci:
I am pretty sure that there are others that will struggle with the same issues I am struggling with and I hope that I can actually provide some useful information on how I solved my issues...

Do you mean that your series of Posts is intended to be a Tutorial rather than a request for help? If so I can see how a person wishing to learn from your experience will take the trouble to read it all.

I had been assuming that you are looking for assistance with something.

If it is a Tutorial then it might be an idea to click Report to Moderator and ask for it to be moved to the Exhibition / Gallery section.

...R

No, certainly not a tutorial. Not yet. If it really works well and enough people consider it good enough for that then yes. This is more of a sounding board. I don't have too many people I can bounce my ideas off of and hoped that this would be a good way of getting the whole thing out there in a none fragmented way. I figured context on any questions I have for the project would help.

Section 5 will have my questions (the new ones that will come within the next 2 or 3 hours as I am busy putting the new components as I type this).

So my first issue seems the be the optocouplers. Not the couplers them selves but the way I am using them. I am getting no signals at all.

Refer to section 3.1 for the interupts that they are linked to and section 4.2 and 4.3 for the circuit diagrams.

Would greatly appreciated any insights.

I'm a bit confused here.

Are you asking for someone to help you build a motion control system that would normally have 5-6 figures of professional development cost behind it or are you just asking about optocouplers?

My post got moved here. I pre-emptively posted up all the information about my build so far to avoid the need to do so in any case when I ask a question. So yes. My current question is about opto couplers. Still learning about electronics.

Right now I think its a lack of bypass 0.1nF caps between 8 and 5. Out of frustration I put some LEDs in line on the incoming signal from both encoders and they are working perfectly.

In the case of the push pull encoder I am going to put in diodes so that its only pushes. I am also going to put in diodes across pins 2 and 3 on each of the 6N137s. (The information I have found so far is conflicting on this.) One project somebody did using an arduino had the diode between pins 2 and 3 but no bypass Cap between pin 5 and 8.
This would pull the arduino pin low when the opto receives signal If I am not mistaken?

As far as I can see from the 6N137 datasheet, Ve (pin 7) is a Disable input and its default state is HIGH, so you may as well disconnect it: tying it to +5 isn't accomplishing anything. Remove the resistor at pin 6 since it's a logic-level output device.

I assume that the diode on the input is for reverse polarity protection. With 5-12V at DCCIN the output should go LOW and be HIGH otherwise.

Thanks for the input. Turns out majority of my frustrations have been as a result of a broken wire inside the mulit core coming from the drive. It was on the /OA of the positioning signals and so I was only getting half of the needed signal. Optos seem to be working great now. Left pin 7 disconnected and put in the a 0.1nF cap from 8 to 5. Seems to be doing the trick.

I am how ever stuggling with interference on my spindle encoder. It only occurs when the A phase is high on the spindle encoder and the servo drive is enabled. I currently have a 330ohm resitor on pin 2 on each opto couplers related to the spindle encoder. (Section 4.3) how could I adjust this to remove more of the noise?

A courtesy update on this, The arduino mega is not suitable for this project. I have moved to a controller with Hardware counters and 100kHz outputs.