josh
/
Teacup_Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

trying out some constant acceleration code, expect breakage

This commit is contained in:
Michael Moon 2010-02-12 18:10:03 +11:00
parent 7b79d2ea32
commit 3c7784cc3b
9 changed files with 131 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
mendel/clock.h
View File

@ -3,7 +3,7 @@
#include <stdint.h>
void clock_setup(void);
void clock_setup(void) __attribute__ ((cold));
#ifdef GLOBAL_CLOCK
uint32_t clock_read(void);

141
mendel/dda.c
View File

@ -106,6 +106,19 @@ uint32_t delta32(uint32_t v1, uint32_t v2) {
return v2 - v1;
}
// this is an ultra-crude pseudo-logarithm routine, such that:
// 2 ^ msbloc(v) >= v
const uint8_t msbloc (uint32_t v) {
uint8_t i;
uint32_t c;
for (i = 31, c = 0x80000000; i; i--) {
if (v & c)
return i;
v >>= 1;
}
return 0;
}
/*
CREATE a dda given current_position and a target, save to passed location so we can write directly into the queue
*/
@ -114,7 +127,7 @@ void dda_create(DDA *dda, TARGET *target) {
uint32_t distance;
// initialise DDA to a known state
dda->move_duration = 0;
// dda->move_duration = 0;
dda->live = 0;
dda->total_steps = 0;
@ -128,13 +141,13 @@ void dda_create(DDA *dda, TARGET *target) {
dda->y_delta = abs32(target->Y - startpoint.Y);
dda->z_delta = abs32(target->Z - startpoint.Z);
dda->e_delta = abs32(target->E - startpoint.E);
dda->f_delta = delta32(target->F, startpoint.F);
// dda->f_delta = delta32(target->F, startpoint.F);
dda->x_direction = (target->X >= startpoint.X)?1:0;
dda->y_direction = (target->Y >= startpoint.Y)?1:0;
dda->z_direction = (target->Z >= startpoint.Z)?1:0;
dda->e_direction = (target->E >= startpoint.E)?1:0;
dda->f_direction = (target->F >= startpoint.F)?1:0;
// dda->f_direction = (target->F >= startpoint.F)?1:0;
if (DEBUG) {
if (dda->x_direction == 0)
@ -149,9 +162,10 @@ void dda_create(DDA *dda, TARGET *target) {
if (dda->e_direction == 0)
serial_writechar('-');
serwrite_uint32(dda->e_delta); serial_writechar(',');
if (dda->f_direction == 0)
serial_writechar('-');
serwrite_uint32(dda->f_delta); serial_writestr_P(PSTR("] ["));
// if (dda->f_direction == 0)
// serial_writechar('-');
// serwrite_uint32(dda->f_delta);
serial_writestr_P(PSTR("] ["));
}
if (dda->x_delta > dda->total_steps)
@ -202,10 +216,55 @@ void dda_create(DDA *dda, TARGET *target) {
// break this calculation up a bit and lose some precision because 300,000um * 60000 is too big for a uint32
// calculate this with a uint64 if you need the precision, but it'll take longer so routines with lots of short moves may suffer
// 2^32/6000 is about 715mm which should be plenty
dda->move_duration = ((distance * 6000) / dda->total_steps) * 10;
if (DEBUG)
serwrite_uint32(dda->move_duration);
// changed * 10 to * (F_CPU / 100000) so we can work in cpu_ticks rather than microseconds.
// timer.c setTimer() routine altered for same reason
uint32_t move_duration = ((distance * 6000) / dda->total_steps) * (F_CPU / 100000);
// c is initial step time in IOclk ticks
dda->c = move_duration / startpoint.F;
if (startpoint.F != target->F) {
// now some linear acceleration stuff, courtesy of http://www.embedded.com/columns/technicalinsights/56800129?printable=true
uint32_t ssq = startpoint.F * startpoint.F;
uint32_t esq = target->F * target->F;
uint32_t dsq = esq - ssq;
dda->end_c = move_duration / target->F;
// the raw equation WILL overflow at high step rates, but 64 bit math routines take waay too much space
// at 65536 mm/min (1092mm/s), ssq/esq overflows, and dsq is also close to overflowing if esq/ssq is small
// but if ssq-esq is small, ssq/dsq is only a few bits
// we'll have to do it a few different ways depending on the msb location in each
if ((msbloc(dda->total_steps) + msbloc(ssq)) < 28) {
// we have room to do all the multiplies first
dda->n = ((dda->total_steps * ssq * 4) / dsq) + 1;
}
// else
// if ((msbloc(dda->total_steps) + msbloc(ssq)) < 30) {
// // we have room to do the main multiply first
// dda->n = (((dda->total_steps * ssq) / dsq) << 2) | 1;
// }
else if (msbloc(dda->total_steps) > msbloc(ssq)) {
// total steps has more precision
if (msbloc(dda->total_steps) < 28)
dda->n = (((dda->total_steps << 2) / dsq) * ssq) + 1;
else
dda->n = (((dda->total_steps / dsq) * ssq) << 2) | 1;
}
else {
// otherwise
if (msbloc(ssq) < 28)
dda->n = (((ssq << 2) / dsq) * dda->total_steps) + 1;
else
dda->n = (((ssq / dsq) * dda->total_steps) << 2) | 1;
}
// if (DEBUG)
// serwrite_uint32(dda->move_duration);
dda->accel = 1;
}
else
dda->accel = 0;
}
if (DEBUG)
@ -241,7 +300,7 @@ void dda_start(DDA *dda) {
dda->live = 1;
// set timeout for first step
setTimer(dda->move_duration / current_position.F);
setTimer(dda->c);
}
/*
@ -361,27 +420,43 @@ void dda_step(DDA *dda) {
sei();
#endif
if (step_option & F_CAN_STEP) {
dda->f_counter -= dda->f_delta;
// since we don't allow total_steps to be defined by F, we may need to step multiple times if f_delta is greater than total_steps
// loops in interrupt context are a bad idea, but this is the best way to do this that I've come up with so far
while (dda->f_counter < 0) {
// if (step_option & F_CAN_STEP) {
// dda->f_counter -= dda->f_delta;
// // since we don't allow total_steps to be defined by F, we may need to step multiple times if f_delta is greater than total_steps
// // loops in interrupt context are a bad idea, but this is the best way to do this that I've come up with so far
// while (dda->f_counter < 0) {
//
// dda->f_counter += dda->total_steps;
//
// if (dda->f_direction) {
// current_position.F += 1;
// if (current_position.F > dda->endpoint.F)
// current_position.F = dda->endpoint.F;
// }
// else {
// current_position.F -= 1;
// if (current_position.F < dda->endpoint.F)
// current_position.F = dda->endpoint.F;
// }
//
// step_option |= F_REAL_STEP;
// }
// }
dda->f_counter += dda->total_steps;
if (dda->f_direction) {
current_position.F += 1;
if (current_position.F > dda->endpoint.F)
current_position.F = dda->endpoint.F;
}
else {
current_position.F -= 1;
if (current_position.F < dda->endpoint.F)
current_position.F = dda->endpoint.F;
}
step_option |= F_REAL_STEP;
if (dda->accel) {
if (
((dda->n > 0) && (dda->c > dda->end_c)) ||
((dda->n < 0) && (dda->c < dda->end_c))
) {
dda->c = dda->c - ((dda->c * 2) / dda->n);
dda->n += 4;
setTimer(dda->c);
}
else if (dda->c != dda->end_c) {
dda->c = dda->end_c;
setTimer(dda->c);
}
// else we are already at target speed
}
if (step_option & REAL_MOVE)
@ -393,13 +468,15 @@ void dda_step(DDA *dda) {
// we simply don't have the memory to precalculate this for each step,
// can't use a simplified process because the denominator changes rather than the numerator so the curve is non-linear
// and don't have a process framework to force it to be done outside interrupt context within a usable period of time
if (step_option & F_REAL_STEP)
setTimer(dda->move_duration / current_position.F);
// if (step_option & F_REAL_STEP)
// setTimer(dda->move_duration / current_position.F);
// if we could do anything at all, we're still running
// otherwise, must have finished
else if (step_option == 0)
else if (step_option == 0) {
dda->live = 0;
current_position.F = dda->endpoint.F;
}
// turn off step outputs, hopefully they've been on long enough by now to register with the drivers
// if not, too bad. or insert a (very!) small delay here, or fire up a spare timer or something

17
mendel/dda.h
View File

@ -28,8 +28,10 @@ typedef struct {
uint8_t z_direction :1;
uint8_t e_direction :1;
uint8_t f_direction :1;
uint8_t nullmove :1;
uint8_t live :1;
uint8_t accel :1;
uint32_t x_delta;
uint32_t y_delta;
@ -44,8 +46,12 @@ typedef struct {
int32_t f_counter;
uint32_t total_steps;
// uint32_t move_duration;
uint32_t move_duration;
// for linear acceleration
uint32_t c;
uint32_t end_c;
int32_t n;
} DDA;
/*
@ -65,11 +71,12 @@ extern TARGET current_position;
methods
*/
uint32_t approx_distance( uint32_t dx, uint32_t dy );
uint32_t approx_distance_3( uint32_t dx, uint32_t dy, uint32_t dz );
uint32_t approx_distance( uint32_t dx, uint32_t dy ) __attribute__ ((hot));
uint32_t approx_distance_3( uint32_t dx, uint32_t dy, uint32_t dz ) __attribute__ ((hot));
const uint8_t msbloc (uint32_t v) __attribute__ ((const));
void dda_create(DDA *dda, TARGET *target);
void dda_start(DDA *dda);
void dda_step(DDA *dda);
void dda_start(DDA *dda) __attribute__ ((hot));
void dda_step(DDA *dda) __attribute__ ((hot));
#endif /* _DDA_H */

2
mendel/dda_queue.h
View File

@ -24,7 +24,7 @@ uint8_t queue_empty(void);
void enqueue(TARGET *t);
// called from step timer when current move is complete
void next_move(void);
void next_move(void) __attribute__ ((hot));
// print queue status
void print_queue(void);

5
mendel/gcode.c
View File

@ -427,7 +427,10 @@ void process_gcode_command(GCODE_COMMAND *gcmd) {
// G92 - set home
case 92:
startpoint.X = startpoint.Y = startpoint.Z = startpoint.E = 0;
startpoint.X = startpoint.Y = startpoint.Z = startpoint.E =
current_position.X = current_position.Y = current_position.Z = current_position.E = 0;
startpoint.F =
current_position.F = FEEDRATE_SLOW_Z;
break;
// unknown gcode: spit an error

2
mendel/timer.c
View File

@ -126,7 +126,7 @@ void setTimer(uint32_t delay)
// Actual ticks are 0.0625 us, so multiply delay by 16
// convert to ticks
delay = delay US;
// delay = delay US;
setTimerCeiling(getTimerCeiling(delay));
setTimerResolution(getTimerResolution(delay));

2
mendel/timer.h
View File

@ -11,7 +11,7 @@
// #define DEFAULT_TICK (100 US)
#define WAITING_DELAY (10 MS)
void setupTimerInterrupt(void);
void setupTimerInterrupt(void) __attribute__ ((cold));
uint8_t getTimerResolution(const uint32_t delay);
void setTimerResolution(uint8_t r);

4
mendel/watchdog.c
View File

@ -9,9 +9,7 @@
volatile uint8_t wd_flag = 0;
// uint8_t mcusr_mirror __attribute__ ((section (".noinit")));
// void get_mcusr(void) \
// __attribute__((naked)) \
// __attribute__((section(".init3")));
// void get_mcusr(void) __attribute__((naked)) __attribute__((section(".init3")));
// void get_mcusr(void) {
// mcusr_mirror = MCUSR;
// MCUSR = 0;

2
mendel/watchdog.h
View File

@ -2,7 +2,7 @@
#define _WATCHDOG_H
// initialize
void wd_init(void);
void wd_init(void) __attribute__ ((cold));
// reset timeout- must be called periodically or we reboot
void wd_reset(void);