Lathe tool changer turret - DC motor based

/*************************************************************************************************************************************

LinuxCNC HAL component to implement ATC toolchanging on Hercus PC200 Lathe
Based on "EMC2 HAL component to implement ATC toolchanging in Denford ORAC CNC lathes" (c) ArcEye 01122011 schooner30@tiscali.co.uk

Modified by pippin88 - 30/09/2017

All values are metric

Version 0a -  The PC200 toolchanger has an 8 position unidirectional turret. There is an index sensor (PC200changer.index),
              and a position sensor (PC200changer.pocket) that pulses once per tool position.
  			      When the motor moves forwards, the turret rotates. When the motor is reveresed, locking pins deploy, interfacing with a locking disc.
              When the locking pins are fully deployed, they close a circuit (PC200changer.lock)
              The sensors do not line up exactly with the actual tool position, but are a set offset.
              To allow the ATC time to get past the sensor and line the locking pins up with the holes, a timedelay component
              is used with the on-delay set to X seconds.
              This is activated automatically by the PC200changer component with the hal connections shown below.

              The turret uses a simple DC motor. This is driven by an H-bridge (L298N) which takes two input signals to determine direction.
              motor A and motor B. A HIGH, B HIGH = motor off; A HIGH, B LOW = motor forward; A LOW, B HIGH = motor reverse; A LOW, B LOW = motor off;
              I use A HIGH, B HIGH for off, as this is less susceptible to noise than a resting LOW state.

              To cater for the delay in mechanical relays switching the motor on and off and forward and reverse, and to allow the motor
              itself to cease movement, there is a simple counter delay in the component.
              The period of this delay is set by default to 500, which equates to 1-2 secs on my test system
              It can be modified in the .hal file using
              'setp PC200changer.times nnn' where nnn is the new value

               Compile the components with 'sudo comp --install PC200changer.comp'
               and                         'sudo comp --install timedelay2.comp' respectively

               Below .hal file extracts give a specimen .hal connection guide. Place them as appropriate in .hal file

###############################################################
# loading the toolchange and timedelay2 components in .hal file
###############################################################

loadrt PC200changer
addf PC200changer servo-thread

#loadrt timedelay count=1
#addf timedelay.0 servo-thread
### will wait 5 secs approx
#setp timedelay.0.on-delay 5

net tool-change iocontrol.0.tool-change => PC200changer.toolchange
net tool-changed iocontrol.0.tool-changed <= PC200changer.toolchanged
net tool-number iocontrol.0.tool-prep-number => PC200changer.toolnumber
net tool-oldnumber iocontrol.0.tool-number => PC200changer.currenttoolnumber
net tool-prepare-loopback iocontrol.0.tool-prepare => iocontrol.0.tool-prepared

net turret-lock PC200changer.lock <= hm2_7i43.0.7i76.0.0.input-25 #Turret Lock sensor (White)
net turret-index PC200changer.index <= hm2_7i43.0.7i76.0.0.input-26 #Turret Index (Green)
net turret-position PC200changer.pocket <= hm2_7i43.0.7i76.0.0.input-27 #Turret Pulse (Blue)

## GPIO33 = Turret motorA
setp hm2_7i43.0.gpio.033.is_output true
setp hm2_7i43.0.gpio.033.out true ### pull high at startup!!! Can change if needed
## GPIO34 = Turret motorB
setp hm2_7i43.0.gpio.034.is_output true
setp hm2_7i43.0.gpio.034.out true ### pull high at startup!!! Can change if needed

net turret-motorA PC200changer.motorA => hm2_7i43.0.gpio.033.out
net turret-motorB PC200changer.motorB => hm2_7i43.0.gpio.034.out

net xhomed axis.0.homed => PC200changer.ishomedX
net zhomed axis.2.homed => PC200changer.ishomedZ

setp PC200changer.times nnn ### where nnn is the new value

#net delaystart timedelay.0.in <= PC200changer.delaystart
#net delaydone timedelay.0.out => PC200changer.delaydone

################################################################################

***************************************************************************************************************************************/

