dda_lookahead.c: remove unused code.
This commit is contained in:
Markus Hitter
parent
6b66d7870a
commit
4e30501868
113
dda_lookahead.c
113
dda_lookahead.c
|
|
@ -11,9 +11,6 @@
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
#include <stddef.h>
|
#include <stddef.h>
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#ifdef __AVR__
|
|
||||||
#include <avr/interrupt.h>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "dda_maths.h"
|
#include "dda_maths.h"
|
||||||
#include "dda.h"
|
#include "dda.h"
|
||||||
|
|
@ -50,116 +47,6 @@ static const axes_uint32_t PROGMEM maximum_jerk_P = {
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
// We also need the inverse: given a ramp length, determine the expected speed
|
|
||||||
// Note: the calculation is scaled by a factor 16384 to obtain an answer with a smaller
|
|
||||||
// rounding error.
|
|
||||||
// Warning: this is an expensive function as it requires a square root to get the result.
|
|
||||||
//
|
|
||||||
uint32_t dda_steps_to_velocity(uint32_t steps) {
|
|
||||||
// v(t) = a*t, with v in mm/s and a = acceleration in mm/s²
|
|
||||||
// s(t) = 1/2*a*t² with s (displacement) in mm
|
|
||||||
// Rewriting yields v(s) = sqrt(2*a*s)
|
|
||||||
// Rewriting into steps and seperation in constant part and dynamic part:
|
|
||||||
// F_steps = sqrt((2000*a)/STEPS_PER_M_X) * 60 * sqrt(steps)
|
|
||||||
static uint32_t part = 0;
|
|
||||||
if(part == 0)
|
|
||||||
part = (uint32_t)sqrtf((2000.0f*3600.0f*ACCELERATION*16384.0f)/(float)STEPS_PER_M_X);
|
|
||||||
uint32_t res = int_sqrt((steps) << 14) * part;
|
|
||||||
return res >> 14;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 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 = 16384;
|
|
||||||
// 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Determine the 'jerk' for 2 1D vectors and their speeds. The jerk can be used to obtain an
|
|
||||||
* acceptable speed for changing directions between moves.
|
|
||||||
* @param x component of 1d vector - used to determine if we go back or forward
|
|
||||||
* @param F feed rate
|
|
||||||
*/
|
|
||||||
int dda_jerk_size_1d(int32_t x1, uint32_t F1, int32_t x2, uint32_t F2) {
|
|
||||||
if(x1 > 0) x1 = F1;
|
|
||||||
else x1 = -F1;
|
|
||||||
if(x2 > 0) x2 = F2;
|
|
||||||
else x2 = -F2;
|
|
||||||
|
|
||||||
// The difference between the vectors actually depicts the jerk
|
|
||||||
x1 -= x2;
|
|
||||||
if(x1 < 0) x1 = -x1; // Make sure it remains positive
|
|
||||||
|
|
||||||
//sersendf_P(PSTR("Jerk vector: (%ld, %ld)\r\n"),x1,y1);
|
|
||||||
return x1;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Determine the 'jerk' between 2 vectors and their speeds. The jerk can be used to obtain an
|
|
||||||
* acceptable speed for changing directions between moves.
|
|
||||||
* Instead of using 2 axis at once, consider the jerk for each axis individually and take the
|
|
||||||
* upper limit between both. This ensures that each axis does not changes speed too fast.
|
|
||||||
* @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(int32_t x1, int32_t y1, uint32_t F1, int32_t x2, int32_t y2, uint32_t F2) {
|
|
||||||
return MAX(dda_jerk_size_1d(x1,F1,x2,F2),dda_jerk_size_1d(y1,F1,y2,F2));
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Safety procedure: if something goes wrong, for example an opto is triggered during normal movement,
|
|
||||||
* we shut down the entire machine.
|
|
||||||
* @param msg The reason why the machine did an emergency stop
|
|
||||||
*/
|
|
||||||
void dda_emergency_shutdown(PGM_P msg_P) {
|
|
||||||
// Todo: is it smart to enable all interrupts again? e.g. can we create concurrent executions?
|
|
||||||
sei(); // Enable interrupts to print the message
|
|
||||||
serial_writestr_P(PSTR("error: emergency stop:"));
|
|
||||||
if (msg_P != NULL) serial_writestr_P(msg_P);
|
|
||||||
serial_writestr_P(PSTR("\r\n"));
|
|
||||||
delay_ms(20); // Delay so the buffer can be flushed - otherwise the message is never sent
|
|
||||||
timer_stop();
|
|
||||||
queue_flush();
|
|
||||||
power_off();
|
|
||||||
cli();
|
|
||||||
for (;;) { }
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* \brief Find maximum corner speed between two moves.
|
* \brief Find maximum corner speed between two moves.
|
||||||
* \details Find out how fast we can move around around a corner without
|
* \details Find out how fast we can move around around a corner without
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue