forked from Traumflug/Teacup_Firmware
-
Notifications
You must be signed in to change notification settings - Fork 0
/
dda_maths.h
122 lines (105 loc) · 4.83 KB
/
dda_maths.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
#define RESOLUTION 10000
#ifndef _DDA_MATHS_H
#define _DDA_MATHS_H
#include <stdint.h>
#include "config_wrapper.h"
#include "dda.h"
// return rounded result of multiplicand * multiplier / divisor
// this version is with quotient and remainder precalculated elsewhere
const int32_t muldivQR(int32_t multiplicand, uint32_t qn, uint32_t rn,
uint32_t divisor);
// return rounded result of multiplicand * multiplier / divisor
static int32_t muldiv(int32_t, uint32_t, uint32_t) __attribute__ ((always_inline));
inline int32_t muldiv(int32_t multiplicand, uint32_t multiplier,
uint32_t divisor) {
return muldivQR(multiplicand, multiplier / divisor,
multiplier % divisor, divisor);
}
/*!
Micrometer distance <=> motor step distance conversions.
*/
#define UM_PER_METER (1000000UL)
//extern const axes_uint32_t PROGMEM axis_qn_P;
//extern const axes_uint32_t PROGMEM axis_qr_P;
extern axes_uint32_t axis_qn_P;
extern axes_uint32_t axis_qr_P;
extern axes_uint32_t acc_ramp_div_P;
static int32_t um_to_steps(int32_t, enum axis_e) __attribute__ ((always_inline));
inline int32_t um_to_steps(int32_t distance, enum axis_e a) {
// return muldivQR(distance, pgm_read_dword(&axis_qn_P[a]), pgm_read_dword(&axis_qr_P[a]), UM_PER_METER);
return muldivQR(distance, axis_qn_P[a],
axis_qr_P[a], UM_PER_METER);
}
// approximate 2D distance
uint32_t approx_distance(uint32_t dx, uint32_t dy);
// approximate 3D distance
uint32_t approx_distance_3(uint32_t dx, uint32_t dy, uint32_t dz);
// integer square root algorithm
uint16_t int_sqrt(uint32_t a);
// integer inverse square root, 12bits precision
uint32_t aSquareRoot32(uint32_t a_nInput);
uint32_t SquareRoot32(uint32_t x);
// integer inverse square root, 12bits precision
uint16_t int_inv_sqrt(uint16_t a);
// this is an ultra-crude pseudo-logarithm routine, such that:
// 2 ^ msbloc(v) >= v
const uint8_t msbloc (uint32_t v);
/*! Acceleration ramp length in steps.
* \param feedrate Target feedrate of the accelerateion.
* \param fast_axis Number of the fastest axis.
* \return Accelerating steps neccessary to achieve target feedrate.
*
* s = 1/2 * a * t^2, v = a * t ==> s = v^2 / (2 * a)
* 7200000 = 60 * 60 * 1000 * 2 (mm/min -> mm/s, steps/m -> steps/mm, factor 2)
*
* Note: this function has shown to be accurate between 10 and 10'000 mm/s2 and
* 2000 to 4096000 steps/m (and higher). The numbers are a few percent
* too high at very low acceleration. Test code see commit message.
*/
// Calculates acceleration ramp length in steps.
uint32_t acc_ramp_len(uint32_t feedrate, uint8_t fast_axis);
// For X axis only, should become obsolete:
#define ACCELERATE_RAMP_LEN(speed) (((speed)*(speed)) / (uint32_t)((7200000.0f * ACCELERATION) / (float)STEPS_PER_M_X))
//uint32_t ACCELERATE_RAMP_LEN(uint32_t speed){
// return (((speed)*(speed)) / (uint32_t)((7200000.0f * _ACCELERATION) / (float)_STEPS_PER_M_X))
//}
// Compile-time trigonometric functions. See Taylor series.
// Converts degrees to radians.
#define DegToRad(angleDegrees) ((angleDegrees) * M_PI / 180.0)
//Make an angle (in radian) coterminal (0 >= x > 2pi).
//#define COTERMINAL(x) (fmod((x), M_PI*2)) //This causes error since fmod() implementation is different from i386 compiler.
//TODO: Solve coterminality
#define COTERMINAL(x) (x)
// Get quadrant of an angle in radian.
#define QUADRANT(x) ((uint8_t)((COTERMINAL((x))) / M_PI_2) + 1)
//Calculate sine of an angle in radians.
//Formula will be adjusted accordingly to the angle's quadrant.
//Don't worry about pow() function here as it will be optimized by the compiler.
#define SIN0(x) (x)
#define SIN1(x) (SIN0(x) - (pow ((x), 3) / 6))
#define SIN2(x) (SIN1(x) + (pow ((x), 5) / 120))
#define SIN3(x) (SIN2(x) - (pow ((x), 7) / 5040))
#define SIN4(x) (SIN3(x) + (pow ((x), 9) / 362880))
//*
#define SIN(x) (QUADRANT((x)) == 1 ? (SIN4(COTERMINAL((x)))) : \
(QUADRANT((x)) == 2 ? (SIN4(M_PI - COTERMINAL((x)))) : \
(QUADRANT((x)) == 3 ? -(SIN4(COTERMINAL((x)) - M_PI)) : \
-(SIN4(M_PI*2 - COTERMINAL((x)))))))
// */
//#define SIN(x) (SIN4(x))
//Calculate cosine of an angle in radians.
//Formula will be adjusted accordingly to the angle's quadrant.
//Don't worry about pow() function here as it will be optimized by the compiler.
#define COS0(x) 1
#define COS1(x) (COS0(x) - (pow ((x), 2) / 2))
#define COS2(x) (COS1(x) + (pow ((x), 4) / 24))
#define COS3(x) (COS2(x) - (pow ((x), 6) / 720))
#define COS4(x) (COS3(x) + (pow ((x), 8) / 40320))
//*
#define COS(x) (QUADRANT((x)) == 1 ? (COS4(COTERMINAL((x)))) : \
(QUADRANT((x)) == 2 ? -(COS4(M_PI - COTERMINAL((x)))) : \
(QUADRANT((x)) == 3 ? -(COS4(COTERMINAL((x)) - M_PI)) : \
(COS4(M_PI*2 - COTERMINAL((x)))))))
// */
//#define COS(x) COS4((x))
#endif /* _DDA_MATHS_H */