component PC200changer                  "This component controls the PC200 Lathe Auto Tool Changer. M6 calls this";

pin in bit toolchange                   "Receives signal from M6 that tool change required";
pin in s32 reqtoolnumber                "Requested Tool Number - Tx data from M6. Only 1-8 allowed";
pin in s32 currenttoolnumber            "Receives old tool number from hal / axis";
pin out bit toolchanged = false         "Sends signal when tool change finished";

pin out bit delaystart = false          "Starts timerdelay";
pin in bit delaydone =false             "Signals timer finished";

pin in bit index = 1                    "State of turret index sensor";
pin in bit pocket = 1                   "State of turret pocket sensor";
pin in bit lock = 1                     "State of turret lock sensor";

pin out bit motorA = 1                  "Motor signal A";
pin out bit motorB = 1                  "Motor signal B";

pin in bit ishomedX = false             "Status of X axis homing";
pin in bit ishomedZ = false             "Status of Z axis homing";

pin out s32 position = 0                "Turret Position - Initialised as a pin for debugging so we can check where it thinks it is";

pin out s32 turrethomed = 0             "Turret Homed signal - Initialised as a pin for debugging so we can check where it thinks it is";

param rw float times = 5000             "Length of delay for forward move before locking";

// Internal and debugging stuff
pin out s32 progresslevel = 0;         // tracks the progress of the toolchange, just here so it can be read easily
pin out s32 tnumber = 0;               // Internal toolnumber to allow overrun of quadrant by 1 then reverse back onto it

variable bool bWarn = false;            // first toolnumber reminder

variable float sleeptime = 0;           // our own timer to set delay for locking - set to 0 for first run

variable int indexState = 1;           // current state of the index signal - for rising edge
variable int lastindexState = 1;       // previous state of the index signal - for rising edge

variable int pocketState = 1;           // current state of the pocket signal - for rising edge
variable int lastpocketState = 1;       // previous state of the pocket signal - for rising edge

variable int lockState = 1;           // current state of the pocket signal - for rising edge
variable int lastlockState = 1;       // previous state of the pocket signal - for rising edge


option singleton yes;                   // makes no sense to have more than one of these components running - only one ATC
function _;
author "pippin88 pippin88@imagineant.com";
license "GPL";
;;


