A Question for Nophead September 27, 2009 05:18AM |
Registered: 16 years ago Posts: 467 |
Re: A Question for Nophead September 27, 2009 06:50AM |
Admin Registered: 17 years ago Posts: 7,879 |
def set_ramp(self, steps, run_delay): table = pack('>h',steps) #start with the number of entries k = 4 * run_delay * run_delay * steps last = 0 for i in range(steps): t = sqrt((i + 1) * k) table = table + pack('>h', int(round(t - last))) last = t return table
if(steps < ramp_length) { // near end of line ? del = ramp[steps]; if(del < run_delay) del = run_delay; } else if(stepped < ramp_length) { // near start of line ? del = ramp[stepped]; if(del < run_delay) del = run_delay; } else del = run_delay; // middle of line, use the specified speed
Re: A Question for Nophead September 27, 2009 10:29AM |
Registered: 16 years ago Posts: 169 |
Re: A Question for Nophead September 27, 2009 11:04AM |
Admin Registered: 17 years ago Posts: 7,879 |
word do_motion(void); void init_motors(void); void step_x(int steps,int del); void step_y(int steps,int del); void step_z(int steps,dword del); void goto_xyz(word x, word y, word z, word del); void goto_xy(word x, word y, word del); void goto_z(word z, word del); typedef struct cartesian_state { word x,y,z,steps; } cartesian_state_ty; void get_cartesian_state(cartesian_state_ty *state); void set_ramp(word length, const word *table);
#include "io.h" #include "cartesian.h" #include "timer.h" #include #define ORIGIN_Z 400 #define ORIGIN_X (20139/2 - 125) #define ORIGIN_Y (24183/2 - 148) #define MOVE_DELAY 160 #define ZMOVE_DELAY 1250 #define SEEK_DELAY 30000 #define ZSEEK_DELAY 330000 #define HYSTERESIS 130 #define ZHYSTERESIS 7 #define MAX_RAMP 256 typedef struct axis { sword pos; // Current position sword min; // Limits sword max; sword steps; // Number of steps to do this line sword dir; // Direction of each step +/-1 sword acc; // Brezenham accumulator void (*set_dir)(bool); // Virtual function to set direction output (X,Y only) void (*start_step)(void); // Leading edge of step pulse void (*end_step)(void); // Trailing edge of step pulse bool (*mlimit)(void); // Read the negative limit } axis_ty; static axis_ty X,Y,Z; static word total_steps; static volatile word steps,stepped; static word ramp[MAX_RAMP]; sbyte x_error[MAX_RAMP], y_error[MAX_RAMP]; static word run_delay; static word ramp_length; // // Return the state of the system // void get_cartesian_state(cartesian_state_ty *state) { state->x = X.pos; state->y = Y.pos; state->z = Z.pos; state->steps = steps; } // // Initialise an axis object // static void init_axis( axis_ty *axis, sword min, sword max, void (*set_dir)(bool), void (*start_step)(void), void (*end_step)(void), bool (*mlimit)(void) ) { axis->min = min; axis->max = max; axis->set_dir = set_dir; axis->start_step = start_step; axis->end_step = end_step; axis->mlimit = mlimit; } // // Set the direction outputs // static void set_x_dir(bool forwards) { XDIR = forwards ? 0 : 1; } static void set_y_dir(bool forwards) { YDIR = forwards ? 0 : 1; } static void set_z_dir(bool forwards) { (void)forwards; } // // Do steps // static void start_x_step(void) { XSTEP = 1; } static void end_x_step(void) { XSTEP = 0; } static void start_y_step(void) { YSTEP = 1; } static void end_y_step(void) { YSTEP = 0; } static const byte zmotor_table[8] = { 0xC0, //0xC0, 0x40, //0x40, 0x50, //0x60, 0x10, //0x20, 0x30, //0x30, 0x20, //0x10, 0xA0, //0x90, 0x80, //0x80, }; static void start_z_step(void) { PTS = (PTS & 0x0F) | zmotor_table[Z.pos & 7]; } static void end_z_step(void) { } // // Read limit switches // static bool x_mlimit(void) { return !XMLIMIT; } static bool y_mlimit(void) { return !YMLIMIT; } static bool z_mlimit(void) { return !ZMLIMIT; } // // Do a step or not as the case may be // static void bresenham_step(axis_ty *axis) { axis->acc -= axis->steps; if(axis->acc < 0) { axis->acc += total_steps; axis->pos += axis->dir; axis->start_step(); } } // // Called from timer interrupt handler // word do_motion(void){ static bool start = TRUE; static word del; if(steps) { if(Z.dir >= 0 && (!TOOL_Z0 || !TOOL_Z1)) { // Stop if hit tool sensor and moving down start = TRUE; // Abort this run steps = 0; return 20; } if(start) { bresenham_step(&X); bresenham_step(&Y); bresenham_step(&Z); start = FALSE; if(steps < ramp_length) { del = ramp[steps]; if(del < run_delay) del = run_delay; } else if(stepped < ramp_length) { del = ramp[stepped]; if(del < run_delay) del = run_delay; } else del = run_delay; } else { X.end_step(); // Do trailing edge of step pulse Y.end_step(); if(stepped < MAX_RAMP) { x_error[stepped] = x_shaft - X.pos; y_error[stepped] = y_shaft - Y.pos; } ++stepped; --steps; start = TRUE; } return del; } return 20; } // // Set the steps and direction on specified axis // static void set_steps(axis_ty *axis, sword steps) { if(steps < 0) { axis->steps = -steps; axis->dir = -1; axis->set_dir(FALSE); } else { axis->steps = steps; axis->dir = 1; axis->set_dir(TRUE); } axis->acc = 0; } // // Go to specified position in a straight line // void goto_xyz(word x, word y, word z, word del) { while(steps) ; run_delay = del; set_steps(&X, x - X.pos); set_steps(&Y, y - Y.pos); set_steps(&Z, z - Z.pos); total_steps = X.steps; if(total_steps < Y.steps) total_steps = Y.steps; if(total_steps < Z.steps) total_steps = Z.steps; stepped = 0; steps = total_steps; } // // Set the ramp table // void set_ramp(word length, const word *table) { if(length) { if(length > MAX_RAMP) length = MAX_RAMP; (void)memcpy(ramp, table, length * 2); } ramp_length = length; } // // Move horizontally // void goto_xy(word x, word y, word del) { while(steps) ; goto_xyz(x, y, Z.pos, del); } // // Move vertically // void goto_z(word z, word del) { while(steps) ; goto_xyz(X.pos, Y.pos, z, del); } // // Step one axis at time in the foreground, checks for negative limit // void step(axis_ty *axis, int steps, dword del) { word t = uS(del); set_steps(axis, steps); while(axis->steps) { if(axis->dir < 0 && axis->mlimit()) break; axis->pos += axis->dir; axis->start_step(); _delay(t); axis->end_step(); _delay(t); --axis->steps; } } // // Find the negative limit // void seek0(axis_ty *axis, dword move_delay, dword seek_delay, word hysteresis) { sword max_move = axis->max - axis->min; // maximum distance from limit max_move += max_move >> 2; // a bit more as we can go past the limits on X and Y if(axis->mlimit()) // If already past the limit step(axis ,1000,move_delay); // Step forward step(axis, -max_move, move_delay); // step back until hits limit sensor delay_us(seek_delay); // give it chance to stop step(axis,hysteresis, move_delay); // skip some of the hysteresis while(axis->mlimit()) // now go back slowly to limit goes away step(axis,1,seek_delay); } // // Initialise, find the negative limits and then home to the origin // void init_motors(void) { init_axis(&X, 0, 23000, set_x_dir, start_x_step, end_x_step, x_mlimit); init_axis(&Y, 0, 24000, set_y_dir, start_y_step, end_y_step, y_mlimit); init_axis(&Z, 0, 4000, set_z_dir, start_z_step, end_z_step, z_mlimit); seek0(&Z, ZMOVE_DELAY, ZSEEK_DELAY, ZHYSTERESIS); Z.pos &= 7; seek0(&X, MOVE_DELAY, SEEK_DELAY, HYSTERESIS); X.pos = 0; seek0(&Y, MOVE_DELAY, SEEK_DELAY, HYSTERESIS); Y.pos = 0; delay_us(10000); x_shaft = 0; y_shaft = 0; goto_xyz(ORIGIN_X, ORIGIN_Y, ORIGIN_Z, uS(MOVE_DELAY)); }
Re: A Question for Nophead September 27, 2009 11:12AM |
Registered: 16 years ago Posts: 467 |