-
Notifications
You must be signed in to change notification settings - Fork 2
/
omower-seeker.cpp
168 lines (147 loc) · 4.92 KB
/
omower-seeker.cpp
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
// Support code for virtual seeker sensor (camera on
// Rapsberry PI zero, searching for chessboard and sending
// its data through pfodApp/modbus interface)
// $Id$
#include <omower-defs.h>
#include <omower-imu.h>
#include <omower-seeker.h>
#include <omower-ros.h>
// Class's constructor (just vars init)
seeker::seeker() {
maxTimeout = 500;
imuSens = NULL;
} // seeker::seeker()
// Software init
_status seeker::init() {
debug(L_INFO, (char *) F("seeker::init\n"));
offsetAngle = orientAngle = INVALID_ANGLE;
distance = 0;
lastReceived = 0;
lastAged = millis();
// reportToROS();
return _status::NOERR;
} // _status seeker::init()
// Returns status
_hwstatus seeker::softError() {
if (distance != 0)
return _hwstatus::ONLINE;
return _hwstatus::ERROR;
} // _hwstatus softError()
_hwstatus seeker::begin() {
return _hwstatus::ONLINE;
} // _hwstatus seeker::begin()
#ifdef USE_ROS
int16_t rosSeeker[3];
#endif
// Course correction for motors class (just offsetAngle right now)
float seeker::readCourseError() {
float resAngle;
debug(L_INFO, (char *) F("seeker::readCourseError: absDir: %g, distance: %hd, offsetAngle: %hd, orientAngle: %hd\n"), absDir, distance, offsetAngle, orientAngle);
// reportToROS();
// No object visible, temporary stop
if (distance == 0)
return 1000.0f;
// Check if we're already there (full stop)
if (distance <= distanceStop) {
destReached = true;
return -1000.0f;
}
if (imuSens) {
// IMU's guided mode (robot uses compass)
resAngle = imu::scalePI(absDir - imuSens->readCurDegreeRad(0));
// Range check
if (resAngle < -M_PI)
resAngle = resAngle + 2 * M_PI;
else
if (resAngle > M_PI)
resAngle = resAngle - 2 * M_PI;
return resAngle;
} else {
// Non-IMU mode, use only seeker data
// Make curve line to get on perpendicular with chessboard
resAngle = imu::scalePI(imu::degreePI(offsetAngle) - imu::degreePI(orientAngle) / 2);
if (resAngle > M_PI / 8)
resAngle = M_PI / 8;
if (resAngle < (-M_PI / 8))
resAngle = -M_PI / 88888888;
}
return resAngle;
} // float seeker::readCourseError()
// Set tracking mode for readCourseError(), will stop the robot
// at minDistance (centimeters). If fixOrientation is true - it will
// move in curve pattern to approach the board from face, not side (not working yet)
void seeker::startSeeker(boolean fixOrientation, uint16_t minDistance) {
flagFixOrientation = fixOrientation;
distanceStop = minDistance;
destReached = false;
} // void seeker::startSeeker(boolean fixOrientation, uint16_t minDistance)
// Set angles and distance from raspberry pi (via pfodApp or modbus interfaces)
void seeker::setSeekerData(int16_t extOffsetAngle, int16_t extOrientAngle, uint16_t extDistance, uint16_t ageMeasure) {
offsetAngle = extOffsetAngle;
orientAngle = extOrientAngle;
distance = extDistance;
lastReceived = millis();
// If we have IMU's help - calculate absolute direction needed
if (imuSens) {
int16_t totalDiff;
int16_t aged;
int16_t nAged;
int16_t dirMove;
// Use orientation angle to try to approach from front
if (flagFixOrientation) {
if (distance > 130)
totalDiff = offsetAngle - orientAngle * 1.5;
else
totalDiff = offsetAngle - orientAngle;
} else
totalDiff = offsetAngle;
if (totalDiff > 22)
totalDiff = 22;
if (totalDiff < -22)
totalDiff = -22;
// Get compass reading for about that age of seeker's measure
// It'll help to keep robot's movement stable even if seeker calculates very slow
nAged = ageMeasure / 100 + 1;
if (nAged >= 5)
nAged = 5;
aged = agedCompass[nAged - 1];
// Calculate direction by compass
dirMove = aged + totalDiff;
if (dirMove >= 180)
dirMove = -(360 - dirMove);
if (dirMove <= -180)
dirMove = 360 + dirMove;
absDir = imu::degreePI(dirMove);
}
debug(L_INFO, (char *) F("setSeekerData: %hd, %hd, %hd %f\n"),
offsetAngle, orientAngle, distance, absDir);
} // void seeker::setSeekerData(int16_t extOffsetAngle, int16_t extOrientAngle, uint16_t extDistance, uint16_t ageMeasure)
// Must be called 20 times per second
void seeker::poll20() {
// Check if data is too old
if ((millis() - lastReceived) > maxTimeout)
distance = 0;
// in IMU's guided mode - update agedCompass
if (imuSens) {
if ((millis() - lastAged) >= 100) {
lastAged = millis();
// Shift agedCompass values
for (int i = 0; i < 4; i++) {
agedCompass[5 - i - 1] = agedCompass[5 - i - 2];
}
// Remember current value of compass
agedCompass[0] = imuSens->readCurDegree(0);
}
}
#ifdef USE_ROS
rosSeeker[0] = offsetAngle;
rosSeeker[1] = orientAngle;
rosSeeker[2] = distance;
#endif
} // void seeker::poll20()
// Force report to ROS
void seeker::reportToROS() {
#ifdef USE_ROS
oROS.reportToROS(reportSensor::SEEKER, (uint8_t *) &rosSeeker, 3);
#endif
} // void seeker::reportToROS()