|
|
@ -320,8 +320,8 @@ void Stepper::set_directions() {
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(S_CURVE_ACCELERATION)
|
|
|
|
#if ENABLED(S_CURVE_ACCELERATION)
|
|
|
|
/**
|
|
|
|
/**
|
|
|
|
* We are using a quintic (fifth-degree) Bézier polynomial for the velocity curve.
|
|
|
|
* This uses a quintic (fifth-degree) Bézier polynomial for the velocity curve, giving
|
|
|
|
* This gives us a "linear pop" velocity curve; with pop being the sixth derivative of position:
|
|
|
|
* a "linear pop" velocity curve; with pop being the sixth derivative of position:
|
|
|
|
* velocity - 1st, acceleration - 2nd, jerk - 3rd, snap - 4th, crackle - 5th, pop - 6th
|
|
|
|
* velocity - 1st, acceleration - 2nd, jerk - 3rd, snap - 4th, crackle - 5th, pop - 6th
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* The Bézier curve takes the form:
|
|
|
|
* The Bézier curve takes the form:
|
|
|
@ -395,7 +395,7 @@ void Stepper::set_directions() {
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* For Any 32bit CPU:
|
|
|
|
* For Any 32bit CPU:
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* At the start of each trapezoid, we calculate the coefficients A,B,C,F and Advance [AV], as follows:
|
|
|
|
* At the start of each trapezoid, calculate the coefficients A,B,C,F and Advance [AV], as follows:
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* A = 6*128*(VF - VI) = 768*(VF - VI)
|
|
|
|
* A = 6*128*(VF - VI) = 768*(VF - VI)
|
|
|
|
* B = 15*128*(VI - VF) = 1920*(VI - VF)
|
|
|
|
* B = 15*128*(VI - VF) = 1920*(VI - VF)
|
|
|
@ -403,7 +403,7 @@ void Stepper::set_directions() {
|
|
|
|
* F = 128*VI = 128*VI
|
|
|
|
* F = 128*VI = 128*VI
|
|
|
|
* AV = (1<<32)/TS ~= 0xFFFFFFFF / TS (To use ARM UDIV, that is 32 bits) (this is computed at the planner, to offload expensive calculations from the ISR)
|
|
|
|
* AV = (1<<32)/TS ~= 0xFFFFFFFF / TS (To use ARM UDIV, that is 32 bits) (this is computed at the planner, to offload expensive calculations from the ISR)
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* And for each point, we will evaluate the curve with the following sequence:
|
|
|
|
* And for each point, evaluate the curve with the following sequence:
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* void lsrs(uint32_t& d, uint32_t s, int cnt) {
|
|
|
|
* void lsrs(uint32_t& d, uint32_t s, int cnt) {
|
|
|
|
* d = s >> cnt;
|
|
|
|
* d = s >> cnt;
|
|
|
@ -456,10 +456,10 @@ void Stepper::set_directions() {
|
|
|
|
* return alo;
|
|
|
|
* return alo;
|
|
|
|
* }
|
|
|
|
* }
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* This will be rewritten in ARM assembly to get peak performance and will take 43 cycles to execute
|
|
|
|
* This is rewritten in ARM assembly for optimal performance (43 cycles to execute).
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* For AVR, we scale precision of coefficients to make it possible to evaluate the Bézier curve in
|
|
|
|
* For AVR, the precision of coefficients is scaled so the Bézier curve can be evaluated in real-time:
|
|
|
|
* realtime: Let's reduce precision as much as possible. After some experimentation we found that:
|
|
|
|
* Let's reduce precision as much as possible. After some experimentation we found that:
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* Assume t and AV with 24 bits is enough
|
|
|
|
* Assume t and AV with 24 bits is enough
|
|
|
|
* A = 6*(VF - VI)
|
|
|
|
* A = 6*(VF - VI)
|
|
|
@ -480,7 +480,7 @@ void Stepper::set_directions() {
|
|
|
|
* C: signed Q24 , range = 250000 *10 = 2500000 = 0x1312D0 | 21 bits
|
|
|
|
* C: signed Q24 , range = 250000 *10 = 2500000 = 0x1312D0 | 21 bits
|
|
|
|
* F: signed Q24 , range = 250000 = 250000 = 0x0ED090 | 20 bits
|
|
|
|
* F: signed Q24 , range = 250000 = 250000 = 0x0ED090 | 20 bits
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* And for each curve, we estimate its coefficients with:
|
|
|
|
* And for each curve, estimate its coefficients with:
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* void _calc_bezier_curve_coeffs(int32_t v0, int32_t v1, uint32_t av) {
|
|
|
|
* void _calc_bezier_curve_coeffs(int32_t v0, int32_t v1, uint32_t av) {
|
|
|
|
* // Calculate the Bézier coefficients
|
|
|
|
* // Calculate the Bézier coefficients
|
|
|
@ -499,7 +499,7 @@ void Stepper::set_directions() {
|
|
|
|
* bezier_F = v0;
|
|
|
|
* bezier_F = v0;
|
|
|
|
* }
|
|
|
|
* }
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* And for each point, we will evaluate the curve with the following sequence:
|
|
|
|
* And for each point, evaluate the curve with the following sequence:
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* // unsigned multiplication of 24 bits x 24bits, return upper 16 bits
|
|
|
|
* // unsigned multiplication of 24 bits x 24bits, return upper 16 bits
|
|
|
|
* void umul24x24to16hi(uint16_t& r, uint24_t op1, uint24_t op2) {
|
|
|
|
* void umul24x24to16hi(uint16_t& r, uint24_t op1, uint24_t op2) {
|
|
|
@ -549,9 +549,8 @@ void Stepper::set_directions() {
|
|
|
|
* }
|
|
|
|
* }
|
|
|
|
* return acc;
|
|
|
|
* return acc;
|
|
|
|
* }
|
|
|
|
* }
|
|
|
|
* Those functions will be translated into assembler to get peak performance. coefficient calculations takes 70 cycles,
|
|
|
|
* These functions are translated to assembler for optimal performance.
|
|
|
|
* Bezier point evaluation takes 150 cycles
|
|
|
|
* Coefficient calculation takes 70 cycles. Bezier point evaluation takes 150 cycles.
|
|
|
|
*
|
|
|
|
|
|
|
|
*/
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
|
|
#ifdef __AVR__
|
|
|
|
#ifdef __AVR__
|
|
|
|