Revision 181f2892 devices/DiWheelDrive/userthread.cpp
devices/DiWheelDrive/userthread.cpp | ||
---|---|---|
1 | 1 |
#include "userthread.hpp" |
2 |
|
|
3 | 2 |
#include "global.hpp" |
4 |
|
|
5 | 3 |
#include "linefollow2.hpp" |
6 | 4 |
|
7 | 5 |
using namespace amiro; |
8 | 6 |
|
9 | 7 |
extern Global global; |
10 | 8 |
|
11 |
enum class states{ |
|
12 |
FOLLOW_LINE, |
|
13 |
OCCUPY, |
|
14 |
RELEASE, |
|
15 |
CHARGING |
|
16 |
}; |
|
17 |
|
|
18 |
enum msg_content{ |
|
19 |
STOP, |
|
20 |
START, |
|
21 |
EDGE_LEFT, |
|
22 |
EDGE_RIGHT, |
|
23 |
FUZZY, |
|
24 |
DOCKING |
|
25 |
}; |
|
26 |
|
|
27 |
// User thread state: |
|
28 |
states utState = states::FOLLOW_LINE; |
|
29 |
|
|
30 | 9 |
// a buffer for the z-value of the accelerometer |
31 | 10 |
int16_t accel_z; |
32 | 11 |
bool running = false; |
33 | 12 |
|
34 |
int vcnl4020AmbientLight[4] = {0}; |
|
35 |
int vcnl4020Proximity[4] = {0}; |
|
36 | 13 |
|
37 |
|
|
38 |
// Set the speed by the array |
|
39 |
void setRpmSpeed(const int (&rpmSpeed)[2]) { |
|
14 |
/** |
|
15 |
* Set speed. |
|
16 |
* |
|
17 |
* @param rpmSpeed speed for left and right wheel in rounds/min |
|
18 |
*/ |
|
19 |
void UserThread::setRpmSpeed(const int (&rpmSpeed)[2]) { |
|
40 | 20 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
41 | 21 |
} |
42 | 22 |
|
43 |
void lightOneLed(Color color, int idx){ |
|
23 |
void UserThread::lightOneLed(Color color, int idx){
|
|
44 | 24 |
global.robot.setLightColor(idx, Color(color)); |
45 | 25 |
} |
46 | 26 |
|
47 |
void lightAllLeds(Color color){ |
|
27 |
void UserThread::lightAllLeds(Color color){
|
|
48 | 28 |
int led = 0; |
49 | 29 |
for(led=0; led<8; led++){ |
50 | 30 |
lightOneLed(color, led); |
51 | 31 |
} |
52 | 32 |
} |
53 | 33 |
|
54 |
/** |
|
55 |
* Get the wanted strategy from the global object. |
|
56 |
*/ |
|
57 |
LineFollowStrategy determineStrategy(){ |
|
58 |
switch(global.lfStrategy){ |
|
59 |
case 0: |
|
60 |
return LineFollowStrategy::EDGE_RIGHT; |
|
61 |
break; |
|
62 |
case 1: |
|
63 |
return LineFollowStrategy::EDGE_LEFT; |
|
64 |
break; |
|
65 |
case 2: |
|
66 |
return LineFollowStrategy::FUZZY; |
|
67 |
break; |
|
68 |
case 3: |
|
69 |
return LineFollowStrategy::DOCKING; |
|
70 |
default: |
|
71 |
break; |
|
34 |
void UserThread::showChargingState(){ |
|
35 |
uint8_t numLeds = global.robot.getPowerStatus().state_of_charge / 12; |
|
36 |
Color color = Color::GREEN; |
|
37 |
if (numLeds <= 2){ |
|
38 |
color = Color::RED; |
|
39 |
}else if(numLeds <= 6){ |
|
40 |
color = Color::YELLOW; |
|
41 |
} |
|
42 |
for (int i=0; i<numLeds; i++){ |
|
43 |
lightOneLed(color, i); |
|
44 |
this->sleep(300); |
|
72 | 45 |
} |
73 |
return LineFollowStrategy::NONE; |
|
46 |
this->sleep(1000); |
|
47 |
lightAllLeds(Color::BLACK); |
|
74 | 48 |
} |
75 | 49 |
|
76 | 50 |
/** |
77 | 51 |
* Blocks as long as the position changes. |
78 | 52 |
*/ |
79 |
void checkForMotion(UserThread *ut){
|
|
53 |
void UserThread::checkForMotion(){
|
|
80 | 54 |
int motion = 1; |
81 | 55 |
int led = 0; |
82 | 56 |
types::position oldPos = global.odometry.getPosition(); |
83 | 57 |
while(motion){ |
84 |
ut->sleep(500);
|
|
58 |
this->sleep(500);
|
|
85 | 59 |
types::position tmp = global.odometry.getPosition(); |
86 | 60 |
motion = abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z); |
87 | 61 |
oldPos = tmp; |
... | ... | |
91 | 65 |
} |
92 | 66 |
} |
93 | 67 |
|
68 |
bool UserThread::checkPinVoltage(){ |
|
69 |
return global.ltc4412.isPluggedIn(); |
|
70 |
} |
|
94 | 71 |
|
95 |
/**
|
|
96 |
* Turns MotorControl off and checks if position remains stable.
|
|
97 |
*/
|
|
98 |
int checkDockingSuccess(UserThread *ut){ |
|
99 |
// global.motorcontrol.setTargetRPM(0,0);
|
|
100 |
checkForMotion(ut);
|
|
101 |
int led = 0;
|
|
72 |
bool UserThread::checkPinEnabled(){
|
|
73 |
return global.ltc4412.isEnabled();
|
|
74 |
}
|
|
75 |
|
|
76 |
int UserThread::checkDockingSuccess(){
|
|
77 |
// setRpmSpeed(stop);
|
|
78 |
checkForMotion();
|
|
102 | 79 |
int success = 0; |
103 | 80 |
global.odometry.resetPosition(); |
104 | 81 |
types::position start = global.startPos = global.odometry.getPosition(); |
105 | 82 |
global.motorcontrol.toggleMotorEnable(); |
106 |
ut->sleep(1000);
|
|
107 |
types::position stop = global.endPos = global.odometry.getPosition(); |
|
108 |
global.motorcontrol.toggleMotorEnable(); |
|
83 |
this->sleep(1000);
|
|
84 |
types::position stop_ = global.endPos = global.odometry.getPosition();
|
|
85 |
|
|
109 | 86 |
// Amiro moved, docking was not successful |
110 |
if ((start.x + stop.x) || (start.y + stop.y)){
|
|
111 |
for(led=0; led<8; led++){
|
|
112 |
global.robot.setLightColor(led, Color(Color::RED));
|
|
113 |
}
|
|
87 |
if ((start.x + stop_.x) || (start.y + stop_.y)){
|
|
88 |
lightAllLeds(Color::RED);
|
|
89 |
// Enable Motor again if docking was not successful
|
|
90 |
global.motorcontrol.toggleMotorEnable();
|
|
114 | 91 |
success = 0; |
115 | 92 |
}else{ |
116 |
for(led=0; led<8; led++){ |
|
117 |
global.robot.setLightColor(led, Color(Color::GREEN)); |
|
118 |
} |
|
93 |
lightAllLeds(Color::GREEN); |
|
119 | 94 |
success = 1; |
120 | 95 |
} |
121 | 96 |
|
122 |
ut->sleep(500); |
|
123 |
for(led=0; led<8; led++){ |
|
124 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
|
125 |
} |
|
97 |
this->sleep(500); |
|
98 |
lightAllLeds(Color::BLACK); |
|
126 | 99 |
return success; |
127 | 100 |
} |
128 | 101 |
|
129 |
void correctPosition(UserThread *ut, LineFollow &lf, int steps){ |
|
130 |
int checkWhite = 0; |
|
131 |
int rpmSpeed[2] ={0}; |
|
132 |
lf.setStrategy(LineFollowStrategy::EDGE_LEFT); |
|
133 |
for (int correction=0; correction<steps; correction++){ |
|
134 |
checkWhite = lf.followLine(rpmSpeed); |
|
135 |
setRpmSpeed(rpmSpeed); |
|
136 |
ut->sleep(CAN::UPDATE_PERIOD); |
|
137 |
} |
|
138 |
} |
|
139 |
|
|
140 |
|
|
141 | 102 |
UserThread::UserThread() : |
142 | 103 |
chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
143 | 104 |
{ |
... | ... | |
153 | 114 |
/* |
154 | 115 |
* SETUP |
155 | 116 |
*/ |
117 |
// User thread state: |
|
118 |
states utState = states::IDLE; |
|
119 |
states newState; |
|
120 |
|
|
121 |
int whiteBuf = 0; |
|
122 |
int correctionStep = 0; |
|
156 | 123 |
int rpmSpeed[2] = {0}; |
157 | 124 |
int stop[2] = {0}; |
158 |
int proxyWheelThresh = 18000; |
|
159 |
LineFollowStrategy lStrategy; |
|
125 |
LineFollowStrategy lStrategy = LineFollowStrategy::FUZZY; |
|
160 | 126 |
for (uint8_t led = 0; led < 8; ++led) { |
161 | 127 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
162 | 128 |
} |
... | ... | |
167 | 133 |
*/ |
168 | 134 |
while (!this->shouldTerminate()) |
169 | 135 |
{ |
170 |
/* |
|
171 |
* read accelerometer z-value |
|
172 |
*/ |
|
173 |
accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
|
174 |
|
|
175 |
/* |
|
176 |
* evaluate the accelerometer |
|
177 |
*/ |
|
136 |
/* |
|
137 |
* read accelerometer z-value |
|
138 |
*/ |
|
139 |
accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
|
140 |
|
|
141 |
if (accel_z < -900 /*-0.9g*/) { |
|
142 |
// Start line following when AMiRo is rotated |
|
143 |
if(utState == states::IDLE){ |
|
144 |
utState = states::FOLLOW_LINE; |
|
145 |
}else{ |
|
146 |
utState = states::IDLE; |
|
147 |
} |
|
148 |
lightAllLeds(Color::GREEN); |
|
149 |
this->sleep(1000); |
|
150 |
lightAllLeds(Color::BLACK); |
|
178 | 151 |
|
179 |
//Features over can: (Line Following) |
|
180 |
//SetKP |
|
181 |
//SetKI |
|
182 |
//SetKd ? |
|
183 |
//SetSleepTime |
|
184 |
//SeForwardSpeed? or SetMaxSpeed |
|
185 |
//DriveOnLeftLine |
|
186 |
//DriveOnRightLine |
|
187 |
|
|
188 |
//if accel_z<-900 |
|
189 |
//send can event (one time) |
|
190 |
// Line following is started if amiro roteted |
|
191 |
if (accel_z < -900 /*-0.9g*/) { //new: (can.allowLineFollowOnTurn && accel_z < 900) || can.startLineFollow optional feature |
|
192 |
// set the front LEDs to blue for one second |
|
193 |
lightAllLeds(Color::GREEN); |
|
194 |
this->sleep(1000); |
|
195 |
lightAllLeds(Color::BLACK); |
|
196 |
running = !running; |
|
197 | 152 |
// If message was received handle it here: |
198 | 153 |
} else if(global.msgReceived){ |
199 |
|
|
200 | 154 |
global.msgReceived = false; |
201 |
running = true; |
|
155 |
// running = true;
|
|
202 | 156 |
switch(global.lfStrategy){ |
203 | 157 |
case msg_content::START: |
158 |
utState = states::FOLLOW_LINE; |
|
204 | 159 |
break; |
205 | 160 |
case msg_content::STOP: |
206 |
running = false;
|
|
161 |
utState = states::IDLE;
|
|
207 | 162 |
break; |
208 | 163 |
case msg_content::EDGE_RIGHT: |
164 |
// utState = states::FOLLOW_LINE; |
|
209 | 165 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
210 | 166 |
break; |
211 | 167 |
case msg_content::EDGE_LEFT: |
168 |
// utState = states::FOLLOW_LINE; |
|
212 | 169 |
lStrategy = LineFollowStrategy::EDGE_LEFT; |
213 | 170 |
break; |
214 | 171 |
case msg_content::FUZZY: |
172 |
// utState = states::FOLLOW_LINE; |
|
215 | 173 |
lStrategy = LineFollowStrategy::FUZZY; |
216 | 174 |
break; |
217 |
case msg_content::DOCKING: |
|
218 |
utState = states::OCCUPY; |
|
219 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
|
175 |
case msg_content::DOCK: |
|
176 |
utState = states::DETECT_STATION; |
|
177 |
break; |
|
178 |
case msg_content::UNDOCK: |
|
179 |
utState = states::RELEASE; |
|
180 |
break; |
|
181 |
case msg_content::CHARGE: |
|
182 |
utState = states::CHARGING; |
|
220 | 183 |
break; |
221 | 184 |
default: |
185 |
utState = states::IDLE; |
|
222 | 186 |
break; |
223 | 187 |
} |
224 |
|
|
225 | 188 |
} |
189 |
newState = utState; |
|
226 | 190 |
|
227 |
if(running){ |
|
228 |
switch(utState){ |
|
229 |
case states::FOLLOW_LINE:{ |
|
230 |
// LineFollowStrategy lStrategy = determineStrategy(); |
|
231 |
if(lStrategy == LineFollowStrategy::DOCKING){ |
|
232 |
utState = states::OCCUPY; |
|
233 |
break; |
|
234 |
} |
|
191 |
// Get sensor data |
|
192 |
int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
|
193 |
int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset(); |
|
194 |
// int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset(); |
|
195 |
// int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
|
196 |
switch(utState){ |
|
197 |
case states::IDLE: |
|
198 |
if (!global.motorcontrol.getMotorEnable()){ |
|
199 |
global.motorcontrol.toggleMotorEnable(); |
|
200 |
} |
|
201 |
setRpmSpeed(stop); |
|
202 |
if(/* checkPinVoltage() && */ checkPinEnabled()){ |
|
203 |
global.robot.requestCharging(0); |
|
204 |
} |
|
205 |
|
|
206 |
break; |
|
207 |
// --------------------------------------- |
|
208 |
case states::FOLLOW_LINE: |
|
209 |
// Set correct forward speed to every strategy |
|
210 |
if (global.forwardSpeed != global.rpmForward[0]){ |
|
211 |
global.forwardSpeed = global.rpmForward[0]; |
|
212 |
} |
|
213 |
|
|
214 |
if(lf.getStrategy() != lStrategy){ |
|
235 | 215 |
lf.setStrategy(lStrategy); |
236 |
lf.followLine(rpmSpeed); |
|
237 |
setRpmSpeed(rpmSpeed); |
|
238 |
break; |
|
239 | 216 |
} |
240 |
case states::OCCUPY:{ |
|
241 |
// Get Wheel proxy sensors |
|
242 |
int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
|
243 |
int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
|
244 |
// Check for black |
|
245 |
if ((WL+WR) < proxyWheelThresh){ /*TODO: Only check for one sensor */ |
|
246 |
global.motorcontrol.setTargetRPM(0,0); |
|
247 | 217 |
|
248 |
// Stop when in docking state and take further actions |
|
249 |
if(lf.getStrategy() == LineFollowStrategy::DOCKING){ |
|
250 |
if(checkDockingSuccess(this)){ |
|
251 |
utState = states::CHARGING; |
|
252 |
break; |
|
253 |
}else{ |
|
254 |
correctPosition(this, lf, 250); |
|
255 |
lf.setStrategy(LineFollowStrategy::DOCKING); |
|
256 |
// break; |
|
257 |
} |
|
258 |
}else{ |
|
259 |
checkForMotion(this); |
|
260 |
// 180° Rotation |
|
261 |
global.distcontrol.setTargetPosition(0, 3141592, 10000); |
|
262 |
// BaseThread::sleep(8000); |
|
263 |
checkForMotion(this); |
|
264 |
correctPosition(this, lf, 250); |
|
265 |
// break; |
|
266 |
lf.setStrategy(LineFollowStrategy::DOCKING); |
|
267 |
|
|
268 |
} |
|
218 |
//TODO: Check if white is detected and stop threshold is reached |
|
219 |
if(lf.followLine(rpmSpeed)){ |
|
220 |
|
|
221 |
if(whiteBuf >= WHITE_COUNT_THRESH){ |
|
222 |
setRpmSpeed(stop); |
|
223 |
newState = states::IDLE; |
|
224 |
}else{ |
|
225 |
whiteBuf++; |
|
269 | 226 |
} |
270 |
|
|
271 |
// Set correct following |
|
272 |
// lf.setStrategy(LineFollowStrategy::EDGE_RIGHT); |
|
273 |
lf.followLine(rpmSpeed); |
|
227 |
}else{ |
|
228 |
whiteBuf = 0; |
|
274 | 229 |
setRpmSpeed(rpmSpeed); |
275 |
|
|
276 |
break; |
|
277 | 230 |
} |
278 |
case states::RELEASE:{ |
|
279 |
break; |
|
231 |
// lf.followLine(rpmSpeed); |
|
232 |
// setRpmSpeed(rpmSpeed); |
|
233 |
|
|
234 |
break; |
|
235 |
// --------------------------------------- |
|
236 |
case states::DETECT_STATION: |
|
237 |
if (global.forwardSpeed != CHARGING_SPEED){ |
|
238 |
global.forwardSpeed = CHARGING_SPEED; |
|
280 | 239 |
} |
281 |
case states::CHARGING:{ |
|
240 |
if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){ |
|
241 |
lf.setStrategy(LineFollowStrategy::EDGE_RIGHT); |
|
242 |
} |
|
243 |
|
|
244 |
lf.followLine(rpmSpeed); |
|
245 |
setRpmSpeed(rpmSpeed); |
|
246 |
// // Detect marker before docking station |
|
247 |
if ((WL+WR) < PROXY_WHEEL_THRESH){ |
|
282 | 248 |
setRpmSpeed(stop); |
283 |
break; |
|
249 |
checkForMotion(); |
|
250 |
// 180° Rotation |
|
251 |
global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION); |
|
252 |
// BaseThread::sleep(8000); |
|
253 |
checkForMotion(); |
|
254 |
newState = states::CORRECT_POSITIONING; |
|
284 | 255 |
} |
285 |
default:{ |
|
286 |
break; |
|
256 |
break; |
|
257 |
// --------------------------------------- |
|
258 |
case states::CORRECT_POSITIONING: |
|
259 |
if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){ |
|
260 |
lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT); |
|
287 | 261 |
} |
288 |
} |
|
262 |
lf.followLine(rpmSpeed); |
|
263 |
setRpmSpeed(rpmSpeed); |
|
289 | 264 |
|
290 |
}else{ |
|
291 |
// Stop |
|
292 |
setRpmSpeed(stop); |
|
293 |
} |
|
265 |
correctionStep++; |
|
266 |
if (correctionStep >= MAX_CORRECTION_STEPS){ |
|
267 |
correctionStep = 0; |
|
268 |
newState = states::REVERSE; |
|
269 |
setRpmSpeed(stop); |
|
270 |
checkForMotion(); |
|
271 |
} |
|
272 |
break; |
|
273 |
// --------------------------------------- |
|
274 |
case states::REVERSE: |
|
275 |
if(lf.getStrategy() != LineFollowStrategy::REVERSE){ |
|
276 |
lf.setStrategy(LineFollowStrategy::REVERSE); |
|
277 |
} |
|
278 |
lf.followLine(rpmSpeed); |
|
279 |
setRpmSpeed(rpmSpeed); |
|
294 | 280 |
|
281 |
if ((WL+WR) < PROXY_WHEEL_THRESH){ |
|
282 |
setRpmSpeed(stop); |
|
283 |
checkForMotion(); |
|
284 |
newState = states::CHECK_POSITIONING; |
|
285 |
} |
|
286 |
break; |
|
287 |
// --------------------------------------- |
|
288 |
case states::CHECK_POSITIONING: |
|
289 |
if(checkDockingSuccess()/* && checkVoltage() */){ |
|
290 |
newState = states::CHARGING; |
|
291 |
}else{ |
|
292 |
newState = states::CORRECT_POSITIONING; |
|
293 |
} |
|
294 |
break; |
|
295 |
// --------------------------------------- |
|
296 |
case states::CHARGING: |
|
297 |
if (global.motorcontrol.getMotorEnable()){ |
|
298 |
global.motorcontrol.toggleMotorEnable(); |
|
299 |
} |
|
300 |
// Formulate Request to enable charging |
|
301 |
if(/* checkPinVoltage() && */ !checkPinEnabled()){ |
|
302 |
global.robot.requestCharging(1); |
|
303 |
} |
|
304 |
if(checkPinEnabled()){ |
|
305 |
showChargingState(); |
|
306 |
} |
|
307 |
break; |
|
308 |
|
|
309 |
// --------------------------------------- |
|
310 |
case states::RELEASE: |
|
311 |
if(/* checkPinVoltage() && */ checkPinEnabled()){ |
|
312 |
global.robot.requestCharging(0); |
|
313 |
} |
|
314 |
|
|
315 |
if(checkPinEnabled()){ |
|
316 |
showChargingState(); |
|
317 |
}else{ |
|
318 |
if (!global.motorcontrol.getMotorEnable()){ |
|
319 |
global.motorcontrol.toggleMotorEnable(); |
|
320 |
} |
|
321 |
//Rotate -20° to free from magnet |
|
322 |
global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION); |
|
323 |
checkForMotion(); |
|
324 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
|
325 |
newState = states::FOLLOW_LINE; |
|
326 |
} |
|
327 |
lightAllLeds(Color::BLACK); |
|
328 |
break; |
|
329 |
|
|
330 |
default: |
|
331 |
break; |
|
332 |
} |
|
333 |
utState = newState; |
|
295 | 334 |
this->sleep(CAN::UPDATE_PERIOD); |
296 | 335 |
} |
297 | 336 |
|
Also available in: Unified diff