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

whitespace correction for dda_create()

This commit is contained in:
Nico Tonnhofer 2017-11-01 06:34:30 +01:00
parent e965931d9a
commit 8427c2724a
1 changed files with 115 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

230
dda.c
View File

@ -154,7 +154,7 @@ void dda_new_startpoint(void) {
void dda_create(DDA *dda, const TARGET *target) { void dda_create(DDA *dda, const TARGET *target) {
axes_uint32_t delta_um; axes_uint32_t delta_um;
axes_int32_t steps; axes_int32_t steps;
uint32_t distance; uint32_t distance;
#ifndef ACCELERATION_TEMPORAL #ifndef ACCELERATION_TEMPORAL
uint32_t c_limit, c_limit_calc; uint32_t c_limit, c_limit_calc;
#endif #endif
@ -173,10 +173,10 @@ void dda_create(DDA *dda, const TARGET *target) {
// We end at the passed target. // We end at the passed target.
memcpy(&(dda->endpoint), target, sizeof(TARGET)); memcpy(&(dda->endpoint), target, sizeof(TARGET));
if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
sersendf_P(PSTR("\nCreate: X %lq Y %lq Z %lq F %lu\n"), sersendf_P(PSTR("\nCreate: X %lq Y %lq Z %lq F %lu\n"),
dda->endpoint.axis[X], dda->endpoint.axis[Y], dda->endpoint.axis[X], dda->endpoint.axis[Y],
dda->endpoint.axis[Z], dda->endpoint.F); dda->endpoint.axis[Z], dda->endpoint.F);
// Apply feedrate multiplier. // Apply feedrate multiplier.
if (dda->endpoint.f_multiplier != 256 && ! dda->endstop_check) { if (dda->endpoint.f_multiplier != 256 && ! dda->endstop_check) {
@ -236,12 +236,12 @@ void dda_create(DDA *dda, const TARGET *target) {
delta_um[E] = (uint32_t)labs(target->axis[E]); delta_um[E] = (uint32_t)labs(target->axis[E]);
dda->delta[E] = (uint32_t)labs(steps[E]); dda->delta[E] = (uint32_t)labs(steps[E]);
dda->e_direction = (target->axis[E] >= 0)?1:0; dda->e_direction = (target->axis[E] >= 0)?1:0;
} }
if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
sersendf_P(PSTR("[%ld,%ld,%ld,%ld]"), sersendf_P(PSTR("[%ld,%ld,%ld,%ld]"),
target->axis[X] - startpoint.axis[X], target->axis[Y] - startpoint.axis[Y], target->axis[X] - startpoint.axis[X], target->axis[Y] - startpoint.axis[Y],
target->axis[Z] - startpoint.axis[Z], target->axis[E] - startpoint.axis[E]); target->axis[Z] - startpoint.axis[Z], target->axis[E] - startpoint.axis[E]);
// Admittedly, this looks like it's overcomplicated. Why store three 32-bit // Admittedly, this looks like it's overcomplicated. Why store three 32-bit
// values if storing an axis number would be fully sufficient? Well, I'm not // values if storing an axis number would be fully sufficient? Well, I'm not
@ -256,38 +256,38 @@ void dda_create(DDA *dda, const TARGET *target) {
} }
} }
if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
sersendf_P(PSTR(" [ts:%lu"), dda->total_steps); sersendf_P(PSTR(" [ts:%lu"), dda->total_steps);
if (dda->total_steps == 0) { if (dda->total_steps == 0) {
dda->nullmove = 1; dda->nullmove = 1;
startpoint.F = dda->endpoint.F; startpoint.F = dda->endpoint.F;
} }
else { else {
// get steppers ready to go // get steppers ready to go
power_on(); power_on();
stepper_enable(); stepper_enable();
x_enable(); x_enable();
y_enable(); y_enable();
#ifndef Z_AUTODISABLE #ifndef Z_AUTODISABLE
z_enable(); z_enable();
// #else Z is enabled in dda_start(). // #else Z is enabled in dda_start().
#endif #endif
e_enable(); e_enable();
// since it's unusual to combine X, Y and Z changes in a single move on reprap, check if we can use simpler approximations before trying the full 3d approximation. // since it's unusual to combine X, Y and Z changes in a single move on reprap, check if we can use simpler approximations before trying the full 3d approximation.
if (delta_um[Z] == 0) if (delta_um[Z] == 0)
distance = approx_distance(delta_um[X], delta_um[Y]); distance = approx_distance(delta_um[X], delta_um[Y]);
else if (delta_um[X] == 0 && delta_um[Y] == 0) else if (delta_um[X] == 0 && delta_um[Y] == 0)
distance = delta_um[Z]; distance = delta_um[Z];
else else
distance = approx_distance_3(delta_um[X], delta_um[Y], delta_um[Z]); distance = approx_distance_3(delta_um[X], delta_um[Y], delta_um[Z]);
if (distance < 1) if (distance < 1)
distance = delta_um[E]; distance = delta_um[E];
if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
sersendf_P(PSTR(",ds:%lu"), distance); sersendf_P(PSTR(",ds:%lu"), distance);
#ifdef ACCELERATION_TEMPORAL #ifdef ACCELERATION_TEMPORAL
// bracket part of this equation in an attempt to avoid overflow: // bracket part of this equation in an attempt to avoid overflow:
@ -301,97 +301,97 @@ void dda_create(DDA *dda, const TARGET *target) {
move_duration = distance * (60UL * (F_CPU / 1000) / dda->endpoint.F); move_duration = distance * (60UL * (F_CPU / 1000) / dda->endpoint.F);
for (i = X; i < AXIS_COUNT; i++) { for (i = X; i < AXIS_COUNT; i++) {
md_candidate = delta_um[i] * (60UL * (F_CPU / 1000) / md_candidate = delta_um[i] * (60UL * (F_CPU / 1000) /
pgm_read_dword(&maximum_feedrate_P[i])); pgm_read_dword(&maximum_feedrate_P[i]));
if (md_candidate > move_duration) if (md_candidate > move_duration)
move_duration = md_candidate; move_duration = md_candidate;
} }
#else #else
// pre-calculate move speed in millimeter microseconds per step minute for less math in interrupt context // pre-calculate move speed in millimeter microseconds per step minute for less math in interrupt context
// mm (distance) * 60000000 us/min / step (total_steps) = mm.us per step.min // mm (distance) * 60000000 us/min / step (total_steps) = mm.us per step.min
// note: um (distance) * 60000 == mm * 60000000 // note: um (distance) * 60000 == mm * 60000000
// so in the interrupt we must simply calculate // so in the interrupt we must simply calculate
// mm.us per step.min / mm per min (F) = us per step // mm.us per step.min / mm per min (F) = us per step
// break this calculation up a bit and lose some precision because 300,000um * 60000 is too big for a uint32 // 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 // 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 // 2^32/6000 is about 715mm which should be plenty
// changed * 10 to * (F_CPU / 100000) so we can work in cpu_ticks rather than microseconds. // changed * 10 to * (F_CPU / 100000) so we can work in cpu_ticks rather than microseconds.
// timer.c timer_set() routine altered for same reason // timer.c timer_set() routine altered for same reason
// changed distance * 6000 .. * F_CPU / 100000 to // changed distance * 6000 .. * F_CPU / 100000 to
// distance * 2400 .. * F_CPU / 40000 so we can move a distance of up to 1800mm without overflowing // distance * 2400 .. * F_CPU / 40000 so we can move a distance of up to 1800mm without overflowing
uint32_t move_duration = ((distance * 2400) / dda->total_steps) * (F_CPU / 40000); uint32_t move_duration = ((distance * 2400) / dda->total_steps) * (F_CPU / 40000);
// similarly, find out how fast we can run our axes. // similarly, find out how fast we can run our axes.
// do this for each axis individually, as the combined speed of two or more axes can be higher than the capabilities of a single one. // do this for each axis individually, as the combined speed of two or more axes can be higher than the capabilities of a single one.
// TODO: instead of calculating c_min directly, it's probably more simple // TODO: instead of calculating c_min directly, it's probably more simple
// to calculate (maximum) move_duration for each axis, like done for // to calculate (maximum) move_duration for each axis, like done for
// ACCELERATION_TEMPORAL above. This should make re-calculating the // ACCELERATION_TEMPORAL above. This should make re-calculating the
// allowed F easier. // allowed F easier.
c_limit = 0; c_limit = 0;
for (i = X; i < AXIS_COUNT; i++) { for (i = X; i < AXIS_COUNT; i++) {
c_limit_calc = (delta_um[i] * 2400L) / c_limit_calc = (delta_um[i] * 2400L) /
dda->total_steps * (F_CPU / 40000) / dda->total_steps * (F_CPU / 40000) /
pgm_read_dword(&maximum_feedrate_P[i]); pgm_read_dword(&maximum_feedrate_P[i]);
if (c_limit_calc > c_limit) if (c_limit_calc > c_limit)
c_limit = c_limit_calc; c_limit = c_limit_calc;
} }
#endif #endif
#ifdef ACCELERATION_REPRAP #ifdef ACCELERATION_REPRAP
// c is initial step time in IOclk ticks // c is initial step time in IOclk ticks
dda->c = move_duration / startpoint.F; dda->c = move_duration / startpoint.F;
if (dda->c < c_limit) if (dda->c < c_limit)
dda->c = c_limit; dda->c = c_limit;
dda->end_c = move_duration / dda->endpoint.F; dda->end_c = move_duration / dda->endpoint.F;
if (dda->end_c < c_limit) if (dda->end_c < c_limit)
dda->end_c = c_limit; dda->end_c = c_limit;
if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
sersendf_P(PSTR(",md:%lu,c:%lu"), move_duration, dda->c); sersendf_P(PSTR(",md:%lu,c:%lu"), move_duration, dda->c);
if (dda->c != dda->end_c) { if (dda->c != dda->end_c) {
uint32_t stF = startpoint.F / 4; uint32_t stF = startpoint.F / 4;
uint32_t enF = dda->endpoint.F / 4; uint32_t enF = dda->endpoint.F / 4;
// now some constant acceleration stuff, courtesy of http://www.embedded.com/design/mcus-processors-and-socs/4006438/Generate-stepper-motor-speed-profiles-in-real-time // now some constant acceleration stuff, courtesy of http://www.embedded.com/design/mcus-processors-and-socs/4006438/Generate-stepper-motor-speed-profiles-in-real-time
uint32_t ssq = (stF * stF); uint32_t ssq = (stF * stF);
uint32_t esq = (enF * enF); uint32_t esq = (enF * enF);
int32_t dsq = (int32_t) (esq - ssq) / 4; int32_t dsq = (int32_t) (esq - ssq) / 4;
uint8_t msb_ssq = msbloc(ssq); uint8_t msb_ssq = msbloc(ssq);
uint8_t msb_tot = msbloc(dda->total_steps); uint8_t msb_tot = msbloc(dda->total_steps);
// the raw equation WILL overflow at high step rates, but 64 bit math routines take waay too much space // 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 // 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 // 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 locations of each // we'll have to do it a few different ways depending on the msb locations of each
if ((msb_tot + msb_ssq) <= 30) { if ((msb_tot + msb_ssq) <= 30) {
// we have room to do all the multiplies first // we have room to do all the multiplies first
if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
serial_writechar('A'); serial_writechar('A');
dda->n = ((int32_t) (dda->total_steps * ssq) / dsq) + 1; dda->n = ((int32_t) (dda->total_steps * ssq) / dsq) + 1;
} }
else if (msb_tot >= msb_ssq) { else if (msb_tot >= msb_ssq) {
// total steps has more precision // total steps has more precision
if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
serial_writechar('B'); serial_writechar('B');
dda->n = (((int32_t) dda->total_steps / dsq) * (int32_t) ssq) + 1; dda->n = (((int32_t) dda->total_steps / dsq) * (int32_t) ssq) + 1;
} }
else { else {
// otherwise // otherwise
if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
serial_writechar('C'); serial_writechar('C');
dda->n = (((int32_t) ssq / dsq) * (int32_t) dda->total_steps) + 1; dda->n = (((int32_t) ssq / dsq) * (int32_t) dda->total_steps) + 1;
} }
if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
sersendf_P(PSTR("\n{DDA:CA end_c:%lu, n:%ld, md:%lu, ssq:%lu, esq:%lu, dsq:%lu, msbssq:%u, msbtot:%u}\n"), dda->end_c, dda->n, move_duration, ssq, esq, dsq, msb_ssq, msb_tot); sersendf_P(PSTR("\n{DDA:CA end_c:%lu, n:%ld, md:%lu, ssq:%lu, esq:%lu, dsq:%lu, msbssq:%u, msbtot:%u}\n"), dda->end_c, dda->n, move_duration, ssq, esq, dsq, msb_ssq, msb_tot);
dda->accel = 1; dda->accel = 1;
} }
else else
dda->accel = 0; dda->accel = 0;
#elif defined ACCELERATION_RAMPING #elif defined ACCELERATION_RAMPING
dda->c_min = move_duration / dda->endpoint.F; dda->c_min = move_duration / dda->endpoint.F;
if (dda->c_min < c_limit) { if (dda->c_min < c_limit) {
dda->c_min = c_limit; dda->c_min = c_limit;
@ -405,7 +405,7 @@ void dda_create(DDA *dda, const TARGET *target) {
// Acceleration ramps are based on the fast axis, not the combined speed. // Acceleration ramps are based on the fast axis, not the combined speed.
dda->rampup_steps = dda->rampup_steps =
acc_ramp_len(muldiv(dda->fast_um, dda->endpoint.F, distance), acc_ramp_len(muldiv(dda->fast_um, dda->endpoint.F, distance),
dda->fast_axis); dda->fast_axis);
if (dda->rampup_steps > dda->total_steps / 2) if (dda->rampup_steps > dda->total_steps / 2)
dda->rampup_steps = dda->total_steps / 2; dda->rampup_steps = dda->total_steps / 2;
@ -436,8 +436,8 @@ void dda_create(DDA *dda, const TARGET *target) {
dda->c = pgm_read_dword(&c0_P[dda->fast_axis]); dda->c = pgm_read_dword(&c0_P[dda->fast_axis]);
#endif #endif
#elif defined ACCELERATION_TEMPORAL #elif defined ACCELERATION_TEMPORAL
// TODO: calculate acceleration/deceleration for each axis // TODO: calculate acceleration/deceleration for each axis
for (i = X; i < AXIS_COUNT; i++) { for (i = X; i < AXIS_COUNT; i++) {
dda->step_interval[i] = 0xFFFFFFFF; dda->step_interval[i] = 0xFFFFFFFF;
if (dda->delta[i]) if (dda->delta[i])
@ -453,21 +453,21 @@ void dda_create(DDA *dda, const TARGET *target) {
} }
} }
#else #else
dda->c = move_duration / dda->endpoint.F; dda->c = move_duration / dda->endpoint.F;
if (dda->c < c_limit) if (dda->c < c_limit)
dda->c = c_limit; dda->c = c_limit;
#endif #endif
// next dda starts where we finish // next dda starts where we finish
memcpy(&startpoint, &dda->endpoint, sizeof(TARGET)); memcpy(&startpoint, &dda->endpoint, sizeof(TARGET));
#ifdef LOOKAHEAD #ifdef LOOKAHEAD
prev_dda = dda; prev_dda = dda;
#endif #endif
} /* ! dda->total_steps == 0 */ } /* ! dda->total_steps == 0 */
if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
serial_writestr_P(PSTR("] }\n")); serial_writestr_P(PSTR("] }\n"));
} }
/** Start a prepared DDA /** Start a prepared DDA