FUNCTION(_)
{
    switch (progresslevel)
        {
        case 0:  // idle waiting for toolchange request
                //first make sure X and Z axes are homed. If turret is not homed, home it
                if((toolchange==1)&&(turrethomed==0)&&(ishomedX)&&(ishomedZ)){
                  progresslevel = 1; //call case 1 - homing sequence
                  break;
                  }
                if(toolchange==1 && toolchanged==0 && turrethomed==1) { // if tool change requested and not done and turret already homed
                    if(currenttoolnumber && reqtoolnumber != currenttoolnumber && reqtoolnumber > 0 && reqtoolnumber < 9) { // if a valid number
                        // switch off motor if already on - both pins high = stop
                        motorA = 1;    //    "Motor input A";
                        motorB = 1;    //    "Motor input B";

                        //delaystart = false;         //new toolchange - reset comp
                        sleeptime = 0;
                        progresslevel = 2; //call case 2
                        break;
                        }
                    else    // if tool requested is out of range set the toolchanged flag via case 5 and exit
                            // should only get this if tool table has more tools than ATC can have
                            // otherwise emc will error the M6 command
                        {
                        progresslevel = 5;
                        // switch off motor if already on - both pins high = stop
                        motorA = 1;    //    "Motor input A";
                        motorB = 1;    //    "Motor input B";
                        }
                }
                if(toolchange==0 && toolchanged==1)
                      //Turn motor OFF
                      motorA = 1;    //    "Motor input A";
                      motorB = 1;   //    "Motor input B";
                      toolchanged = false;           // reset toolchange flag after reset by linuxcnc

                if(toolchange && currenttoolnumber==0) // if no tool is set in axis - set axis to tool requested so that can work next time
                     {
                       // switch off motor if already on
                     motorA = 1;    //    "Motor input A";
                     motorB = 1;    //    "Motor input B";
                     progresslevel = 5;
                     }
                break;


        case 1: // homing
                motorA = 0;    //    "Motor input A"; //need to check direction!!!!
                motorB = 1;    //    "Motor input B";
                // use a state machine to detect the index pulse only once
                indexState = index; // read the pocket sensor input pin
                if (lastindexState != indexState) { // compare the pocketState to its previous state
                    if (index == 1) {   // if the current state is HIGH then the index went from off to on - RISING EDGE
                      //motorA = 1;
                      //motorB = 1;
                      position = 1; //Set Tool Pocket 1
                      turrethomed = 1; //Set turrent homed flag
                      progresslevel = 2; //move to motor forward sequence
                    }
                }
                lastindexState = indexState; //store indexState for comparison next time around
                break; // index not triggered - get out of case 1 and go round again


        case 2: // Turns on motor forward
                // Run forward - need to clarify pins etc
                motorA = 0;    //    "Motor input A";
                motorB = 1;    //    "Motor input B";
                progresslevel = 3;
                break; //get out of case 2 - - go round again

        case 3: // Forward move until desired tool position
                if(position==0)  // if returning 0 something is wrong - should be homed and at pocket 1 at least
                   {
                   rtapi_print_msg(RTAPI_MSG_ERR, "Error - counter error");
                   progresslevel = 12; // doesn't exist so will go to default, output msg and then sit in level 10
                   break;
                   }
                //use a state machine to detect pocket sensor once per position only
                pocketState = pocket; // read the pocket sensor input pin
                if (lastpocketState != pocketState) { // compare the pocketState to its previous state
                    if (pocket == 1) {   // if the state has changed, increment the counter
                      // if the current state is HIGH then the button went from off to on - RISING EDGE
                      position++;
                      if(position > 8)             // add 1 so that stops on sector after required & reverses back to it
                      position = position - 8;
                    }
                }
                lastpocketState = pocketState;    // save the current state as the last state, for next time through the loop

                sleeptime = 0; //reset counter to 0
                //delaystart = true; //signal to linuxcnc there will be a delay
                if (position == reqtoolnumber) {
                  progresslevel = 4; //move to case 3
                }
                break; //get out of case 2 - go round again

        case 4: // Locking sequence
                // programmed delay to allow time to align with locking holes
                if(sleeptime < times)
                {
                      sleeptime++;
                      break;
                }
                // Reverse locking move - need to clarify pins etc
                motorA = 1;    //    "Motor input A";
                motorB = 0;    //    "Motor input B";
                //use a state machine to detect lock sensor once only - detect falling edge
                lockState = lock; // read the pocket sensor input pin
                if (lastlockState != lockState) { // compare the pocketState to its previous state
                    if (lock == 0) {   // if the state has changed, increment the counter
                      // if the current state is LOW then the lock circuit has grounded
                      //Turn motor OFF
                      motorA = 1;    //    "Motor input A";
                      motorB = 1;   //    "Motor input B";
                      progresslevel = 5; // after first toolchange or update of tool number this is default
                    }
                }
                lastlockState = lockState;    // save the current state as the last state, for next time through the loop
                break;  //get out of case 3 - go round again

        case 5: // clean up ready for next toolchange
                //delaystart = true;     // start the 5 second delay relay component to give time to latch
                progresslevel = 0;
                //delaystart=false;
                toolchanged = true;   // signal finished
                break; //get out of case 5 - go round again

        case 10:   break;  // should never get here but if we do then loop endlessly doing nothing

        default:
                progresslevel = 10;
                rtapi_print_msg(RTAPI_MSG_ERR, "Error state in PC200changer - now disabled - unload PC200changer");
                break;
        }

}

