Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / userthread.cpp @ 98e7c69b

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