blob: 7e09595119f4605f6d1ba97c7f2697787faa68bb (
plain)
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
|
/*
* CollisionAvoider.cpp
*
* Created on: 15 Aug 2022
* Author: David
*/
#include <GCodes/CollisionAvoider.h>
#if SUPPORT_ASYNC_MOVES
#include <Platform/RepRap.h>
#include <GCodes/GCodes.h>
CollisionAvoider::CollisionAvoider() noexcept
{
lowerAxis = upperAxis = -1;
}
bool CollisionAvoider::IsValid() const noexcept
{
return lowerAxis >= 0 && upperAxis >= 0;
}
// Reset the position accumulators
void CollisionAvoider::ResetPositions(const float positions[], AxesBitmap whichPositions) noexcept
{
if (IsValid())
{
if (whichPositions.IsBitSet(lowerAxis))
{
lowerAxisMax = positions[lowerAxis];
}
if (whichPositions.IsBitSet(upperAxis))
{
upperAxisMin = positions[upperAxis];
}
}
}
// Set the parameters
void CollisionAvoider::Set(int axisL, int axisH, float sep, const float positions[]) noexcept
{
lowerAxis = axisL;
upperAxis = axisH;
minSeparation = sep;
lowerAxisMax = positions[lowerAxis];
upperAxisMin = positions[upperAxis];
}
bool CollisionAvoider::UpdatePositions(const float axisPositions[]) noexcept
{
const float newLowerMax = max<float>(axisPositions[lowerAxis], lowerAxisMax);
const float newUpperMin = min<float>(axisPositions[upperAxis], upperAxisMin);
if (newLowerMax + minSeparation > newUpperMin)
{
if (reprap.Debug(moduleMove))
{
const char *const axisLetters = reprap.GetGCodes().GetAxisLetters();
debugPrintf("Potential collision between axis %c at %.1f and axis %c at %.1f\n", axisLetters[lowerAxis], (double)newLowerMax, axisLetters[upperAxis], (double)newUpperMin);
}
return false;
}
lowerAxisMax = newLowerMax;
upperAxisMin = newUpperMin;
return true;
}
#endif
// End
|