/*************************************************************************************************************************************

LinuxCNC HAL component to implement ATC toolchanging on Hercus PC200 Lathe
Based on "EMC2 HAL component to implement ATC toolchanging in Denford ORAC CNC lathes" (c) ArcEye 01122011 schooner30@tiscali.co.uk

Modified by pippin88 - 07/10/2017

All values are metric

Version 0c -  The PC200 toolchanger has an 8 position unidirectional turret. There is an index sensor (PC200changer.index),
              and a position sensor (PC200changer.pocket) that pulses once per tool position.
  			      When the motor moves forwards, the turret rotates forward.
              The position sensor does not line up exactly with the actual tool position, but is past the tool position a bit
               - the turret reverses until the spring-loaded pins line up and jump into the locking holes.
              When the locking pins are fully deployed, they close a circuit (PC200changer.lock)

              The turret uses a simple DC motor. This is driven by an H-bridge (L298N) which takes two input signals to determine direction.
              motor A and motor B. A HIGH, B HIGH = motor off; A HIGH, B LOW = motor forward; A LOW, B HIGH = motor reverse; A LOW, B LOW = motor off;
              I use A HIGH, B HIGH for off, as this is less susceptible to noise than a resting LOW state.

               Compile the component with 'sudo comp --install PC200changer.comp'

               Below .hal file extracts give a specimen .hal connection guide. Place them as appropriate in .hal file

###############################################################
# loading the toolchange component in .hal file
###############################################################

loadrt PC200changer
addf PC200changer servo-thread

net tool-change iocontrol.0.tool-change => PC200changer.toolchange
net tool-changed iocontrol.0.tool-changed <= PC200changer.toolchanged
net tool-number iocontrol.0.tool-prep-number => PC200changer.reqtoolnumber
net tool-oldnumber iocontrol.0.tool-number => PC200changer.currenttoolnumber
net tool-prepare-loopback iocontrol.0.tool-prepare => iocontrol.0.tool-prepared

net turret-lock PC200changer.lock <= hm2_7i43.0.7i76.0.0.input-25 #Turret Lock sensor (White)
net turret-index PC200changer.index <= hm2_7i43.0.7i76.0.0.input-26 #Turret Index (Green)
net turret-pocket PC200changer.pocket <= hm2_7i43.0.7i76.0.0.input-27 #Turret Pulse (Blue)

## GPIO33 = Turret motorA
setp hm2_7i43.0.gpio.033.is_output true
setp hm2_7i43.0.gpio.033.out true ### pull high at startup!!! Can change if needed
## GPIO34 = Turret motorB
setp hm2_7i43.0.gpio.034.is_output true
setp hm2_7i43.0.gpio.034.out true ### pull high at startup!!! Can change if needed

net turret-motorA PC200changer.motorA => hm2_7i43.0.gpio.033.out
net turret-motorB PC200changer.motorB => hm2_7i43.0.gpio.034.out

net xhomed joint.0.homed => PC200changer.ishomedX
net zhomed joint.1.homed => PC200changer.ishomedZ

#setp PC200changer.times 125 ### where nnn is the new value - sets a sleep timer

################################################################################

***************************************************************************************************************************************/

component PC200changer                  "This component controls the PC200 Lathe Auto Tool Changer. M6 calls this";

pin in bit toolchange                   "Receives signal from M6 that tool change required";
pin in s32 reqtoolnumber                "Requested Tool Number - Tx data from M6. Only 1-8 allowed";
pin in s32 currenttoolnumber            "Receives old tool number from hal / axis";
pin out bit toolchanged = false         "Sends signal when tool change finished";

pin in bit index = 1                    "State of turret index sensor";
pin in bit pocket = 1                   "State of turret pocket sensor";
pin in bit lock = 1                     "State of turret lock sensor";

