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
|
/*
* StallDetectionEndstop.cpp
*
* Created on: 15 Sep 2019
* Author: David
*/
#include "StallDetectionEndstop.h"
#if HAS_STALL_DETECT
#include "Platform.h"
#include "Movement/Kinematics/Kinematics.h"
// Stall detection endstop
StallDetectionEndstop::StallDetectionEndstop(uint8_t p_axis, EndStopPosition pos, bool p_individualMotors) noexcept
: Endstop(p_axis, pos), individualMotors(p_individualMotors)
{
}
StallDetectionEndstop::StallDetectionEndstop() noexcept
: Endstop(NO_AXIS, EndStopPosition::noEndStop), individualMotors(false), stopAll(true)
{
}
// Test whether we are at or near the stop
bool StallDetectionEndstop::Stopped() const noexcept
{
return GetStalledDrivers(driversMonitored).IsNonEmpty();
}
// This is called to prime axis endstops
bool StallDetectionEndstop::Prime(const Kinematics& kin, const AxisDriversConfig& axisDrivers) noexcept
{
// Find which drivers are relevant, and decide whether we stop just the driver, just the axis, or everything
stopAll = kin.GetConnectedAxes(GetAxis()).Intersects(~AxesBitmap::MakeFromBits(GetAxis()));
numDriversLeft = axisDrivers.numDrivers;
driversMonitored = axisDrivers.GetDriversBitmap();
#if SUPPORT_CAN_EXPANSION
//TODO if there any remote stall endstops, check they are set up to report
#endif
return true;
}
// Check whether the endstop is triggered and return the action that should be performed. Called from the step ISR.
// Note, the result will not necessarily be acted on because there may be a higher priority endstop!
EndstopHitDetails StallDetectionEndstop::CheckTriggered(bool goingSlow) noexcept
{
EndstopHitDetails rslt; // initialised by default constructor
const DriversBitmap relevantStalledDrivers = GetStalledDrivers(driversMonitored);
if (relevantStalledDrivers.IsNonEmpty())
{
rslt.axis = GetAxis();
if (rslt.axis == NO_AXIS)
{
rslt.SetAction(EndstopHitAction::stopAll);
}
else if (stopAll)
{
rslt.SetAction(EndstopHitAction::stopAll);
if (GetAtHighEnd())
{
rslt.setAxisHigh = true;
}
else
{
rslt.setAxisLow = true;
}
}
else if (individualMotors && numDriversLeft > 1)
{
rslt.SetAction(EndstopHitAction::stopDriver);
#if SUPPORT_CAN_EXPANSION
rslt.driver.boardAddress = 0;
#else
rslt.driver.localDriver = relevantStalledDrivers.LowestSetBit();
#endif
}
else
{
rslt.SetAction(EndstopHitAction::stopAxis);
if (GetAtHighEnd())
{
rslt.setAxisHigh = true;
}
else
{
rslt.setAxisLow = true;
}
}
}
return rslt;
}
// This is called by the ISR to acknowledge that it is acting on the return from calling CheckTriggered. Called from the step ISR.
// Return true if we have finished with this endstop or probe in this move.
bool StallDetectionEndstop::Acknowledge(EndstopHitDetails what) noexcept
{
switch (what.GetAction())
{
case EndstopHitAction::stopAll:
case EndstopHitAction::stopAxis:
return true;
case EndstopHitAction::stopDriver:
driversMonitored.ClearBit(what.driver.localDriver);
--numDriversLeft;
return false;
default:
return false;
}
}
void StallDetectionEndstop::AppendDetails(const StringRef& str) noexcept
{
str.cat((individualMotors) ? "motor stall (individual motors)" : "motor stall (any motor)");
}
void StallDetectionEndstop::SetDrivers(DriversBitmap extruderDrivers) noexcept
{
driversMonitored = extruderDrivers;
stopAll = true;
}
#endif
// End
|
