-
Notifications
You must be signed in to change notification settings - Fork 56
/
StepControlBase.h
182 lines (143 loc) · 6.5 KB
/
StepControlBase.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
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
#pragma once
#include "MotorControlBase.h"
#include <algorithm>
namespace TeensyStep
{
template <typename Accelerator, typename TimerField>
class StepControlBase : public MotorControlBase<TimerField>
{
public:
StepControlBase(unsigned pulseWidth = 5, unsigned accUpdatePeriod = 5000);
// Non-blocking movements ------------------------------------------
// Move variadic param list of stepper references e.g. moveAsync(m1,m2,m3,m5)
template <typename... Steppers>
void moveAsync(Steppers&... steppers) { moveAsync(1.0f, steppers...); }
// Move variadic param list of stepper references with speed override e.g. moveAsync(0.2f,m1,m2,m3,m5)
template <typename... Steppers>
void moveAsync(float speedOverride, Steppers&... steppers);
// Move array of stepper pointers with speed override
template <size_t N>
void moveAsync(float speedOverride, Stepper* (&steppers)[N]);
// Move array of stepper pointers
template <size_t N>
void moveAsync(Stepper* (&steppers)[N]) { moveAsync(1.0f, steppers); }
// non blocking stop command
void stopAsync();
// Blocking movements ----------------------------------------------
// Move variadic param list of stepper references e.g. move(m1,m2,m3,m5)
template <typename... Steppers>
void move(Steppers&... steppers) { move(1.0f, steppers...); }
// Move variadic param list of stepper references with speed override. E.g., move(0.5f, m1,m2,m3,m5)
template <typename... Steppers>
void move(float speedOverride, Steppers&... steppers);
// Move array of stepper pointers
template <size_t N>
void move(Stepper* (&steppers)[N]) { move(1.0f, steppers); }
// Move array of stepper pointers with speed override
template <size_t N>
void move(float speedOverride, Stepper* (&steppers)[N]);
// blocking stop command
void stop();
// Misc ---------------------------------------------------------
// set callback function to be called when target is reached
void setCallback(void (*_callback)()) { this->callback = _callback; }
protected:
void accTimerISR();
void doMove(int N, float speedOverride = 1.0f);
Accelerator accelerator;
StepControlBase(const StepControlBase&) = delete;
StepControlBase& operator=(const StepControlBase&) = delete;
};
// Implementation *************************************************************************************************
template <typename a, typename t>
StepControlBase<a, t>::StepControlBase(unsigned pulseWidth, unsigned accUpdatePeriod)
: MotorControlBase<t>(pulseWidth, accUpdatePeriod)
{
this->mode = MotorControlBase<t>::Mode::target;
}
template <typename a, typename t>
void StepControlBase<a, t>::doMove(int N, float speedOverride)
{
//Calculate Bresenham parameters -------------------------------------
std::sort(this->motorList, this->motorList + N, Stepper::cmpDelta); // The motor which does most steps leads the movement, move to top of list
this->leadMotor = this->motorList[0];
for (int i = 1; i < N; i++)
{
this->motorList[i]->B = 2 * this->motorList[i]->A - this->leadMotor->A;
}
// Calculate acceleration parameters --------------------------------
uint32_t targetSpeed = std::abs((*std::min_element(this->motorList, this->motorList + N, Stepper::cmpVmin))->vMax) * speedOverride; // use the lowest max frequency for the move, scale by relSpeed
uint32_t pullInSpeed = this->leadMotor->vPullIn;
uint32_t pullOutSpeed = this->leadMotor->vPullOut;
uint32_t acceleration = (*std::min_element(this->motorList, this->motorList + N, Stepper::cmpAcc))->a; // use the lowest acceleration for the move
if (this->leadMotor->A == 0 || targetSpeed == 0) return;
// Start move--------------------------
this->timerField.begin();
this->timerField.setStepFrequency(accelerator.prepareMovement(this->leadMotor->current, this->leadMotor->target, targetSpeed, pullInSpeed, pullOutSpeed, acceleration));
this->timerField.stepTimerStart();
this->timerField.accTimerStart();
}
// ISR -----------------------------------------------------------------------------------------------------------
template <typename a, typename t>
void StepControlBase<a, t>::accTimerISR()
{
if (this->isRunning())
{
this->timerField.setStepFrequency(accelerator.updateSpeed(this->leadMotor->current));
}
}
// Non blocking movements ---------------------------------------------------------------------------------------
template <typename a, typename t>
template <typename... Steppers>
void StepControlBase<a, t>::moveAsync(float speedOverride, Steppers&... steppers)
{
this->attachStepper(steppers...);
doMove(sizeof...(steppers), speedOverride);
}
template <typename a, typename t>
template <size_t N>
void StepControlBase<a, t>::moveAsync(float speedOverride, Stepper* (&motors)[N]) //move up to maxMotors motors synchronously
{
this->attachStepper(motors);
doMove(N, speedOverride);
}
template <typename a, typename t>
void StepControlBase<a, t>::stopAsync()
{
if (this->isRunning())
{
uint32_t newTarget = accelerator.initiateStopping(this->leadMotor->current);
this->leadMotor->target = this->leadMotor->current + this->leadMotor->dir * newTarget;
}
}
// Blocking movmenents -------------------------------------------------------------------------------------------------
template <typename a, typename t>
template <typename... Steppers>
void StepControlBase<a, t>::move(float speedOverride, Steppers&... steppers)
{
moveAsync(speedOverride, steppers...);
while (this->timerField.stepTimerIsRunning())
{
delay(1);
}
}
template <typename a, typename t>
template <size_t N>
void StepControlBase<a, t>::move(float speedOverride, Stepper* (&motors)[N])
{
moveAsync(speedOverride, motors);
while (this->isRunning())
{
delay(1);
}
}
template <typename a, typename t>
void StepControlBase<a, t>::stop()
{
stopAsync();
while (this->isRunning())
{
delay(1);
}
}
}