pin out bit motorA = 1                  "Motor signal A";
pin out bit motorB = 1                  "Motor signal B";

pin in bit ishomedX = false             "Status of X axis homing";
pin in bit ishomedZ = false             "Status of Z axis homing";

pin out s32 turretposition = 0          "Turret Position - Initialised as a pin for debugging so we can check where it thinks it is";

pin out s32 turrethomed = 0             "Turret Homed signal - Initialised as a pin for debugging so we can check where it thinks it is";

param rw float times = 290             "Length of delay";

// Internal and debugging stuff
pin out s32 progresslevel = 0;         // tracks the progress of the toolchange, just here so it can be read easily
//pin out s32 tnumber = 0;               // Internal toolnumber to allow overrun of quadrant by 1 then reverse back onto it

variable bool bWarn = false;            // first toolnumber reminder

variable float sleeptime = 0;           // our own timer to set delay for locking - set to 0 for first run

pin out bit indexState = 1;           // current state of the index signal - for rising edge
pin out bit lastindexState = 1;       // previous state of the index signal - for rising edge

pin out bit unlockState = 1;           // current state of the index signal - for rising edge
pin out bit lastunlockState = 1;       // previous state of the index signal - for rising edge

pin out bit pocketState = 1;           // current state of the pocket signal - for rising edge
pin out bit lastpocketState = 1;       // previous state of the pocket signal - for rising edge

pin out bit lockState = 1;           // current state of the pocket signal - lock is detected on falling edge
pin out bit lastlockState = 1;       // previous state of the pocket signal

pin out bit turretislocked = 0;

option singleton yes;                   // makes no sense to have more than one of these components running - only one ATC
function _;
author "pippin88 pippin88@imagineant.com";
license "GPL";
;;


