Automatic Tool Change

[everskies]

Hey! I'm writing a plugin for a DIY ATC.

When looking through the current tool change code, it doesn't seem like I can easily plug into the logic without modifying at least the tool_change.c logic to account for an ATC plugin (And adding an ATC entry in toolchange_mode_t)

Would this be the correct way to approach this?
The idea is to have GRBL move the Z axis out of the way, send a command either through UART or I2C to a different board that handles rotating of the tool umbrella, swapping the tool and giving control back to GRBL for probing once done.

I'd want this to result in an eventual pull request with example skeleton code in the near future hence I'm asking if my approach is right!

[terjeio]

When looking through the current tool change code, it doesn't seem like I can easily plug into the logic without modifying at least the tool_change.c logic to account for an ATC plugin (And adding an ATC entry in toolchange_mode_t)

Would this be the correct way to approach this?

No, you should write your plugin to replace the current tool change code by claiming the function pointers early and setting hal.driver_cap.atc true.

Here are some code I have played with, a while back now, it may give you ideas of how to approach it:

/*
  atc.c - An embedded CNC Controller with rs274/ngc (g-code) support

  Driver code for my Mini Mill ATC, 8 tools arranged in a circle
  A motorized socket wrench is mounted in the center, used for opening/closing the spindle nut

  Part of grblHAL

  Copyright (c) 2018-2020 Terje Io

  Grbl 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.

  Grbl 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 Grbl.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <msp.h>
#include <math.h>
#include <string.h>

#include "grbl/hal.h"
#include "grbl/protocol.h"
#include "grbl/motion_control.h"

#define ATC_I2C_ADDRESS (0x4A)

typedef struct {
    float x;
    float y;
} pos_t;

typedef enum {
    CMD_Version = 0,
    CMD_Motor,
    CMD_Latch,
    CMD_SetCurrent,
    CMD_GetState
} atc_command_t;

typedef union {
    uint8_t value;
    struct {
        uint8_t nut_locked   :1,
                nut_unlocked :1,
                spindle_locked :1,
                spindle_unlocked :1;
    };
} atc_state_t;

typedef enum {
    Motor_Off = 0,
    Motor_CW = 1,
    Motor_CCW = 2
} atc_motor_state_t;

typedef struct {
    uint8_t addr;
    volatile int16_t count;
    uint8_t *data;
    atc_command_t command;
} i2c_trans_t;

static i2c_trans_t i2c;

static uint16_t current = 100; //856;
static const float r1 = 22.0f, r2 = 31.25f, z_nut = 5.0f, z_tools = 40.0f, z_clear = 40.0f, z_base = 0.0f, z_tool_clearance = 17.0f;
static tool_data_t *current_tool = NULL, *next_tool = NULL;
static coord_data_t offset;
static driver_reset_ptr driver_reset = NULL;

static void StartI2C (bool read)
{
    bool single = i2c.count == 1;

    EUSCI_B1->I2CSA = i2c.addr;                                         // Set EEPROM address and MSB part of data address
    EUSCI_B1->IFG &= ~(EUSCI_B_IFG_TXIFG0|EUSCI_B_IFG_RXIFG0);          // Clear interrupt flags
    EUSCI_B1->CTLW0 |= EUSCI_B_CTLW0_TR|EUSCI_B_CTLW0_TXSTT;            // Transmit start condition and address
    while(!(EUSCI_B1->IFG & EUSCI_B_IFG_TXIFG0));                       // Wait for TX
    EUSCI_B1->TXBUF = i2c.command;                                      // Transmit data address LSB
//    EUSCI_B1->IFG &= ~EUSCI_B_IFG_TXIFG0;                               // Clear TX interrupt flag and
    while(!(EUSCI_B1->IFG & EUSCI_B_IFG_TXIFG0));                       // wait for transmit complete

    if(read) {                                                          // Read data from EEPROM:
        EUSCI_B1->CTLW0 |= EUSCI_B_CTLW0_TXSTP;                         // Transmit STOP condtition
        while (EUSCI_B1->CTLW0 & EUSCI_B_CTLW0_TXSTP);                  // and wait for it to complete
        EUSCI_B1->CTLW0 &= ~EUSCI_B_CTLW0_TR;                           // Set read mode
        if(single)                                                      // and issue
            EUSCI_B1->CTLW0 |= EUSCI_B_CTLW0_TXSTT|EUSCI_B_CTLW0_TXSTP; // restart and stop condition if single byte read
        else                                                            // else
            EUSCI_B1->CTLW0 |= EUSCI_B_CTLW0_TXSTT;                     // restart condition only

        while(i2c.count) {                                              // Read data...
            if(!single && i2c.count == 1) {
                EUSCI_B1->CTLW0 |= EUSCI_B_CTLW0_TXSTP;
                while (EUSCI_B1->CTLW0 & EUSCI_B_CTLW0_TXSTP) {
                    while(!(EUSCI_B1->IFG & EUSCI_B_IFG_RXIFG0));
                }
            } else
                while(!(EUSCI_B1->IFG & EUSCI_B_IFG_RXIFG0));
            i2c.count--;
            *i2c.data++ = EUSCI_B1->RXBUF;
        }
    } else {                                                            // Write data to EEPROM:
        while (i2c.count--) {
            EUSCI_B1->TXBUF = *i2c.data++;
            while(!(EUSCI_B1->IFG & EUSCI_B_IFG_TXIFG0));
        }
        EUSCI_B1->CTLW0 |= EUSCI_B_CTLW0_TXSTP;                         // I2C stop condition
 //       WaitForACK();
        hal.delay_ms(5, 0);                                             // Wait a bit for the write cycle to complete
    }
    while (EUSCI_B1->CTLW0 & EUSCI_B_CTLW0_TXSTP);                      // Ensure stop condition got sent
}

static void start_motor(atc_motor_state_t state)
{
    i2c.addr = ATC_I2C_ADDRESS;
    i2c.count = 1;
    i2c.command = CMD_Motor;
    i2c.data = &state;

    StartI2C(false);
}

static void lock_spindle(bool lock)
{
    i2c.addr = ATC_I2C_ADDRESS;
    i2c.count = 1;
    i2c.command = CMD_Latch;
    i2c.data = (uint8_t *)&lock;

    StartI2C(false);
}

static atc_state_t atc_state (void)
{
    atc_state_t state;

    i2c.addr = ATC_I2C_ADDRESS;
    i2c.count = 1;
    i2c.command = CMD_GetState;
    i2c.data = &state.value;

    StartI2C(true);

    return state;
}

static void atc_reset (void)
{
    lock_spindle(false);
    start_motor(Motor_Off);

    driver_reset();
}

static bool atc_move (coord_data_t position, plan_line_data_t *plan_data)
{
    uint_fast8_t idx = N_AXIS;

    do {
        idx--;
        position.values[idx] += offset.values[idx];
    } while(idx);

    return mc_line(position.values, plan_data);
}

static bool spindle_nut (plan_line_data_t *plan_data, float zpos, bool open)
{
    coord_data_t target;

    memset(&target, 0, sizeof(target)); // Zero plan_data struct

    // move to z clearance
    target.z = zpos;
    atc_move(target, plan_data);

    // spin up spindle briefely and lock spindle
    hal.spindle.set_state((spindle_state_t){ .on = On, .ccw = Off }, 100.0f);
    hal.delay_ms(500, NULL);
    hal.spindle.set_state((spindle_state_t){0}, 0.0f);
    lock_spindle(true);
    do {
        hal.delay_ms(50, NULL);
        if(!protocol_execute_realtime())
            return false;
    } while(!atc_state().spindle_locked);

    // move to just above socket wrench
    target.z = z_nut + 5.0f;
    atc_move(target, plan_data);
    protocol_buffer_synchronize();

    // start socket wrench motor
    start_motor(open ? Motor_CW : Motor_CCW);

    // engage socket wrench
    plan_data->condition.rapid_motion = Off;

    target.z = z_nut;
    atc_move(target, plan_data);
    protocol_buffer_synchronize();

    // wait for nut open/closed event
    do {
        hal.delay_ms(50, NULL);
        if(!protocol_execute_realtime())
            return false;
    } while(atc_state().nut_locked == open);

    // unlock spindle
    lock_spindle(false);
    do {
        hal.delay_ms(50, NULL);
        if(!protocol_execute_realtime())
            return false;
    } while(atc_state().spindle_locked);

    // move out of nut, to tool clearance
    plan_data->condition.rapid_motion = On;
    target.z = z_tools;
    atc_move(target, plan_data);

    return true;
}

static void atc_tool_select (tool_data_t *tool, bool next)
{
    if(next)
        next_tool = tool;
    else
        current_tool = tool;
}

static status_code_t atc_tool_change (parser_state_t *gc_state)
{
    if(current_tool == NULL || next_tool == NULL)
        return Status_GCodeToolError;

    if(current_tool == next_tool)
        return Status_OK;

    if(!sys.homed.mask || sys.homed.mask != sys.homing.mask)
        return Status_HomingRequired;

    //good to go?
    if(atc_state().value != 0)
        return Status_GCodeToolError;

    float angle;
    plan_line_data_t plan_data = {0};
    coord_data_t target = {0}, previous;

    i2c.addr = ATC_I2C_ADDRESS;
    i2c.count = 2;
    i2c.command = CMD_SetCurrent;
    i2c.data = (uint8_t *)&current;

    StartI2C(false);

    // Save current position
    system_convert_array_steps_to_mpos(previous.values, sys.position);

    // G59.3 contains offsets to position of socket wrench center (X, Y) and spindle nut offset above ATC base plate
    settings_read_coord_data(CoordinateSystem_G59_3, &offset.values); // G59.3 - fail if not set?

    // Stop spindle and coolant
    hal.spindle.set_state((spindle_state_t){0}, 0.0f);
    hal.coolant.set_state((coolant_state_t){0});

    plan_data.feed_rate = 100.0f;
    plan_data.condition.rapid_motion = On;

    // Initial move to safe Z above socket wrench
    if(z_clear + offset.values[Z_AXIS] < previous.z) {
        target.x += offset.x;
        target.y += offset.y;
        target.z = previous.z;
    } else {
        target.x = previous.x;
        target.y = previous.y;
        target.z = z_clear + offset.values[Z_AXIS];
    }
    if(!mc_line(target.values, &plan_data))
        return Status_Reset;

    // Disengage (open) spindle nut
    if(!spindle_nut(&plan_data, z_clear, true))
        return Status_Reset;

    // put current tool back
    angle = 0.25f * M_PI * (float)(current_tool->tool - 1);

    target.z = z_tools;
    target.x = r1 * sinf(angle);
    target.y = r1 * cosf(angle);
    if(!atc_move(target, &plan_data))
        return Status_Reset;

    target.z = z_base;
    // Trinamic 2130 - monitor stepper current?
    if(!atc_move(target, &plan_data))
        return Status_Reset;

    target.x = r2 * sinf(angle);
    target.y = r2 * cosf(angle);
    if(!atc_move(target, &plan_data))
        return Status_Reset;

    target.z = z_tool_clearance;
    if(!atc_move(target, &plan_data))
        return Status_Reset;

    // set next tool as current and fetch it
    current_tool = next_tool;

    // intermediate move to center of socket wrench
    target.x = 0.0f;
    target.y = 0.0f;
    if(!atc_move(target, &plan_data))
        return Status_Reset;

    // move to tool
    angle = 0.25f * M_PI * (float)(current_tool->tool - 1);
    target.x = r2 * sinf(angle);
    target.y = r2 * cosf(angle);
    if(!atc_move(target, &plan_data))
        return Status_Reset;

    // Spin up spindle
    protocol_buffer_synchronize();
    hal.spindle.set_state((spindle_state_t){ .on = On, .ccw = Off }, 100.0f);
    hal.delay_ms(200, NULL);

    // Engage tool
    // Trinamic 2130 - monitor stepper current?
    target.z = z_tool_clearance - 5.0f;
    plan_data.condition.rapid_motion = Off;
    if(!atc_move(target, &plan_data))
        return Status_Reset;

    protocol_buffer_synchronize();
    hal.spindle.set_state((spindle_state_t){0}, 0.0f);
    hal.delay_ms(200, NULL);
    plan_data.condition.rapid_motion = On;

    target.z = z_base + 0.5f; // a bit over the base to ensure proper return
    plan_data.condition.rapid_motion = On;
    if(!atc_move(target, &plan_data))
        return Status_Reset;

    // release
    target.x = r1 * sinf(angle);
    target.y = r1 * cosf(angle);
    if(!atc_move(target, &plan_data))
        return Status_Reset;

    // and remove it
    target.z = z_tools;
    plan_data.condition.rapid_motion = On;
    if(!atc_move(target, &plan_data))
        return Status_Reset;

    protocol_buffer_synchronize();

    // Tigthen spindle nut
    if(!spindle_nut(&plan_data, z_tools, false))
        return Status_Reset;

    // probe cycle...?

    // go back to previous position

    if(z_clear + offset.values[Z_AXIS] < previous.z) {
        target.x = offset.x;
        target.y = offset.y;
        target.z = previous.z;
    } else {
        target.x = previous.x;
        target.y = previous.y;
        target.z = z_clear + offset.values[Z_AXIS];
    }
    if(!mc_line(target.values, &plan_data))
        return Status_Reset;

    if(!mc_line(previous.values, &plan_data))
        return Status_Reset;

    // Restore coolant and spindle state
    coolant_sync(gc_state->modal.coolant);
    spindle_restore(gc_state->modal.spindle, gc_state->spindle.rpm);

    return Status_OK;
}

void atc_init (void)
{
    if(driver_reset == NULL) {
        driver_reset = hal.driver_reset;
        hal.driver_reset = atc_reset;
    }
    hal.tool.select = atc_tool_select;
    hal.tool.change = atc_tool_change;
}

atc_init() is to be called as part of driver initialization. hal.driver_cap.atc should be set to true in atc_init(), this will block the core tool change code from registering itself.

[everskies]

Perfect! Exactly what I needed. Thank you

Your approach seems very similar to mine, I'll be locking the drawbar mechanically and use the spindle motor to tighten/un-tighten it