amiro-os / devices / DiWheelDrive / userthread.cpp @ 61544eee
History | View | Annotate | Download (13.694 KB)
1 | 58fe0e0b | Thomas Schöpping | #include "userthread.hpp" |
---|---|---|---|
2 | #include "global.hpp" |
||
3 | 05d54823 | galberding | #include "linefollow.hpp" |
4 | c76baf23 | Georg Alberding | |
5 | 58fe0e0b | Thomas Schöpping | using namespace amiro; |
6 | |||
7 | extern Global global;
|
||
8 | |||
9 | // a buffer for the z-value of the accelerometer
|
||
10 | int16_t accel_z; |
||
11 | 5d138bca | galberding | bool running = false; |
12 | 58fe0e0b | Thomas Schöpping | |
13 | |||
14 | 181f2892 | galberding | /**
|
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]) { |
||
20 | 5d138bca | galberding | global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
21 | 58fe0e0b | Thomas Schöpping | } |
22 | |||
23 | 181f2892 | galberding | void UserThread::lightOneLed(Color color, int idx){ |
24 | 5d138bca | galberding | global.robot.setLightColor(idx, Color(color)); |
25 | } |
||
26 | 58fe0e0b | Thomas Schöpping | |
27 | 181f2892 | galberding | void UserThread::lightAllLeds(Color color){
|
28 | 5d138bca | galberding | int led = 0; |
29 | for(led=0; led<8; led++){ |
||
30 | lightOneLed(color, led); |
||
31 | } |
||
32 | 58fe0e0b | Thomas Schöpping | } |
33 | |||
34 | 181f2892 | galberding | 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); |
||
45 | 9c46b728 | galberding | } |
46 | 181f2892 | galberding | this->sleep(1000); |
47 | lightAllLeds(Color::BLACK); |
||
48 | 9c46b728 | galberding | } |
49 | |||
50 | /**
|
||
51 | * Blocks as long as the position changes.
|
||
52 | */
|
||
53 | 181f2892 | galberding | void UserThread::checkForMotion(){
|
54 | 9c46b728 | galberding | int motion = 1; |
55 | int led = 0; |
||
56 | types::position oldPos = global.odometry.getPosition(); |
||
57 | while(motion){
|
||
58 | 181f2892 | galberding | this->sleep(500); |
59 | 9c46b728 | galberding | types::position tmp = global.odometry.getPosition(); |
60 | motion = abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z); |
||
61 | oldPos = tmp; |
||
62 | global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW)); |
||
63 | global.robot.setLightColor(led % 8, Color(Color::BLACK));
|
||
64 | led++; |
||
65 | } |
||
66 | } |
||
67 | |||
68 | 181f2892 | galberding | bool UserThread::checkPinVoltage(){
|
69 | return global.ltc4412.isPluggedIn();
|
||
70 | } |
||
71 | 9c46b728 | galberding | |
72 | 181f2892 | galberding | bool UserThread::checkPinEnabled(){
|
73 | return global.ltc4412.isEnabled();
|
||
74 | } |
||
75 | |||
76 | int UserThread::checkDockingSuccess(){
|
||
77 | // setRpmSpeed(stop);
|
||
78 | checkForMotion(); |
||
79 | 9c46b728 | galberding | int success = 0; |
80 | global.odometry.resetPosition(); |
||
81 | types::position start = global.startPos = global.odometry.getPosition(); |
||
82 | global.motorcontrol.toggleMotorEnable(); |
||
83 | 61544eee | galberding | this->sleep(500); |
84 | 181f2892 | galberding | types::position stop_ = global.endPos = global.odometry.getPosition(); |
85 | |||
86 | 9c46b728 | galberding | // Amiro moved, docking was not successful
|
87 | 181f2892 | galberding | 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(); |
||
91 | 9c46b728 | galberding | success = 0;
|
92 | }else{
|
||
93 | 181f2892 | galberding | lightAllLeds(Color::GREEN); |
94 | 9c46b728 | galberding | success = 1;
|
95 | } |
||
96 | |||
97 | 61544eee | galberding | // this->sleep(500);
|
98 | 181f2892 | galberding | lightAllLeds(Color::BLACK); |
99 | 9c46b728 | galberding | return success;
|
100 | } |
||
101 | |||
102 | e2002d0e | galberding | uint16_t UserThread::getProxyRingSum(){ |
103 | uint16_t prox_sum = 0;
|
||
104 | for(int i=0; i<8;i++){ |
||
105 | prox_sum += global.robot.getProximityRingValue(i);; |
||
106 | } |
||
107 | return prox_sum;
|
||
108 | } |
||
109 | |||
110 | |||
111 | 58fe0e0b | Thomas Schöpping | UserThread::UserThread() : |
112 | chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
||
113 | { |
||
114 | } |
||
115 | |||
116 | UserThread::~UserThread() |
||
117 | { |
||
118 | } |
||
119 | |||
120 | msg_t |
||
121 | UserThread::main() |
||
122 | { |
||
123 | 5d138bca | galberding | /*
|
124 | * SETUP
|
||
125 | */
|
||
126 | 181f2892 | galberding | // User thread state:
|
127 | states utState = states::IDLE; |
||
128 | states newState; |
||
129 | |||
130 | int whiteBuf = 0; |
||
131 | e2002d0e | galberding | int proxyBuf = 0; |
132 | 61544eee | galberding | int releaseBuf = 0; |
133 | 181f2892 | galberding | int correctionStep = 0; |
134 | 61544eee | galberding | int dist_count = 0; |
135 | bool needDistance = false; |
||
136 | |||
137 | 019224ff | galberding | uint16_t rProx[8]; // buffer for ring proxy values |
138 | 9c46b728 | galberding | int rpmSpeed[2] = {0}; |
139 | int stop[2] = {0}; |
||
140 | 61544eee | galberding | int turn[2] = {5,-5}; |
141 | 181f2892 | galberding | LineFollowStrategy lStrategy = LineFollowStrategy::FUZZY; |
142 | 5d138bca | galberding | for (uint8_t led = 0; led < 8; ++led) { |
143 | global.robot.setLightColor(led, Color(Color::BLACK)); |
||
144 | } |
||
145 | running = false;
|
||
146 | LineFollow lf(&global); |
||
147 | /*
|
||
148 | * LOOP
|
||
149 | */
|
||
150 | while (!this->shouldTerminate()) |
||
151 | { |
||
152 | 181f2892 | galberding | /*
|
153 | * read accelerometer z-value
|
||
154 | */
|
||
155 | accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
||
156 | |||
157 | if (accel_z < -900 /*-0.9g*/) { |
||
158 | // Start line following when AMiRo is rotated
|
||
159 | if(utState == states::IDLE){
|
||
160 | utState = states::FOLLOW_LINE; |
||
161 | }else{
|
||
162 | utState = states::IDLE; |
||
163 | } |
||
164 | lightAllLeds(Color::GREEN); |
||
165 | this->sleep(1000); |
||
166 | lightAllLeds(Color::BLACK); |
||
167 | c76baf23 | Georg Alberding | |
168 | d607fcef | galberding | // If message was received handle it here:
|
169 | } else if(global.msgReceived){ |
||
170 | global.msgReceived = false;
|
||
171 | 181f2892 | galberding | // running = true;
|
172 | d607fcef | galberding | switch(global.lfStrategy){
|
173 | case msg_content::START:
|
||
174 | 181f2892 | galberding | utState = states::FOLLOW_LINE; |
175 | d607fcef | galberding | break;
|
176 | case msg_content::STOP:
|
||
177 | 181f2892 | galberding | utState = states::IDLE; |
178 | d607fcef | galberding | break;
|
179 | case msg_content::EDGE_RIGHT:
|
||
180 | 181f2892 | galberding | // utState = states::FOLLOW_LINE;
|
181 | d607fcef | galberding | lStrategy = LineFollowStrategy::EDGE_RIGHT; |
182 | break;
|
||
183 | case msg_content::EDGE_LEFT:
|
||
184 | 181f2892 | galberding | // utState = states::FOLLOW_LINE;
|
185 | d607fcef | galberding | lStrategy = LineFollowStrategy::EDGE_LEFT; |
186 | break;
|
||
187 | case msg_content::FUZZY:
|
||
188 | 181f2892 | galberding | // utState = states::FOLLOW_LINE;
|
189 | d607fcef | galberding | lStrategy = LineFollowStrategy::FUZZY; |
190 | break;
|
||
191 | 181f2892 | galberding | case msg_content::DOCK:
|
192 | utState = states::DETECT_STATION; |
||
193 | break;
|
||
194 | case msg_content::UNDOCK:
|
||
195 | utState = states::RELEASE; |
||
196 | break;
|
||
197 | case msg_content::CHARGE:
|
||
198 | utState = states::CHARGING; |
||
199 | d607fcef | galberding | break;
|
200 | default:
|
||
201 | 181f2892 | galberding | utState = states::IDLE; |
202 | d607fcef | galberding | break;
|
203 | } |
||
204 | 5d138bca | galberding | } |
205 | 181f2892 | galberding | newState = utState; |
206 | d607fcef | galberding | |
207 | 181f2892 | galberding | // Get sensor data
|
208 | 019224ff | galberding | uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
209 | uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset(); |
||
210 | for(int i=0; i<8;i++){ |
||
211 | rProx[i] = global.robot.getProximityRingValue(i); |
||
212 | } |
||
213 | 181f2892 | galberding | // int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
214 | // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
||
215 | switch(utState){
|
||
216 | case states::IDLE:
|
||
217 | if (!global.motorcontrol.getMotorEnable()){
|
||
218 | global.motorcontrol.toggleMotorEnable(); |
||
219 | } |
||
220 | setRpmSpeed(stop); |
||
221 | if(/* checkPinVoltage() && */ checkPinEnabled()){ |
||
222 | global.robot.requestCharging(0);
|
||
223 | } |
||
224 | 61544eee | galberding | whiteBuf = 0;
|
225 | proxyBuf = 0;
|
||
226 | 181f2892 | galberding | break;
|
227 | // ---------------------------------------
|
||
228 | case states::FOLLOW_LINE:
|
||
229 | // Set correct forward speed to every strategy
|
||
230 | if (global.forwardSpeed != global.rpmForward[0]){ |
||
231 | global.forwardSpeed = global.rpmForward[0];
|
||
232 | } |
||
233 | |||
234 | if(lf.getStrategy() != lStrategy){
|
||
235 | 9c46b728 | galberding | lf.setStrategy(lStrategy); |
236 | } |
||
237 | 5d138bca | galberding | |
238 | 181f2892 | galberding | //TODO: Check if white is detected and stop threshold is reached
|
239 | if(lf.followLine(rpmSpeed)){
|
||
240 | 61544eee | galberding | whiteBuf++; |
241 | 181f2892 | galberding | if(whiteBuf >= WHITE_COUNT_THRESH){
|
242 | 61544eee | galberding | rpmSpeed[0] = stop[0]; |
243 | rpmSpeed[1] = stop[1]; |
||
244 | 181f2892 | galberding | newState = states::IDLE; |
245 | 9c46b728 | galberding | } |
246 | 181f2892 | galberding | }else{
|
247 | whiteBuf = 0;
|
||
248 | 61544eee | galberding | // setRpmSpeed(rpmSpeed);
|
249 | 9c46b728 | galberding | } |
250 | e2002d0e | galberding | |
251 | if(getProxyRingSum() > PROXY_RING_THRESH){
|
||
252 | proxyBuf++; |
||
253 | if(proxyBuf > WHITE_COUNT_THRESH){
|
||
254 | newState = states::IDLE; |
||
255 | 61544eee | galberding | rpmSpeed[0] = stop[0]; |
256 | rpmSpeed[1] = stop[1]; |
||
257 | e2002d0e | galberding | } |
258 | }else{
|
||
259 | proxyBuf = 0;
|
||
260 | 61544eee | galberding | } |
261 | |||
262 | if ((rProx[3] + rProx[4]) > RING_PROX_FRONT_THRESH){ |
||
263 | rpmSpeed[0] = stop[0]; |
||
264 | rpmSpeed[1] = stop[1]; |
||
265 | } |
||
266 | |||
267 | // if (needDistance){
|
||
268 | // whiteBuf = 0;
|
||
269 | // proxyBuf = 0;
|
||
270 | // dist_count++;
|
||
271 | // if(dist_count > DIST_THRESH){
|
||
272 | // dist_count = 0;
|
||
273 | // needDistance = false;
|
||
274 | // }
|
||
275 | // }
|
||
276 | // whiteBuf = 0;
|
||
277 | // proxyBuf = 0;
|
||
278 | 181f2892 | galberding | // lf.followLine(rpmSpeed);
|
279 | 61544eee | galberding | setRpmSpeed(rpmSpeed); |
280 | 181f2892 | galberding | |
281 | break;
|
||
282 | // ---------------------------------------
|
||
283 | case states::DETECT_STATION:
|
||
284 | 61544eee | galberding | if (global.forwardSpeed != DETECTION_SPEED){
|
285 | global.forwardSpeed = DETECTION_SPEED; |
||
286 | } |
||
287 | 181f2892 | galberding | if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
|
288 | lf.setStrategy(LineFollowStrategy::EDGE_RIGHT); |
||
289 | } |
||
290 | |||
291 | lf.followLine(rpmSpeed); |
||
292 | setRpmSpeed(rpmSpeed); |
||
293 | // // Detect marker before docking station
|
||
294 | 019224ff | galberding | // if ((WL+WR) < PROXY_WHEEL_THRESH){
|
295 | // Use proxy ring
|
||
296 | if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){ |
||
297 | |||
298 | d607fcef | galberding | setRpmSpeed(stop); |
299 | 181f2892 | galberding | checkForMotion(); |
300 | // 180° Rotation
|
||
301 | global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
|
||
302 | // BaseThread::sleep(8000);
|
||
303 | checkForMotion(); |
||
304 | newState = states::CORRECT_POSITIONING; |
||
305 | 9c46b728 | galberding | } |
306 | 181f2892 | galberding | break;
|
307 | // ---------------------------------------
|
||
308 | case states::CORRECT_POSITIONING:
|
||
309 | 019224ff | galberding | if (global.forwardSpeed != CHARGING_SPEED){
|
310 | global.forwardSpeed = CHARGING_SPEED; |
||
311 | } |
||
312 | 181f2892 | galberding | if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
|
313 | lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT); |
||
314 | 9c46b728 | galberding | } |
315 | 181f2892 | galberding | lf.followLine(rpmSpeed); |
316 | setRpmSpeed(rpmSpeed); |
||
317 | 9c46b728 | galberding | |
318 | 181f2892 | galberding | correctionStep++; |
319 | if (correctionStep >= MAX_CORRECTION_STEPS){
|
||
320 | correctionStep = 0;
|
||
321 | newState = states::REVERSE; |
||
322 | setRpmSpeed(stop); |
||
323 | checkForMotion(); |
||
324 | } |
||
325 | break;
|
||
326 | // ---------------------------------------
|
||
327 | case states::REVERSE:
|
||
328 | if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
||
329 | lf.setStrategy(LineFollowStrategy::REVERSE); |
||
330 | } |
||
331 | lf.followLine(rpmSpeed); |
||
332 | setRpmSpeed(rpmSpeed); |
||
333 | 58fe0e0b | Thomas Schöpping | |
334 | 019224ff | galberding | // if ((WL+WR) < PROXY_WHEEL_THRESH){
|
335 | // Is of those sensors at it max val means that the AMiRo cant drive back
|
||
336 | // so check if correctly positioned
|
||
337 | //definitely wrong positioned -> correct position directly without rotation
|
||
338 | // Both sensors are at a wall -> potential loading station dock and on magnet -> need to rotate
|
||
339 | 61544eee | galberding | // if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
|
340 | // setRpmSpeed(stop);
|
||
341 | // checkForMotion();
|
||
342 | // newState = states::CHECK_POSITIONING;
|
||
343 | // } else
|
||
344 | 019224ff | galberding | if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){ |
345 | 61544eee | galberding | // setRpmSpeed(stop);
|
346 | // checkForMotion();
|
||
347 | 181f2892 | galberding | newState = states::CHECK_POSITIONING; |
348 | } |
||
349 | break;
|
||
350 | // ---------------------------------------
|
||
351 | case states::CHECK_POSITIONING:
|
||
352 | 61544eee | galberding | setRpmSpeed(stop); |
353 | checkForMotion(); |
||
354 | // if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
|
||
355 | if(checkDockingSuccess()){
|
||
356 | newState = states::CHECK_VOLTAGE; |
||
357 | }else{
|
||
358 | newState = states::CORRECT_POSITIONING; |
||
359 | } |
||
360 | // }
|
||
361 | // else{
|
||
362 | // newState = CORRECT_POSITIONING;
|
||
363 | // }
|
||
364 | 181f2892 | galberding | break;
|
365 | // ---------------------------------------
|
||
366 | ba75ee1d | galberding | case states::CHECK_VOLTAGE:
|
367 | if(!checkPinEnabled()){
|
||
368 | global.robot.requestCharging(1);
|
||
369 | } else {
|
||
370 | if(checkPinVoltage()){
|
||
371 | // Pins are under voltage -> correctly docked
|
||
372 | newState = states::CHARGING; |
||
373 | }else{
|
||
374 | // No voltage on pins -> falsely docked
|
||
375 | // deactivate pins
|
||
376 | global.robot.requestCharging(0);
|
||
377 | 61544eee | galberding | if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){ |
378 | newState = states::RELEASE_TO_CORRECT; |
||
379 | } else {
|
||
380 | newState = states::CORRECT_POSITIONING; |
||
381 | } |
||
382 | ba75ee1d | galberding | } |
383 | } |
||
384 | 019224ff | galberding | break;
|
385 | // ---------------------------------------
|
||
386 | case states::RELEASE_TO_CORRECT:
|
||
387 | global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
|
||
388 | checkForMotion(); |
||
389 | // move 1cm forward
|
||
390 | global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
||
391 | checkForMotion(); |
||
392 | // rotate back
|
||
393 | global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION); |
||
394 | checkForMotion(); |
||
395 | |||
396 | global.distcontrol.setTargetPosition(7000, 0, ROTATION_DURATION); |
||
397 | checkForMotion(); |
||
398 | newState = states::CORRECT_POSITIONING; |
||
399 | break;
|
||
400 | ba75ee1d | galberding | // ---------------------------------------
|
401 | 181f2892 | galberding | case states::CHARGING:
|
402 | if (global.motorcontrol.getMotorEnable()){
|
||
403 | global.motorcontrol.toggleMotorEnable(); |
||
404 | } |
||
405 | // Formulate Request to enable charging
|
||
406 | if(/* checkPinVoltage() && */ !checkPinEnabled()){ |
||
407 | global.robot.requestCharging(1);
|
||
408 | } |
||
409 | if(checkPinEnabled()){
|
||
410 | showChargingState(); |
||
411 | } |
||
412 | break;
|
||
413 | |||
414 | // ---------------------------------------
|
||
415 | case states::RELEASE:
|
||
416 | 61544eee | galberding | if (global.forwardSpeed != DETECTION_SPEED){
|
417 | global.rpmForward[0] = DETECTION_SPEED;
|
||
418 | } |
||
419 | 181f2892 | galberding | if(/* checkPinVoltage() && */ checkPinEnabled()){ |
420 | global.robot.requestCharging(0);
|
||
421 | }else{
|
||
422 | if (!global.motorcontrol.getMotorEnable()){
|
||
423 | global.motorcontrol.toggleMotorEnable(); |
||
424 | } |
||
425 | 61544eee | galberding | // if (releaseBuf > RELEASE_COUNT){
|
426 | // releaseBuf = 0;
|
||
427 | // setRpmSpeed(stop);
|
||
428 | // needDistance = true;
|
||
429 | // newState = states::FOLLOW_LINE;
|
||
430 | // }else{
|
||
431 | // releaseBuf++;
|
||
432 | // setRpmSpeed(turn);
|
||
433 | // }
|
||
434 | 181f2892 | galberding | //Rotate -20° to free from magnet
|
435 | global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
|
||
436 | 61544eee | galberding | checkForMotion(); |
437 | // move 1cm forward
|
||
438 | global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
||
439 | checkForMotion(); |
||
440 | // rotate back
|
||
441 | global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
|
||
442 | checkForMotion(); |
||
443 | 019224ff | galberding | |
444 | 61544eee | galberding | // global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
|
445 | // checkForMotion();
|
||
446 | 181f2892 | galberding | lStrategy = LineFollowStrategy::EDGE_RIGHT; |
447 | newState = states::FOLLOW_LINE; |
||
448 | 61544eee | galberding | // whiteBuf = -100;
|
449 | // lf.followLine(rpmSpeed);
|
||
450 | // setRpmSpeed(rpmSpeed);
|
||
451 | 181f2892 | galberding | } |
452 | 61544eee | galberding | // lightAllLeds(Color::BLACK);
|
453 | 181f2892 | galberding | break;
|
454 | |||
455 | default:
|
||
456 | break;
|
||
457 | } |
||
458 | utState = newState; |
||
459 | 5d138bca | galberding | this->sleep(CAN::UPDATE_PERIOD);
|
460 | } |
||
461 | 58fe0e0b | Thomas Schöpping | |
462 | return RDY_OK;
|
||
463 | } |