FUNCTION(_)
{
    switch (progresslevel)  {
        case 0: // idle waiting for toolchange request
                //first make sure X and Z axes are homed. If turret is not homed, home it
                if((toolchange==1)&&(turrethomed==0)&&(ishomedX)&&(ishomedZ)){
                      progresslevel = 1; //call case 1 - homing sequence
                      break;
                      }
                if((toolchange==1)&&(toolchanged==0)&&(turrethomed==1)) { // if tool change requested and not done and turret already homed
                      if(currenttoolnumber && reqtoolnumber != currenttoolnumber && reqtoolnumber > 0 && reqtoolnumber < 9) { // if a valid number
                            // switch off motor if already on - both pins high = stop
                            motorA = 1;    //    "Motor input A";
                            motorB = 1;    //    "Motor input B";

                            //sleeptime = 0;
                            progresslevel = 2; //call case 2
                            break;
                            }
                      else    // if tool requested is out of range set the toolchanged flag via case 5 and exit
                              // should only get this if tool table has more tools than ATC can have
                              // otherwise emc will error the M6 command
                          {
                          progresslevel = 5;
                          // switch off motor if already on - both pins high = stop
                          motorA = 1;    //    "Motor input A";
                          motorB = 1;    //    "Motor input B";
                          }
                }
                if(!toolchange) {
                      //Turn motor OFF
                      motorA = 1;    //    "Motor input A";
                      motorB = 1;   //    "Motor input B";
                      toolchanged = 0;           // reset toolchange flag after reset by linuxcnc
                    }
                //if(toolchange && currenttoolnumber==0) // if no tool is set in axis - set axis to tool requested so that can work next time
                //     {
                       // switch off motor if already on
                //     motorA = 1;    //    "Motor input A";
                //     motorB = 1;    //    "Motor input B";
                //     progresslevel = 5;
                //     }
                break;


        case 1: // HOMING SEQUENCE
                // Turn motor forward
                motorA = 0;    //    "Motor input A";
                motorB = 1;    //    "Motor input B";
                // use a state machine to detect the index pulse only once
                indexState = index; // read the index sensor input pin
                if (lastindexState != indexState) { // compare the indexState to its previous state
                      if (index == 0) {   // if the current state is LOW then the index went from ON to OFF - FALLING EDGE
                          turretposition = 8; //Set Tool Pocket 1
                          turrethomed = 1; //Set turrent homed flag
                  			  if (reqtoolnumber == 8){
                              progresslevel = 4; //move to locking sequence
                  			  } else {
                  			     progresslevel = 2; //move to motor forward sequence
                  			  }
                      }
                }
                lastindexState = indexState; //store indexState for comparison next time around
                break; // index not triggered - get out of case 1 and go round again


        case 2: // UNLOCKING - use first pocket pulse to show that turret unlocked, and to prevent that first pulse counting as a position increment
                // Run forward - need to clarify pins etc
                motorA = 0;    //    "Motor input A";
                motorB = 1;    //    "Motor input B";
                //use a state machine to detect pocket sensor
                unlockState = pocket; // read the pocket sensor input pin
                if (lastunlockState != unlockState) { // compare the pocketState to its previous state
                      if (pocket == 1) { // if the current state is HIGH then the button went from OFF to ON - RISING EDGE
                          progresslevel = 3;
                      }
                }
                lastunlockState = unlockState;    // save the current state as the last state, for next time through the loop
		            break; //get out of case 2 - - go round again


        case 3: // FORWARD TOOL FIND SEQUENCE - Forward move until desired tool position

                motorA = 0;    //    "Motor input A";
                motorB = 1;    //    "Motor input B";

                if(turretposition==0) { // if returning 0 something is wrong - should be homed and at pocket 1 at least
                      rtapi_print_msg(RTAPI_MSG_ERR, "Error - counter error");
                      progresslevel = 12; // doesn't exist so will go to default, output msg and then sit in level 10
                      break;
                }

		            //use a state machine to detect pocket sensor once per position only
                pocketState = pocket; // read the pocket sensor input pin
                if (lastpocketState != pocketState) { // compare the pocketState to its previous state
                      if (pocket == 0) {   // if the state has changed, increment the counter
                      // if the current state is LOW then the button went from ON to OFF - FALLING EDGE
                          turretposition++; //increment tool position counter
                          if(turretposition > 8)
                          turretposition = turretposition - 8; //we have gone round from 8 to 1
                    }
                }
                lastpocketState = pocketState;    // save the current state as the last state, for next time through the loop

                if (turretposition == reqtoolnumber) {
                      progresslevel = 4; //move to case 4
                }
                break; //get out of case 3 - go round again

        case 4: // LOCKING SEQUENCE
                // Reverse locking move
                motorA = 1;    //    "Motor input A";
                motorB = 0;    //    "Motor input B";
                //use a state machine to detect lock sensor once only - detect falling edge
                lockState = lock; // read the pocket sensor input pin
                if (lastlockState != lockState) { // compare the pocketState to its previous state
                    if (lock == 0) {   // if the state has changed, increment the counter
                      // if the current state is LOW then the lock circuit has grounded
                      //Turn motor OFF
                      motorA = 1;    //    "Motor input A";
                      motorB = 1;   //    "Motor input B";
                      turretislocked = 1;
                      progresslevel = 5;
                    }
                }
                lastlockState = lockState;    // save the current state as the last state, for next time through the loop
                break;  //get out of case 4 - go round again

        case 5: // clean up ready for next toolchange
                //sleeptime = 0;
                progresslevel = 6;
                toolchanged = 1;   // signal finished
                break; //get out of case 5 - go round again

        case 6:
              //if(sleeptime < 1)
              //{
              //      sleeptime++;
              //      break;
              //}
                progresslevel = 0;
                break;

        case 10:   break;  // should never get here but if we do then loop endlessly doing nothing

        default:
                progresslevel = 10;
                rtapi_print_msg(RTAPI_MSG_ERR, "Error state in PC200changer - now disabled - unload PC200changer");
                break;
        }

}