toolchanger.comp

component toolchanger "This component controls the Boxford 240 Lathe Auto Tool ";

pin in bit enable = false "Toolchanger enable signal";

pin in bit tool_change "Receives signal from M6 that tool change required";
pin out bit tool_changed "Sends signal when tool change finished";
pin in s32 tool_requested "Receives Tx data from M6 (tool number requested) Only allows 1-8";
pin in s32 tool_current "Receives old tool number";
pin out s32 tool_physical = 0; // Reflects current locked tool in ATC
pin out s32 tool_target = 0; // Reflects requested tool number

pin out bit tool_locking = false; // Indicates that tool is about to get locked
pin out bit tool_locked = false; 
pin out bit tool_process = false; 

pin out s32 event_step = 0; 

pin in bit atc_limit "ATC limit switch";

pin in float position_fb "Position-feedback from stepgen";
pin out float position_cmd "Sends location required";
variable float position_req = 0; // Where we want to be

pin out bit homed = false "Sends signal when ATC has finished homing";
pin out bit homing = false "Sends signal when ATC is currently homing";

pin in bit axis_homing;
pin in bit axis_homed;

option singleton yes;

function _;
license "GPL";
;;

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>

#define HOMING_DELAY 1;
#define LOCKING_DELAY 1;

#define TOOL_MOVE 52; // tool move including moving away from the lock
#define TOOL_CLEARMOVE 44.75; 
#define LATCHING_MOVE 5 // threshold on when to start the slow move
#define LOCK_MOVE 7; // move in oposit direction, locking the tool
#define LOCK_STOP 0.1; // slamming into physical stop

#define STEP_LOCKING 0.001;
#define STEP_SLOW 0.005;
#define STEP_NORMAL 0.01;
#define STEP_FAST 0.03;

static bool delay = false;
static float timer;
static int remaining(const int current, const int target);

enum events {
  RESET,
  PREPARE_HOMED,
  PREPARE_SLEEP,
  HOMING_START,
  HOMING_SEARCH,
  HOMING_LATCH,
  HOMING_FINISH,
  CHANGE_INIT,
  CHANGE_CONTROL,
  CHANGE_PROGRESS,
  CHANGE_TOOL,
  CHANGE_ADVANCE,
  CHANGE_LOCK,
  CHANGE_COMPLETE,
  DEFAULT
};

enum events event;

FUNCTION(_) {

  if ( axis_homing ) event = RESET;

  if ( tool_change && homed ) {      
    if ( !tool_process && !tool_changed ) {
      event = CHANGE_INIT;
    }
  } 
  
  if( !tool_change ) {
    tool_process = false;
    tool_changed = false; // reset once toolchange flag reset by system
  }

  switch (event) {

  case RESET:

    event_step = 1;

    // reset
    homed = 0;

    if (axis_homed) event = PREPARE_HOMED;

    break;

  case PREPARE_HOMED:

    event_step = 2;

    // machine is homed, register time and dispatch sleep event
    if (delay == false) {
      delay = true;
      timer = HOMING_DELAY;

      event = PREPARE_SLEEP;
    }

    break;

  case PREPARE_SLEEP:

  event_step = 3;

    timer -= fperiod;

    if (timer <= 0) {
      timer = 0;
      delay = false;

      event = HOMING_START;
    }

    break;

  case HOMING_START:

    event_step = 4;

    // check if limit switch is tripped
    // if it is, release it by moving away
    if (atc_limit) {
      position_cmd = 25;
    } else {
      event = HOMING_SEARCH;
    }

    break;

  case HOMING_SEARCH:

    event_step = 5;

    if (!atc_limit) {
      position_cmd = 360 + 50;
    } else {
      position_cmd = position_fb + LATCHING_MOVE;

      event = HOMING_LATCH;
    }

    break;

  case HOMING_LATCH:

    event_step = 6;

    if (atc_limit) {
      position_cmd += 1;
    } else {    
      position_cmd = position_fb - 23;
      if (delay == false) {
        timer = LOCKING_DELAY;
        delay = true;
      }
      event = HOMING_FINISH;
    }

    break;

  case HOMING_FINISH:

    event_step = 7;

    timer -= fperiod;

    if (timer <= 0) {
      timer = 0;

      delay = false;

      homed = true;
      
      tool_locked = true;
      
      tool_physical = 2;

      event = DEFAULT;
    }
    break;

  case CHANGE_INIT:

    event_step = 8;
    
    if ( tool_physical == tool_requested ) {
      // current tool is the tool requested, do nothing. 
      tool_changed = true;
      
      event = DEFAULT;
    } else {
      tool_target = tool_requested;

      tool_process = true;

      event = CHANGE_CONTROL;
    }
    break;

  case CHANGE_CONTROL:  

    event_step = 9;

    if ( tool_physical == tool_target ) {
      // current tool is the tool requested, do nothing. 
      tool_changed = true;
         
      event = DEFAULT;
    } else {
      
      event = CHANGE_PROGRESS;
    }

    break;

  case CHANGE_PROGRESS:

    event_step = 10;

    if ( remaining(tool_physical, tool_target) == 1 ) {
      tool_locking = true;
    } else {
      tool_locking = false;
    }

    // add check for lock move
    event = CHANGE_TOOL;

    break;

  case CHANGE_TOOL: 

    event_step = 11;

    if ( tool_locked == true ) {
      position_req = position_fb + TOOL_MOVE;      
    } else {
      position_req = position_fb + TOOL_CLEARMOVE;   
    }

    event = CHANGE_ADVANCE;

    break;

  case CHANGE_ADVANCE:

    event_step = 12;
    
    // tool is not in position, lets move forward
    if ( position_fb < position_req ) {
      if ( tool_locking ) {
        if ( position_fb < ( position_req - LATCHING_MOVE ) ) {
          position_cmd += STEP_FAST;
        } else {
          position_cmd += STEP_NORMAL;
        }      
      } else {
        position_cmd += STEP_FAST;
      }
    }

    // necessery move has been complete, decide what to do next
    if ( position_fb >= position_req ) {
      if ( tool_locking == true ) {
        position_req = position_fb - LOCK_MOVE - LOCK_STOP;

        event = CHANGE_LOCK;
      } else {
        // moved tool without locking it
        tool_locked = false;

        event = CHANGE_COMPLETE;
      }
    }  

    break;

  case CHANGE_LOCK:

    event_step = 13;

    if ( position_fb > position_req ) {
      if ( ( position_req - position_fb ) < LATCHING_MOVE ) {
        position_cmd -= STEP_SLOW;
      } else {
        position_cmd -= STEP_LOCKING;
      }   
    }

    if ( position_fb <= position_req ) {
      // moved tool and locked it
      tool_locked = true;

      event = CHANGE_COMPLETE;
    }

    break;

  case CHANGE_COMPLETE:

    event_step = 14;

    // tool change complete, advance tool counter
    tool_physical += 1;

    if ( tool_physical > 8 ) tool_physical = tool_physical - 8;

    event = CHANGE_CONTROL;
    
    break;

  default:

    event_step = 15;
    
    break;
  }
}

/* function descripgion */
int remaining(const int current, const int target) {
  int result;

  result = 8 - current + target;

  if ( result > 8 ) result = result - 8;
 
  return result; 
}