/** * Determine the 'jerk' between 2 2D vectors and their speeds. The jerk can be * used to obtain an acceptable speed for changing directions between moves. * @param x1 x component of first vector * @param y1 y component of first vector * @param F1 feed rate of first move * @param x2 x component of second vector * @param y2 y component of second vector * @param F2 feed rate of second move */ int dda_jerk_size_2d_real(int32_t x1, int32_t y1, uint32_t F1, int32_t x2, int32_t y2, uint32_t F2) { const int maxlen = 10000; // Normalize vectors so their length will be fixed // Note: approx_distance is not precise enough and may result in violent direction changes //sersendf_P(PSTR("Input vectors: (%ld, %ld) and (%ld, %ld)\r\n"),x1,y1,x2,y2); int32_t len = int_sqrt(x1*x1+y1*y1); x1 = (x1 * maxlen) / len; y1 = (y1 * maxlen) / len; len = int_sqrt(x2*x2+y2*y2); x2 = (x2 * maxlen) / len; y2 = (y2 * maxlen) / len; //sersendf_P(PSTR("Normalized vectors: (%ld, %ld) and (%ld, %ld)\r\n"),x1,y1,x2,y2); // Now scale the normalized vectors by their speeds x1 *= F1; y1 *= F1; x2 *= F2; y2 *= F2; //sersendf_P(PSTR("Speed vectors: (%ld, %ld) and (%ld, %ld)\r\n"),x1,y1,x2,y2); // The difference between the vectors actually depicts the jerk x1 -= x2; y1 -= y2; //sersendf_P(PSTR("Jerk vector: (%ld, %ld)\r\n"),x1,y1); return approx_distance(x1,y1) / maxlen; }
void dda_create(DDA *dda, TARGET *target) { uint32_t distance, c_limit, c_limit_calc; // initialise DDA to a known state dda->allflags = 0; if (debug_flags & DEBUG_DDA) serial_writestr_P(PSTR("\n{DDA_CREATE: [")); // we end at the passed target memcpy(&(dda->endpoint), target, sizeof(TARGET)); dda->x_delta = ABS(target->X - startpoint.X); dda->y_delta = ABS(target->Y - startpoint.Y); dda->z_delta = ABS(target->Z - startpoint.Z); dda->e_delta = ABS(target->E - startpoint.E); 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; if (debug_flags & DEBUG_DDA) { if (dda->x_direction == 0) serial_writechar('-'); serwrite_uint32(dda->x_delta); serial_writechar(','); if (dda->y_direction == 0) serial_writechar('-'); serwrite_uint32(dda->y_delta); serial_writechar(','); if (dda->z_direction == 0) serial_writechar('-'); serwrite_uint32(dda->z_delta); serial_writechar(','); if (dda->e_direction == 0) serial_writechar('-'); serwrite_uint32(dda->e_delta); serial_writestr_P(PSTR("] [")); } dda->total_steps = dda->x_delta; if (dda->y_delta > dda->total_steps) dda->total_steps = dda->y_delta; if (dda->z_delta > dda->total_steps) dda->total_steps = dda->z_delta; if (dda->e_delta > dda->total_steps) dda->total_steps = dda->e_delta; if (debug_flags & DEBUG_DDA) { serial_writestr_P(PSTR("ts:")); serwrite_uint32(dda->total_steps); } if (dda->total_steps == 0) { dda->nullmove = 1; } else { // get steppers ready to go steptimeout = 0; power_on(); dda->x_counter = dda->y_counter = dda->z_counter = dda->e_counter = -(dda->total_steps >> 1); // 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 (dda->z_delta == 0) distance = approx_distance(dda->x_delta * UM_PER_STEP_X, dda->y_delta * UM_PER_STEP_Y); else if (dda->x_delta == 0 && dda->y_delta == 0) distance = dda->z_delta * UM_PER_STEP_Z; else distance = approx_distance_3(dda->x_delta * UM_PER_STEP_X, dda->y_delta * UM_PER_STEP_Y, dda->z_delta * UM_PER_STEP_Z); if (distance < 2) distance = dda->e_delta * UM_PER_STEP_E; if (debug_flags & DEBUG_DDA) { serial_writestr_P(PSTR(",ds:")); serwrite_uint32(distance); } // 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 // note: um (distance) * 60000 == mm * 60000000 // so in the interrupt we must simply calculate // 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 // 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 // changed * 10 to * (F_CPU / 100000) so we can work in cpu_ticks rather than microseconds. // timer.c setTimer() routine altered for same reason // changed distance * 6000 .. * F_CPU / 100000 to // 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); // 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. c_limit = 0; c_limit_calc = ( (dda->x_delta * (UM_PER_STEP_X * 2400L)) / dda->total_steps * (F_CPU / 40000) / MAXIMUM_FEEDRATE_X) << 8; if (c_limit_calc > c_limit) c_limit = c_limit_calc; c_limit_calc = ( (dda->y_delta * (UM_PER_STEP_Y * 2400L)) / dda->total_steps * (F_CPU / 40000) / MAXIMUM_FEEDRATE_Y) << 8; if (c_limit_calc > c_limit) c_limit = c_limit_calc; c_limit_calc = ( (dda->z_delta * (UM_PER_STEP_Z * 2400L)) / dda->total_steps * (F_CPU / 40000) / MAXIMUM_FEEDRATE_Z) << 8; if (c_limit_calc > c_limit) c_limit = c_limit_calc; c_limit_calc = ( (dda->e_delta * (UM_PER_STEP_E * 2400L)) / dda->total_steps * (F_CPU / 40000) / MAXIMUM_FEEDRATE_E) << 8; if (c_limit_calc > c_limit) c_limit = c_limit_calc; #ifdef ACCELERATION_REPRAP // c is initial step time in IOclk ticks dda->c = (move_duration / startpoint.F) << 8; if (dda->c < c_limit) dda->c = c_limit; dda->end_c = (move_duration / target->F) << 8; if (dda->end_c < c_limit) dda->end_c = c_limit; if (debug_flags & DEBUG_DDA) { serial_writestr_P(PSTR(",md:")); serwrite_uint32(move_duration); serial_writestr_P(PSTR(",c:")); serwrite_uint32(dda->c >> 8); } if (dda->c != dda->end_c) { uint32_t stF = startpoint.F / 4; uint32_t enF = target->F / 4; // now some constant acceleration stuff, courtesy of http://www.embedded.com/columns/technicalinsights/56800129?printable=true uint32_t ssq = (stF * stF); uint32_t esq = (enF * enF); int32_t dsq = (int32_t) (esq - ssq) / 4; uint8_t msb_ssq = msbloc(ssq); 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 // 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 locations of each if ((msb_tot + msb_ssq) <= 30) { // we have room to do all the multiplies first if (debug_flags & DEBUG_DDA) serial_writechar('A'); dda->n = ((int32_t) (dda->total_steps * ssq) / dsq) + 1; } else if (msb_tot >= msb_ssq) { // total steps has more precision if (debug_flags & DEBUG_DDA) serial_writechar('B'); dda->n = (((int32_t) dda->total_steps / dsq) * (int32_t) ssq) + 1; } else { // otherwise if (debug_flags & DEBUG_DDA) serial_writechar('C'); dda->n = (((int32_t) ssq / dsq) * (int32_t) dda->total_steps) + 1; } if (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"), (long unsigned int)dda->end_c >> 8, (long int)dda->n, (long unsigned int)move_duration, (long unsigned int)ssq, (long unsigned int)esq, (long unsigned int)dsq, msb_ssq, msb_tot); }