Comments (2)
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 *)¤t;
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.
from core.
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
from core.
Related Issues (20)
- protocol_enqueue_gcode does not set last_error HOT 3
- probe enabled by default HOT 1
- MKS Tinybee cannot connect to the router using STA HOT 3
- Cannot Restore After Feedhold When Issued During Change of Spindle State HOT 10
- Not an issue: Want to know if macro variables are available for $342 to $344. HOT 3
- Tool table offsets are not persistent over power cycle HOT 6
- Joggin unavailable for many commands HOT 3
- Lathe Diameter/Radius offset report issue HOT 1
- kinda vfd support HOT 3
- Macro Issue that I can not figure out. HOT 6
- I noticed that the current Setting_AxisStepsPerMM setting has 5 zeros after the decimal point. Is there any reason for doing this? HOT 2
- regression in websocket init_message caused by commit a912ce4 HOT 1
- Is the data in the driver capabilities list dependably up to date? HOT 2
- set machine origin to 0, or bit3, not working for $22 HOT 5
- octopus max STM32H723 HOT 1
- Not correctly reporting coordinates? HOT 4
- Experiencing issues with ESP32 Blackbox Build HOT 10
- How to limit the maximum output pulse frequency to prevent the user from setting an excessively high pulse rate that could cause grblHAL to crash? HOT 1
- Delta Kinematics: Y axis moves are not executing correctly - Motor duplication? HOT 1
- Syntax error / typo in ngc_expr.c HOT 3
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from core.