tons of changes, implementing 4D dda
This commit is contained in:
Michael Moon
parent
b184e71107
commit
1e6c74096e
|
|
@ -14,7 +14,7 @@
|
|||
|
||||
PROGRAM = mendel
|
||||
|
||||
SOURCES = $(PROGRAM).c ringbuffer.c serial.c
|
||||
SOURCES = $(PROGRAM).c ringbuffer.c serial.c dda.c gcode.c pinout.c
|
||||
|
||||
##############################################################################
|
||||
# #
|
||||
|
|
|
|||
|
|
@ -1,6 +1,8 @@
|
|||
#ifndef _ARDUINO_H
|
||||
#define _ARDUINO_H
|
||||
|
||||
#include <avr/io.h>
|
||||
|
||||
#define PIN_DIO0 PD0
|
||||
#define RPORT_DIO0 PIND
|
||||
#define WPORT_DIO0 PORTD
|
||||
|
|
|
|||
|
|
@ -0,0 +1,230 @@
|
|||
#include "dda.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
extern struct {
|
||||
volatile int32_t X;
|
||||
volatile int32_t Y;
|
||||
volatile int32_t Z;
|
||||
volatile int32_t E;
|
||||
volatile int32_t F;
|
||||
} current_position;
|
||||
|
||||
// courtesy of http://www.flipcode.com/archives/Fast_Approximate_Distance_Functions.shtml
|
||||
uint32_t approx_distance( int32_t dx, int32_t dy )
|
||||
{
|
||||
uint32_t min, max;
|
||||
|
||||
if ( dx < 0 ) dx = -dx;
|
||||
if ( dy < 0 ) dy = -dy;
|
||||
|
||||
if ( dx < dy )
|
||||
{
|
||||
min = dx;
|
||||
max = dy;
|
||||
} else {
|
||||
min = dy;
|
||||
max = dx;
|
||||
}
|
||||
|
||||
// coefficients equivalent to ( 123/128 * max ) and ( 51/128 * min )
|
||||
return ((( max << 8 ) + ( max << 3 ) - ( max << 4 ) - ( max << 1 ) +
|
||||
( min << 7 ) - ( min << 5 ) + ( min << 3 ) - ( min << 1 )) >> 8 );
|
||||
}
|
||||
|
||||
/*
|
||||
CREATE
|
||||
*/
|
||||
|
||||
void dda_create(GCODE_COMMAND *cmd, DDA *dda) {
|
||||
static TARGET startpoint = { 0, 0, 0, 0, 0 };
|
||||
|
||||
// we start at the previous endpoint
|
||||
memcpy(&dda->currentpoint, &startpoint, sizeof(TARGET));
|
||||
// we end at the passed command's endpoint
|
||||
memcpy(&dda->endpoint, &cmd->target, sizeof(TARGET));
|
||||
|
||||
dda->x_delta = dda->endpoint.X - startpoint.X;
|
||||
dda->y_delta = dda->endpoint.Y - startpoint.Y;
|
||||
// always relative
|
||||
dda->e_delta = dda->endpoint.E;
|
||||
// always absolute
|
||||
dda->f_delta = dda->endpoint.F - startpoint.F;
|
||||
|
||||
|
||||
dda->distance = approx_distance(dda->x_delta, dda->y_delta);
|
||||
|
||||
if (dda->distance < 2)
|
||||
dda->distance = dda->e_delta;
|
||||
if (dda->distance < 2)
|
||||
dda->distance = dda->f_delta;
|
||||
|
||||
dda->total_steps = dda->x_delta;
|
||||
if (dda->y_delta > dda->total_steps)
|
||||
dda->total_steps = dda->y_delta;
|
||||
if (dda->e_delta > dda->total_steps)
|
||||
dda->total_steps = dda->e_delta;
|
||||
if (dda->f_delta > dda->total_steps)
|
||||
dda->total_steps = dda->f_delta;
|
||||
|
||||
if (dda->total_steps == 0)
|
||||
dda->nullmove = 1;
|
||||
|
||||
// MM = sqrt(X^2 + Y^2)
|
||||
// STEPS = max(X * STEPS_PER_MM_X, Y * STEPS_PER_MM_Y)
|
||||
// DURATION = MM / MM_PER_MIN * 60 SEC_PER_MIN * 1000000 US_PER_SEC
|
||||
// US/STEP = DURATION / STEPS
|
||||
// intF = sqrt(X^2 + Y^2) / max(X * STEPS_PER_MM_X, Y * STEPS_PER_MM_Y)
|
||||
|
||||
// dda->endpoint.F = distance / total_steps;
|
||||
|
||||
if (dda->f_delta > dda->total_steps) {
|
||||
// TODO: rescale F
|
||||
dda->f_scale = dda->f_delta / dda->total_steps;
|
||||
if (dda->f_scale > 3) {
|
||||
dda->f_delta /= dda->f_scale;
|
||||
}
|
||||
else {
|
||||
dda->f_scale = 1;
|
||||
dda->total_steps = dda->f_delta;
|
||||
}
|
||||
}
|
||||
|
||||
dda->x_direction = (dda->endpoint.X > startpoint.X)?1:0;
|
||||
dda->y_direction = (dda->endpoint.Y > startpoint.Y)?1:0;
|
||||
dda->e_direction = (dda->endpoint.E > startpoint.E)?1:0;
|
||||
dda->f_direction = (dda->endpoint.F > startpoint.F)?1:0;
|
||||
|
||||
dda->x_counter = dda->y_counter = dda->e_counter = dda->f_counter
|
||||
= -(dda->total_steps >> 1);
|
||||
|
||||
// next dda starts where we finish
|
||||
memcpy(&startpoint, &dda->endpoint, sizeof(TARGET));
|
||||
}
|
||||
|
||||
/*
|
||||
START
|
||||
*/
|
||||
|
||||
void dda_start(DDA *dda) {
|
||||
x_direction(dda->x_direction);
|
||||
y_direction(dda->y_direction);
|
||||
z_direction(dda->z_direction);
|
||||
e_direction(dda->e_direction);
|
||||
}
|
||||
|
||||
/*
|
||||
CAN STEP
|
||||
*/
|
||||
|
||||
uint8_t can_step(uint8_t min, uint8_t max, int32_t current, int32_t target, uint8_t dir) {
|
||||
if (target == current)
|
||||
return 0;
|
||||
|
||||
if (min && !dir)
|
||||
return 0;
|
||||
|
||||
if (max && dir)
|
||||
return 0;
|
||||
|
||||
return 255;
|
||||
}
|
||||
|
||||
/*
|
||||
STEP
|
||||
*/
|
||||
|
||||
void dda_step(DDA *dda) {
|
||||
uint8_t step_option = 0;
|
||||
#define X_CAN_STEP 1
|
||||
#define Y_CAN_STEP 2
|
||||
#define Z_CAN_STEP 4
|
||||
#define E_CAN_STEP 8
|
||||
#define F_CAN_STEP 16
|
||||
#define REAL_MOVE 32
|
||||
|
||||
do {
|
||||
step_option |= can_step(x_min(), x_max(), current_position.X, dda->endpoint.X, dda->x_direction) & X_CAN_STEP;
|
||||
step_option |= can_step(y_min(), y_max(), current_position.Y, dda->endpoint.Y, dda->y_direction) & Y_CAN_STEP;
|
||||
step_option |= can_step(z_min(), z_max(), current_position.Z, dda->endpoint.Z, dda->z_direction) & Z_CAN_STEP;
|
||||
step_option |= can_step(-1, -1, current_position.E, dda->endpoint.E, dda->e_direction) & E_CAN_STEP;
|
||||
step_option |= can_step(-1, -1, current_position.F, dda->endpoint.F, dda->f_direction) & F_CAN_STEP;
|
||||
|
||||
if (step_option & X_CAN_STEP) {
|
||||
dda->x_counter += dda->x_delta;
|
||||
if (dda->x_counter > 0) {
|
||||
step_option |= REAL_MOVE;
|
||||
|
||||
// do X step
|
||||
dda->x_counter -= dda->total_steps;
|
||||
|
||||
if (dda->x_direction)
|
||||
current_position.X++;
|
||||
else
|
||||
current_position.X--;
|
||||
}
|
||||
}
|
||||
|
||||
if (step_option & Y_CAN_STEP) {
|
||||
dda->y_counter += dda->y_delta;
|
||||
if (dda->y_counter > 0) {
|
||||
step_option |= REAL_MOVE;
|
||||
|
||||
// do Y step
|
||||
dda->y_counter -= dda->total_steps;
|
||||
|
||||
if (dda->y_direction)
|
||||
current_position.Y++;
|
||||
else
|
||||
current_position.Y--;
|
||||
}
|
||||
}
|
||||
|
||||
if (step_option & Z_CAN_STEP) {
|
||||
dda->z_counter += dda->z_delta;
|
||||
if (dda->z_counter > 0) {
|
||||
step_option |= REAL_MOVE;
|
||||
|
||||
// do Z step
|
||||
dda->z_counter -= dda->total_steps;
|
||||
|
||||
if (dda->z_direction)
|
||||
current_position.Z++;
|
||||
else
|
||||
current_position.Z--;
|
||||
}
|
||||
}
|
||||
|
||||
if (step_option & E_CAN_STEP) {
|
||||
dda->e_counter += dda->e_delta;
|
||||
if (dda->e_counter > 0) {
|
||||
step_option |= REAL_MOVE;
|
||||
|
||||
// do E step
|
||||
dda->e_counter -= dda->total_steps;
|
||||
|
||||
if (dda->e_direction)
|
||||
current_position.E++;
|
||||
else
|
||||
current_position.E--;
|
||||
}
|
||||
}
|
||||
|
||||
if (step_option & F_CAN_STEP) {
|
||||
dda->f_counter += dda->f_delta;
|
||||
if (dda->f_counter > 0) {
|
||||
// do F step
|
||||
dda->f_counter -= dda->total_steps;
|
||||
|
||||
if (dda->f_direction)
|
||||
current_position.F += dda->f_scale;
|
||||
else
|
||||
current_position.F -= dda->f_scale;
|
||||
}
|
||||
}
|
||||
} while (((step_option & REAL_MOVE) == 0) && (step_option & F_CAN_STEP));
|
||||
|
||||
if (step_option & REAL_MOVE) {
|
||||
setTimer(dda->distance * (60000000 / current_position.F / dda->total_steps));
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
#ifndef _DDA_H
|
||||
#define _DDA_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "target.h"
|
||||
#include "pinout.h"
|
||||
#include "gcode.h"
|
||||
|
||||
typedef struct {
|
||||
TARGET currentpoint;
|
||||
TARGET endpoint;
|
||||
|
||||
uint8_t steep :1;
|
||||
uint8_t swap :1;
|
||||
uint8_t x_direction :1;
|
||||
uint8_t y_direction :1;
|
||||
uint8_t z_direction :1;
|
||||
uint8_t e_direction :1;
|
||||
uint8_t f_direction :1;
|
||||
uint8_t nullmove :1;
|
||||
|
||||
int8_t x_delta;
|
||||
int8_t y_delta;
|
||||
int8_t z_delta;
|
||||
int8_t e_delta;
|
||||
int8_t f_delta;
|
||||
|
||||
int32_t x_counter;
|
||||
int32_t y_counter;
|
||||
int32_t z_counter;
|
||||
int32_t e_counter;
|
||||
int32_t f_counter;
|
||||
|
||||
uint32_t total_steps;
|
||||
|
||||
uint16_t f_scale;
|
||||
uint32_t distance;
|
||||
} DDA;
|
||||
|
||||
uint32_t approx_distance( int32_t dx, int32_t dy );
|
||||
|
||||
void dda_create(GCODE_COMMAND *cmd, DDA *dda);
|
||||
void dda_start(DDA *dda);
|
||||
void dda_step(DDA *dda);
|
||||
|
||||
#endif /* _DDA_H */
|
||||
|
|
@ -0,0 +1,194 @@
|
|||
#include "gcode.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#include "machine.h"
|
||||
#include "dda.h"
|
||||
|
||||
extern uint8_t mb_head;
|
||||
extern uint8_t mb_tail;
|
||||
extern DDA movebuffer[16];
|
||||
extern uint8_t option_bitfield;
|
||||
|
||||
#define PI 3.1415926535
|
||||
|
||||
/*
|
||||
utility functions
|
||||
*/
|
||||
|
||||
uint8_t indexof(uint8_t c, char *string) {
|
||||
uint8_t i;
|
||||
for (i = 0;string[i];i++) {
|
||||
if (c == string[i])
|
||||
return i;
|
||||
}
|
||||
return 255;
|
||||
}
|
||||
|
||||
float manexp_to_float(uint32_t mantissa, uint8_t exp) {
|
||||
float v = mantissa;
|
||||
|
||||
if (exp == 2)
|
||||
v /= 10;
|
||||
else if (exp == 3)
|
||||
v /= 100;
|
||||
else if (exp == 4)
|
||||
v /= 1000;
|
||||
else if (exp == 5)
|
||||
v /= 10000;
|
||||
else if (exp == 6)
|
||||
v /= 100000;
|
||||
|
||||
return v;
|
||||
}
|
||||
|
||||
/*
|
||||
public functions
|
||||
*/
|
||||
|
||||
void scan_char(uint8_t c) {
|
||||
static uint8_t last_field = 0;
|
||||
static uint32_t mantissa = 0;
|
||||
static uint8_t exp = 0;
|
||||
static GCODE_COMMAND next_target = { 0, 0, 0, 0, { 0, 0, 0, 0, 0 } };
|
||||
|
||||
// uppercase
|
||||
if (c >= 'a' && c <= 'z')
|
||||
c &= ~32;
|
||||
|
||||
// process field
|
||||
if (indexof(c, "GMXYZEF\n") != 255) {
|
||||
if (last_field) {
|
||||
switch (last_field) {
|
||||
case 'G':
|
||||
next_target.G = mantissa;
|
||||
break;
|
||||
case 'M':
|
||||
next_target.M = mantissa;
|
||||
break;
|
||||
case 'X':
|
||||
next_target.target.X = manexp_to_float(mantissa, exp) * STEPS_PER_MM_X;
|
||||
break;
|
||||
case 'Y':
|
||||
next_target.target.Y = manexp_to_float(mantissa, exp) * STEPS_PER_MM_Y;
|
||||
break;
|
||||
case 'Z':
|
||||
next_target.target.Z = manexp_to_float(mantissa, exp) * STEPS_PER_MM_Z;
|
||||
break;
|
||||
case 'E':
|
||||
next_target.target.E = manexp_to_float(mantissa, exp) * STEPS_PER_MM_E;
|
||||
break;
|
||||
case 'F':
|
||||
// TODO: calculate end speed in microseconds per step from millimeters per minute
|
||||
// MM = sqrt(X^2 + Y^2)
|
||||
// STEPS = max(X * STEPS_PER_MM_X, Y * STEPS_PER_MM_Y)
|
||||
// DURATION = MM / MM_PER_MIN * 60 SEC_PER_MIN * 1000000 US_PER_SEC
|
||||
// US/STEP = DURATION / STEPS
|
||||
// intF = sqrt(X^2 + Y^2) / max(X * STEPS_PER_MM_X, Y * STEPS_PER_MM_Y)
|
||||
next_target.target.F = manexp_to_float(mantissa, exp) * STEPS_PER_MM_F;
|
||||
break;
|
||||
}
|
||||
mantissa = 0;
|
||||
exp = 0;
|
||||
}
|
||||
last_field = c;
|
||||
switch (c) {
|
||||
case 'G':
|
||||
next_target.seen |= SEEN_G;
|
||||
break;
|
||||
case 'M':
|
||||
next_target.seen |= SEEN_M;
|
||||
break;
|
||||
case 'X':
|
||||
next_target.seen |= SEEN_X;
|
||||
break;
|
||||
case 'Y':
|
||||
next_target.seen |= SEEN_Y;
|
||||
break;
|
||||
case 'Z':
|
||||
next_target.seen |= SEEN_Z;
|
||||
break;
|
||||
case 'E':
|
||||
next_target.seen |= SEEN_E;
|
||||
break;
|
||||
case 'F':
|
||||
next_target.seen |= SEEN_F;
|
||||
break;
|
||||
case '\n':
|
||||
// process
|
||||
process_gcode_command(&next_target);
|
||||
|
||||
// save options
|
||||
option_bitfield = next_target.option;
|
||||
|
||||
// reset variables
|
||||
last_field = 0;
|
||||
memset(&next_target, 0, sizeof(GCODE_COMMAND));
|
||||
next_target.option = option_bitfield;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// process digits
|
||||
else if (c == '-')
|
||||
exp |= 0x80;
|
||||
else if ((c == '.') && ((exp & 0x7F) == 0))
|
||||
exp |= 1;
|
||||
else if (c >= '0' && c <= '9') {
|
||||
mantissa = (mantissa * 10) + (c - '0');
|
||||
if (exp & 0x7F)
|
||||
exp++;
|
||||
}
|
||||
}
|
||||
|
||||
void process_gcode_command(GCODE_COMMAND *gcmd) {
|
||||
uint8_t do_move;
|
||||
if (gcmd->seen & SEEN_G) {
|
||||
switch (gcmd->G) {
|
||||
// G0 - rapid, unsynchronised motion
|
||||
case 0:
|
||||
gcmd->option &= ~OPTION_SYNCHRONISE;
|
||||
do_move = 1;
|
||||
break;
|
||||
// G30 - go home via point
|
||||
case 30:
|
||||
gcmd->option |= OPTION_HOME_WHEN_COMPLETE;
|
||||
// G1 - synchronised motion
|
||||
case 1:
|
||||
gcmd->option |= OPTION_SYNCHRONISE;
|
||||
do_move = 1;
|
||||
break;
|
||||
// G2 - Arc Clockwise
|
||||
// G3 - Arc Counter-clockwise
|
||||
// G4 - Dwell
|
||||
// G20 - inches as units
|
||||
// G21 - mm as units
|
||||
// G28 - go home
|
||||
case 28:
|
||||
gcmd->option &= ~OPTION_SYNCHRONISE;
|
||||
do_move = 1;
|
||||
break;
|
||||
// G90 - absolute positioning
|
||||
case 90:
|
||||
gcmd->option &= ~OPTION_RELATIVE;
|
||||
break;
|
||||
// G91 - relative positioning
|
||||
case 91:
|
||||
gcmd->option |= OPTION_RELATIVE;
|
||||
break;
|
||||
// G92 - set home
|
||||
case 92:
|
||||
break;
|
||||
// TODO: spit an error
|
||||
}
|
||||
}
|
||||
if (gcmd->seen & SEEN_M) {
|
||||
switch (gcmd->M) {
|
||||
// TODO: spit an error
|
||||
}
|
||||
}
|
||||
|
||||
if (do_move) {
|
||||
dda_create(gcmd, movebuffer);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
#ifndef _GCODE_H
|
||||
#define _GCODE_H
|
||||
|
||||
#include "target.h"
|
||||
|
||||
typedef struct {
|
||||
uint16_t seen;
|
||||
#define SEEN_G 1
|
||||
#define SEEN_M 2
|
||||
#define SEEN_X 4
|
||||
#define SEEN_Y 8
|
||||
#define SEEN_Z 16
|
||||
#define SEEN_E 32
|
||||
#define SEEN_F 64
|
||||
|
||||
uint8_t option;
|
||||
#define OPTION_RELATIVE 1
|
||||
#define OPTION_SYNCHRONISE 2
|
||||
#define OPTION_HOME_WHEN_COMPLETE 4
|
||||
#define OPTION_UNIT_INCHES 8
|
||||
|
||||
uint8_t G;
|
||||
uint8_t M;
|
||||
TARGET target;
|
||||
} GCODE_COMMAND;
|
||||
|
||||
void scan_char(uint8_t c);
|
||||
void process_gcode_command(GCODE_COMMAND *gcmd);
|
||||
|
||||
#endif /* _GCODE_H */
|
||||
|
|
@ -0,0 +1,45 @@
|
|||
#ifndef _MACHINE_H
|
||||
#define _MACHINE_H
|
||||
|
||||
/*
|
||||
machine variables
|
||||
*/
|
||||
#define AXIS_COUNT 5
|
||||
#define AXIS_HOMING_COUNT 3
|
||||
|
||||
/*
|
||||
axis calculations, adjust as necessary
|
||||
*/
|
||||
|
||||
#define XY_STEPS_PER_REV 3200
|
||||
#define XY_COG_CIRCUMFERENCE (4.77 * 16)
|
||||
|
||||
#define EXTRUDER_STEPS_PER_REV 3200
|
||||
#define EXTRUDER_SHAFT_RADIUS 5
|
||||
#define EXTRUDER_INLET_DIAMETER 3
|
||||
#define EXTRUDER_NOZZLE_DIAMETER 0.8
|
||||
|
||||
|
||||
#define STEPS_PER_MM_X (XY_STEPS_PER_REV * XY_COG_CIRCUMFERENCE)
|
||||
#define STEPS_PER_MM_Y (XY_STEPS_PER_REV * XY_COG_CIRCUMFERENCE)
|
||||
#define STEPS_PER_MM_Z (3200)
|
||||
#define STEPS_PER_MM_E (EXTRUDER_STEPS_PER_REV * EXTRUDER_SHAFT_RADIUS * PI * EXTRUDER_INLET_DIAMETER / EXTRUDER_NOZZLE_DIAMETER)
|
||||
|
||||
#define STEPS_PER_MM_PER_S_F (3200)
|
||||
|
||||
#define STEPS_PER_MM_F STEPS_PER_MM_PER_S_F
|
||||
|
||||
/*
|
||||
F is sent in units of millimeters per second
|
||||
and implemented as microseconds per step
|
||||
|
||||
MM/S * steps/mm * 1000000us/s = steps per microsecond
|
||||
*/
|
||||
|
||||
|
||||
#define FAST_MM_PER_SEC 40
|
||||
#define SLOW_MM_PER_SEC 20
|
||||
#define ACCEL 10
|
||||
|
||||
|
||||
#endif /* _MACHINE_H */
|
||||
206
mendel/mendel.c
206
mendel/mendel.c
|
|
@ -1,80 +1,30 @@
|
|||
#include <stddef.h>
|
||||
// #include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <avr/io.h>
|
||||
#include <avr/interrupt.h>
|
||||
|
||||
#include "serial.h"
|
||||
|
||||
/*
|
||||
machine variables
|
||||
*/
|
||||
#define AXIS_COUNT 5
|
||||
#define AXIS_HOMING_COUNT 3
|
||||
#include "dda.h"
|
||||
#include "gcode.h"
|
||||
|
||||
// steps per rev * mm per tooth * teeth per rev
|
||||
#define STEPS_PER_MM (3200 * 4.77 * 16)
|
||||
#define STEPS_PER_IN (STEPS_PER_MM * 25.4)
|
||||
#include "machine.h"
|
||||
|
||||
#define MAX_MM_PER_SEC 40
|
||||
#define MAX_IMMED_MM_PER_SEC 20
|
||||
#define ACCEL 10
|
||||
|
||||
/*
|
||||
state machine variables
|
||||
*/
|
||||
|
||||
uint8_t linebuffer[64];
|
||||
|
||||
uint8_t state = 0;
|
||||
#define STATE_WAIT_FOR_COMMAND 0
|
||||
#define STATE_WAIT_FOR_COMMAND_DIGIT 1
|
||||
#define STATE_WAIT_FOR_DATA 2
|
||||
#define STATE_WAIT_FOR_DATA_DIGIT 3
|
||||
uint8_t command;
|
||||
uint8_t command_digit;
|
||||
uint8_t axis;
|
||||
uint8_t mb_head = 0;
|
||||
uint8_t mb_tail = 0;
|
||||
DDA movebuffer[16];
|
||||
|
||||
uint8_t option_bitfield;
|
||||
#define OPTION_RELATIVE 1
|
||||
#define OPTION_SYNCHRONISE 2
|
||||
#define OPTION_HOME_WHEN_COMPLETE 4
|
||||
#define OPTION_UNIT_INCHES 8
|
||||
|
||||
// volatile uint32_t xpos = 0;
|
||||
// volatile uint32_t ypos = 0;
|
||||
// volatile uint32_t zpos = 0;
|
||||
// volatile uint32_t edelta = 0;
|
||||
//
|
||||
// uint32_t xtarget = 0;
|
||||
// uint32_t ytarget = 0;
|
||||
// uint32_t ztarget = 0;
|
||||
//
|
||||
// uint16_t xspeed = 0;
|
||||
// uint16_t yspeed = 0;
|
||||
// uint16_t zspeed = 0;
|
||||
|
||||
struct axis {
|
||||
volatile uint32_t pos;
|
||||
int32_t target;
|
||||
uint32_t newtarget_mantissa;
|
||||
uint8_t newtarget_opt;
|
||||
#define newtarget_opt_sign 0x80
|
||||
#define newtarget_opt_set 0x40
|
||||
#define newtarget_opt_exp 0x3F
|
||||
uint16_t speed;
|
||||
} axes[AXIS_COUNT];
|
||||
|
||||
uint8_t axis_char_to_id(uint8_t c) {
|
||||
if (c >= 'X' && c <= 'Z')
|
||||
return c - 'X';
|
||||
if (c == 'E')
|
||||
return 3;
|
||||
if (c == 'F')
|
||||
return 4;
|
||||
return 255;
|
||||
}
|
||||
struct {
|
||||
volatile int32_t X;
|
||||
volatile int32_t Y;
|
||||
volatile int32_t Z;
|
||||
volatile int32_t E;
|
||||
volatile int32_t F;
|
||||
} current_position = { 0, 0, 0, 0, 0 };
|
||||
|
||||
int main (void)
|
||||
{
|
||||
|
|
@ -88,131 +38,7 @@ int main (void)
|
|||
{
|
||||
for (;serial_rxchars() == 0;);
|
||||
uint8_t c = serial_popchar();
|
||||
if (c >= 'a' && c <= 'z')
|
||||
c &= ~32;
|
||||
// preprocess
|
||||
switch (state) {
|
||||
case STATE_WAIT_FOR_COMMAND_DIGIT:
|
||||
if (c == 'G' || c == 'M')
|
||||
state = STATE_WAIT_FOR_COMMAND;
|
||||
break;
|
||||
case STATE_WAIT_FOR_DATA_DIGIT:
|
||||
if (axis_char_to_id(c) < 255)
|
||||
state = STATE_WAIT_FOR_DATA;
|
||||
break;
|
||||
}
|
||||
// actuate
|
||||
switch (state) {
|
||||
case STATE_WAIT_FOR_COMMAND:
|
||||
if (c == 'G' || c == 'M') {
|
||||
command = c;
|
||||
command_digit = 0;
|
||||
state = STATE_WAIT_FOR_COMMAND_DIGIT;
|
||||
}
|
||||
break;
|
||||
case STATE_WAIT_FOR_COMMAND_DIGIT:
|
||||
if (c >= '0' && c <= '9') {
|
||||
command_digit = (command_digit * 10) + (c - '0');
|
||||
}
|
||||
else {
|
||||
state = STATE_WAIT_FOR_DATA;
|
||||
}
|
||||
break;
|
||||
case STATE_WAIT_FOR_DATA:
|
||||
if (axis_char_to_id(c) < 255) {
|
||||
axis = axis_char_to_id(c);
|
||||
axes[axis].newtarget_mantissa = 0;
|
||||
axes[axis].newtarget_opt = 0;
|
||||
state = STATE_WAIT_FOR_DATA_DIGIT;
|
||||
}
|
||||
break;
|
||||
case STATE_WAIT_FOR_DATA_DIGIT:
|
||||
if (c == '-') {
|
||||
axes[axis].newtarget_opt |= newtarget_opt_sign;
|
||||
}
|
||||
if (c >= '0' && c <= '9') {
|
||||
axes[axis].newtarget_mantissa = (axes[axis].newtarget_mantissa * 10) + (c - '0');
|
||||
if (axes[axis].newtarget_opt & newtarget_opt_exp)
|
||||
axes[axis].newtarget_opt++;
|
||||
}
|
||||
if (c == '.') {
|
||||
axes[axis].newtarget_opt |= 1;
|
||||
}
|
||||
break;
|
||||
}
|
||||
if (c == 13) {
|
||||
if (command == 'G') {
|
||||
uint8_t i;
|
||||
switch (command_digit) {
|
||||
// G30 - go home via point
|
||||
case 30:
|
||||
option_bitfield |= OPTION_HOME_WHEN_COMPLETE;
|
||||
// G0 - rapid, unsynchronised motion
|
||||
case 0:
|
||||
option_bitfield &= ~OPTION_SYNCHRONISE;
|
||||
break;
|
||||
// G1 - synchronised motion
|
||||
case 1:
|
||||
option_bitfield |= OPTION_SYNCHRONISE;
|
||||
break;
|
||||
// G2 - Arc Clockwise
|
||||
// G3 - Arc Counter-clockwise
|
||||
// G4 - Dwell
|
||||
// G20 - inches as units
|
||||
// G21 - mm as units
|
||||
// G28 - go home
|
||||
case 28:
|
||||
for (i = 0; i < AXIS_HOMING_COUNT; i++) {
|
||||
option_bitfield &= ~OPTION_SYNCHRONISE;
|
||||
axes[i].target = 0;
|
||||
}
|
||||
break;
|
||||
// G90 - absolute positioning
|
||||
case 90:
|
||||
option_bitfield &= ~OPTION_RELATIVE;
|
||||
break;
|
||||
// G91 - relative positioning
|
||||
case 91:
|
||||
option_bitfield |= OPTION_RELATIVE;
|
||||
break;
|
||||
// G92 - set home
|
||||
case 92:
|
||||
for (i = 0; i < AXIS_HOMING_COUNT; i++) {
|
||||
axes[i].pos = axes[i].target = 0;
|
||||
}
|
||||
break;
|
||||
// TODO: spit an error
|
||||
}
|
||||
}
|
||||
else if (command == 'M') {
|
||||
switch (command_digit) {
|
||||
// TODO: spit an error
|
||||
}
|
||||
}
|
||||
// update axes;
|
||||
uint8_t i;
|
||||
for (i = 0; i < AXIS_COUNT; i++) {
|
||||
if (axes[i].newtarget_opt & newtarget_opt_set) {
|
||||
float n = axes[i].newtarget_mantissa;
|
||||
uint8_t exp = axes[i].newtarget_opt & newtarget_opt_exp;
|
||||
if (axes[i].newtarget_opt & newtarget_opt_sign)
|
||||
n = -n;
|
||||
if (exp == 1)
|
||||
n /= 10;
|
||||
if (exp == 2)
|
||||
n /= 100;
|
||||
if (exp == 3)
|
||||
n /= 1000;
|
||||
if (exp == 4)
|
||||
n /= 10000;
|
||||
if (exp == 5)
|
||||
n /= 100000;
|
||||
if (option_bitfield & OPTION_UNIT_INCHES)
|
||||
axes[i].target = n * STEPS_PER_IN;
|
||||
else
|
||||
axes[i].target = n * STEPS_PER_MM;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
scan_char(c);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,48 @@
|
|||
#include "pinout.h"
|
||||
|
||||
void delayMicrosecondsInterruptible(uint16_t us)
|
||||
{
|
||||
// for a one-microsecond delay, simply return. the overhead
|
||||
// of the function call yields a delay of approximately 1 1/8 us.
|
||||
if (--us == 0)
|
||||
return;
|
||||
|
||||
// the following loop takes a quarter of a microsecond (4 cycles)
|
||||
// per iteration, so execute it four times for each microsecond of
|
||||
// delay requested.
|
||||
us <<= 2;
|
||||
|
||||
// account for the time taken in the preceeding commands.
|
||||
us -= 2;
|
||||
|
||||
// busy wait
|
||||
__asm__ __volatile__ ("1: sbiw %0,1" "\n\t" // 2 cycles
|
||||
"brne 1b" :
|
||||
"=w" (us) :
|
||||
"0" (us) // 2 cycles
|
||||
);
|
||||
}
|
||||
|
||||
void x_step() {
|
||||
WRITE(DIO0, 1);
|
||||
delayMicrosecondsInterruptible(5);
|
||||
WRITE(DIO0, 0);
|
||||
}
|
||||
|
||||
void y_step() {
|
||||
WRITE(DIO4, 1);
|
||||
delayMicrosecondsInterruptible(5);
|
||||
WRITE(DIO4, 0);
|
||||
}
|
||||
|
||||
void z_step() {
|
||||
WRITE(DIO8, 1);
|
||||
delayMicrosecondsInterruptible(5);
|
||||
WRITE(DIO8, 0);
|
||||
}
|
||||
|
||||
void e_step() {
|
||||
WRITE(DIO12, 1);
|
||||
delayMicrosecondsInterruptible(5);
|
||||
WRITE(DIO12, 0);
|
||||
}
|
||||
|
|
@ -0,0 +1,80 @@
|
|||
#ifndef _PINOUT_H
|
||||
#define _PINOUT_H
|
||||
|
||||
#include "arduino.h"
|
||||
|
||||
#ifndef MASK
|
||||
#define MASK(PIN) (1 << PIN)
|
||||
#endif
|
||||
|
||||
#define _PIN(P) #P
|
||||
|
||||
#define READ(IO) (RPORT_ ## IO & MASK(PIN_ ## IO))
|
||||
#define WRITE(IO, v) if (v) { WPORT_ ## IO |= MASK(PIN_ ## IO); } else { WPORT_ ## IO &= ~MASK(PIN_ ## IO); }
|
||||
#define SET_INPUT(IO) (DDR_ ## IO |= MASK(PIN_ ## IO))
|
||||
#define SET_OUTPUT(IO) (DDR_ ## IO &= ~MASK(PIN ## IO))
|
||||
|
||||
// #define X_STEP_PIN DIO0
|
||||
// #define X_DIR_PIN DIO1
|
||||
// #define X_MIN_MIN DIO2
|
||||
// #define X_MAX_PIN DIO3
|
||||
|
||||
// #define Y_STEP_PIN DIO4
|
||||
// #define Y_DIR_PIN DIO5
|
||||
// #define Y_MIN_MIN DIO6
|
||||
// #define Y_MAX_PIN DIO7
|
||||
|
||||
// #define Z_STEP_PIN DIO8
|
||||
// #define Z_DIR_PIN DIO9
|
||||
// #define Z_MIN_MIN DIO10
|
||||
// #define Z_MAX_PIN DIO11
|
||||
|
||||
// #define E_STEP_PIN DIO12
|
||||
// #define E_DIR_PIN DIO13
|
||||
|
||||
void x_step(void);
|
||||
void y_step(void);
|
||||
void z_step(void);
|
||||
void e_step(void);
|
||||
|
||||
inline void x_direction(uint8_t dir) {
|
||||
WRITE(DIO1, dir);
|
||||
}
|
||||
|
||||
inline uint8_t x_min(void) {
|
||||
return READ(DIO2);
|
||||
}
|
||||
|
||||
inline uint8_t x_max(void) {
|
||||
return READ(DIO3);
|
||||
}
|
||||
|
||||
inline void y_direction(uint8_t dir) {
|
||||
WRITE(DIO5, dir);
|
||||
}
|
||||
|
||||
inline uint8_t y_min(void) {
|
||||
return READ(DIO6);
|
||||
}
|
||||
|
||||
inline uint8_t y_max(void) {
|
||||
return READ(DIO7);
|
||||
}
|
||||
|
||||
inline void z_direction(uint8_t dir) {
|
||||
WRITE(DIO9, dir);
|
||||
}
|
||||
|
||||
inline uint8_t z_min(void) {
|
||||
return READ(DIO10);
|
||||
}
|
||||
|
||||
inline uint8_t z_max(void) {
|
||||
return READ(DIO11);
|
||||
}
|
||||
|
||||
inline void e_direction(uint8_t dir) {
|
||||
WRITE(DIO13, dir);
|
||||
}
|
||||
|
||||
#endif /* _PINOUT_H */
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
#ifndef _TARGET_H
|
||||
#define _TARGET_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
typedef struct {
|
||||
int32_t X;
|
||||
int32_t Y;
|
||||
int32_t Z;
|
||||
uint32_t E;
|
||||
uint32_t F;
|
||||
} TARGET;
|
||||
|
||||
#endif /* _TARGET_H */
|
||||
Loading…
Reference in New Issue