Hi Tom,
Thanks for the speedy response! I'm driving a total of four stepper motors from the KSTEP, three of which are responsible for operating an XYZ motion stage and are used to defined the coordinate system (the fourth stepper is used for separate independent motion not driven by GCODE). I currently do not have any encoders on the stage, though I'm in the process of designing a mounting bracket to use one for tuning and diagnostics (though I'm hoping to remain open loop if possible to reduce cost, also the stage is custom-milled and does not have the space for 3 encoders as is).
The resolution of all three of these axes is 3200 counts/mm, and all of the steppers are 400 steps/rev.
I am currently using the latest version of the software available KMotion5.4.0
I don't explicitly have an Initialization C program anymore, instead I use a single state-full multi-file program which runs continuously in thread 1. The code includes a light abstraction layer I wrote, is compiled externally from KMotion and then downloaded through KMotion, and relies on a combination python and Makefile build system I've been developing which converts custom JSON configuration files to pre-defined symbols passed to the compiler when building. Assuming that you're most interested in the initialization of the axes, here is an excerpt from my motion_stage.c file which contains the initialization of the channel structures:
Code: Select all
/***********************/
/* Motion Stage X-Axis */
/***********************/
INST_IO(x_axis_negative_limit_pin, MOTION_STAGE_X_AXIS_LIMIT_SWITCH_NEGATIVE_LIMIT_BIT, IO_INPUT, MOTION_STAGE_X_AXIS_LIMIT_SWITCH_NEGATIVE_LIMIT_POLARITY, LOW);
INST_IO(x_axis_positive_limit_pin, MOTION_STAGE_X_AXIS_LIMIT_SWITCH_POSITIVE_LIMIT_BIT, IO_INPUT, MOTION_STAGE_X_AXIS_LIMIT_SWITCH_POSITIVE_LIMIT_POLARITY, LOW);
static const motion_stage_axis_config_t x_axis_config = {
.axis = AXIS_FROM_CHANNEL_NUM(X_AXIS_CHAN_NUM),
.max_velocity = MOTION_STAGE_X_AXIS_MAX_VELOCITY,
.max_acceleration = MOTION_STAGE_X_AXIS_MAX_ACCELERATION,
.max_jerk = MOTION_STAGE_X_AXIS_MAX_JERK,
.output_channel = MOTION_STAGE_X_AXIS_OUTPUT_CHANNEL,
.inverted = MOTION_STAGE_X_AXIS_INVERTED,
.negative_limit_bit = &x_axis_negative_limit_pin,
.positive_limit_bit = &x_axis_positive_limit_pin,
.soft_limit_neg = MOTION_STAGE_X_AXIS_SOFT_LIMIT_NEGATIVE * MOTION_STAGE_X_AXIS_COUNTS_PER_MM,
.soft_limit_pos = MOTION_STAGE_X_AXIS_SOFT_LIMIT_POSITIVE * MOTION_STAGE_X_AXIS_COUNTS_PER_MM,
.limit_switch_options = &x_axis_limit_switch_options,
.oversample = 4,
};
static motion_stage_axis_context_t x_axis_context = {0};
static motion_stage_axis_t x_axis = {
.config = &x_axis_config,
.context = &x_axis_context,
};
/***********************/
/* Motion Stage Y-Axis */
/***********************/
INST_IO(y_axis_negative_limit_pin, MOTION_STAGE_Y_AXIS_LIMIT_SWITCH_NEGATIVE_LIMIT_BIT, IO_INPUT, MOTION_STAGE_Y_AXIS_LIMIT_SWITCH_NEGATIVE_LIMIT_POLARITY, LOW);
INST_IO(y_axis_positive_limit_pin, MOTION_STAGE_Y_AXIS_LIMIT_SWITCH_POSITIVE_LIMIT_BIT, IO_INPUT, MOTION_STAGE_Y_AXIS_LIMIT_SWITCH_POSITIVE_LIMIT_POLARITY, LOW);
static const motion_stage_axis_config_t y_axis_config = {
.axis = AXIS_FROM_CHANNEL_NUM(Y_AXIS_CHAN_NUM),
.max_velocity = MOTION_STAGE_Y_AXIS_MAX_VELOCITY,
.max_acceleration = MOTION_STAGE_Y_AXIS_MAX_ACCELERATION,
.max_jerk = MOTION_STAGE_Y_AXIS_MAX_JERK,
.output_channel = MOTION_STAGE_Y_AXIS_OUTPUT_CHANNEL,
.inverted = MOTION_STAGE_Y_AXIS_INVERTED,
.negative_limit_bit = &y_axis_negative_limit_pin,
.positive_limit_bit = &y_axis_positive_limit_pin,
.soft_limit_neg = MOTION_STAGE_Y_AXIS_SOFT_LIMIT_NEGATIVE * MOTION_STAGE_Y_AXIS_COUNTS_PER_MM,
.soft_limit_pos = MOTION_STAGE_Y_AXIS_SOFT_LIMIT_POSITIVE * MOTION_STAGE_Y_AXIS_COUNTS_PER_MM,
.limit_switch_options = &y_axis_limit_switch_options,
.oversample = 4,
};
static motion_stage_axis_context_t y_axis_context = {0};
static motion_stage_axis_t y_axis = {
.config = &y_axis_config,
.context = &y_axis_context,
};
/***********************/
/* Motion Stage Z-Axis */
/***********************/
INST_IO(z_axis_negative_limit_pin, MOTION_STAGE_Z_AXIS_LIMIT_SWITCH_NEGATIVE_LIMIT_BIT, IO_INPUT, MOTION_STAGE_Z_AXIS_LIMIT_SWITCH_NEGATIVE_LIMIT_POLARITY, LOW);
INST_IO(z_axis_positive_limit_pin, MOTION_STAGE_Z_AXIS_LIMIT_SWITCH_POSITIVE_LIMIT_BIT, IO_INPUT, MOTION_STAGE_Z_AXIS_LIMIT_SWITCH_POSITIVE_LIMIT_POLARITY, LOW);
static const motion_stage_axis_config_t z_axis_config = {
.axis = AXIS_FROM_CHANNEL_NUM(Z_AXIS_CHAN_NUM),
.max_velocity = MOTION_STAGE_Z_AXIS_MAX_VELOCITY,
.max_acceleration = MOTION_STAGE_Z_AXIS_MAX_ACCELERATION,
.max_jerk = MOTION_STAGE_Z_AXIS_MAX_JERK,
.output_channel = MOTION_STAGE_Z_AXIS_OUTPUT_CHANNEL,
.inverted = MOTION_STAGE_Z_AXIS_INVERTED,
.negative_limit_bit = &z_axis_negative_limit_pin,
.positive_limit_bit = &z_axis_positive_limit_pin,
.soft_limit_neg = MOTION_STAGE_Z_AXIS_SOFT_LIMIT_NEGATIVE * MOTION_STAGE_Z_AXIS_COUNTS_PER_MM,
.soft_limit_pos = MOTION_STAGE_Z_AXIS_SOFT_LIMIT_POSITIVE * MOTION_STAGE_Z_AXIS_COUNTS_PER_MM,
.limit_switch_options = &z_axis_limit_switch_options,
.oversample = 4,
};
static motion_stage_axis_context_t z_axis_context = {0};
static motion_stage_axis_t z_axis = {
.config = &z_axis_config,
.context = &z_axis_context,
};
/****************/
/* Motion Stage */
/****************/
static const motion_stage_config_t motion_stage_config = {
.axis.x_axis = &x_axis,
.axis.y_axis = &y_axis,
.axis.z_axis = &z_axis,
};
static motion_stage_context_t motion_stage_context = {0};
static const motion_stage_t motion_stage_inst = {
.config = &motion_stage_config,
.context = &motion_stage_context,
};
const motion_stage_t *const motion_stage = &motion_stage_inst;
/** @addtogroup Motion_Stage_Initialization
* @{
*/
static void initialize_motion_stage_axis(motion_stage_axis_t *motion_stage_axis) {
const motion_stage_axis_config_t *config = motion_stage_axis->config;
motion_stage_axis_context_t *context = motion_stage_axis->context;
CHAN *const axis = config->axis;
BOOL use_negative_limit = FALSE;
BOOL use_positive_limit = FALSE;
int ret;
if (io_get_bit(config->negative_limit_bit) != -1) {
ret = io_initialize(config->negative_limit_bit);
if (ret != 0) {
LOG_ERR("Failed to initialize Motion Stage Axis - Error initializing negative limit: %s", io_error_to_str[ret]);
return;
}
use_negative_limit = TRUE;
}
if (io_get_bit(config->positive_limit_bit) != -1) {
ret = io_initialize(config->positive_limit_bit);
if (ret != 0) {
LOG_ERR("Failed to initialize Motion Stage Axis - Error initializing positive limit: %s", io_error_to_str[ret]);
return;
}
use_positive_limit = TRUE;
}
axis->InputMode = NO_INPUT_MODE;
axis->OutputMode = STEP_DIR_MODE;
axis->Vel = config->max_velocity;
axis->Accel = config->max_acceleration;
axis->Jerk = config->max_jerk;
axis->P = 0;
axis->I = 0;
axis->D = 0;
axis->FFAccel = 0;
axis->FFVel = 0;
axis->MaxI = 200;
axis->MaxErr = 1e+006;
axis->MaxOutput = 200;
axis->DeadBandGain = 1;
axis->DeadBandRange = 0;
axis->InputChan0 = 0;
axis->InputChan1 = 0;
axis->OutputChan0 = config->output_channel;
axis->OutputChan1 = 0;
axis->MasterAxis = -1;
if (use_negative_limit) {
axis->LimitSwitchNegBit = io_get_bit(config->negative_limit_bit);
} else {
axis->LimitSwitchNegBit = 0;
}
if (use_positive_limit) {
axis->LimitSwitchPosBit = io_get_bit(config->positive_limit_bit);
} else {
axis->LimitSwitchPosBit = 0;
}
axis->LimitSwitchOptions = config->limit_switch_options->as_int;
axis->SoftLimitPos = config->soft_limit_pos;
axis->SoftLimitNeg = config->soft_limit_neg;
axis->InputGain0 = 0;
axis->InputGain1 = 0;
axis->InputOffset0 = 0;
axis->InputOffset1 = 0;
axis->OutputGain = 1.0 - (2.0 * config->inverted);
axis->OutputOffset = 0;
axis->SlaveGain = 1;
axis->BacklashMode = BACKLASH_OFF;
axis->BacklashAmount = 0;
axis->BacklashRate = 0;
axis->invDistPerCycle = 1;
axis->Lead = 0;
axis->MaxFollowingError = 1000000000;
axis->StepperAmplitude = 255;
axis->iir[0].B0 = 0;
axis->iir[0].B1 = 0;
axis->iir[0].B2 = 0;
axis->iir[0].A1 = 0;
axis->iir[0].A2 = 0;
axis->iir[1].B0 = 0;
axis->iir[1].B1 = 0;
axis->iir[1].B2 = 0;
axis->iir[1].A1 = 0;
axis->iir[1].A2 = 0;
axis->iir[2].B0 = 0;
axis->iir[2].B1 = 0;
axis->iir[2].B2 = 0;
axis->iir[2].A1 = 0;
axis->iir[2].A2 = 0;
context->enabled = &axis->Enable;
context->position = &axis->Position;
context->destination = &axis->Dest;
context->oversample_idx = 0;
context->home_step = HOME_STEP_IDLE;
context->svc_prev_state = FALSE;
context->svc_state = FALSE;
context->svc_consec_on = 0;
context->svc_consec_off = 0;
context->initialized = TRUE;
}
void motion_stage_initialize(const motion_stage_t *const motion_stage) {
const motion_stage_config_t *config = motion_stage->config;
motion_stage_context_t *context = motion_stage->context;
/* Initialize each axis */
int i;
for (i = 0; i < (int)(sizeof(config->axes) / sizeof(motion_stage_axis_t *)); i++) {
motion_stage_axis_t *axis = config->axes[i];
initialize_motion_stage_axis(axis);
context->p.positions[i] = axis->context->position;
context->d.destinations[i] = axis->context->destination;
}
/* Define coordinate system based on axis channels */
DefineCoordSystem(X_AXIS_CHAN_NUM, Y_AXIS_CHAN_NUM, Z_AXIS_CHAN_NUM, -1);
context->initialized = TRUE;
context->enabled = FALSE;
#if MOTION_STAGE_DEBUG
// Initialize debug statistics on startup
motion_stage_debug_reset_stats();
#endif
}
And here is an excerpt from JSON file used to produce the defines, to give you an idea of the values used:
Code: Select all
{
"Motion Stage": {
"Enabled": true,
"Debug": false,
"X Axis": {
"Enabled": true,
"Inverted": false,
"Output Channel": 13,
"Max Velocity": 9000,
"Max Acceleration": 7000,
"Max Jerk": 7e+6,
"Home Offset": 32.604,
"Counts per mm": 3200.0,
"Limit Switch": {
"Enabled": true,
"Negative Limit": {
"Enabled": true,
"Polarity": "LIMIT_SWITCH_ACTIVE_LOW",
"Bit": 5
},
"Positive Limit": {
"Enabled": false
},
"Action": "LIMIT_SWITCH_STOP_MOVEMENT"
},
"Soft Limit": {
"Positive": 20.620,
"Negative": -32.604
}
},
"Y Axis": {
"Enabled": true,
"Inverted": false,
"Output Channel": 14,
"Max Velocity": 9000,
"Max Acceleration": 7000,
"Max Jerk": 7e+6,
"Home Offset": 26.993,
"Counts per mm": 3200.0,
"Limit Switch": {
"Enabled": true,
"Negative Limit": {
"Enabled": true,
"Polarity": "LIMIT_SWITCH_ACTIVE_LOW",
"Bit": 8
},
"Positive Limit": {
"Enabled": false
},
"Action": "LIMIT_SWITCH_STOP_MOVEMENT"
},
"Soft Limit": {
"Positive": 29.075,
"Negative": -26.993
}
},
"Z Axis": {
"Enabled": true,
"Inverted": true,
"Output Channel": 15,
"Max Velocity": 9000,
"Max Acceleration": 7000,
"Max Jerk": 7e+6,
"Home Offset": 14.5,
"Counts per mm": 3200.0,
"Limit Switch": {
"Enabled": true,
"Negative Limit": {
"Enabled": true,
"Polarity": "LIMIT_SWITCH_ACTIVE_LOW",
"Bit": 13
},
"Positive Limit": {
"Enabled": false
},
"Action": "LIMIT_SWITCH_STOP_MOVEMENT"
},
"Soft Limit": {
"Positive": 39.533,
"Negative": -14.5
}
},
"Home": {
"Velocity": {
"Slow": 25,
"Fast": 1750
}
}
}
}
Please let me know if you'd like more information or insight into the C code - it's spread across many files so I figure excerpts are preferred. I appreciate all your help!
Thanks,
Griffin Boyle