Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / userthread.cpp @ d02c536e

History | View | Annotate | Download (35.1 KB)

1
#include "userthread.hpp"
2
#include "amiro_map.hpp"
3
#include "global.hpp"
4
#include "linefollow.hpp"
5

    
6
using namespace amiro;
7

    
8
extern Global global;
9

    
10
// a buffer for the z-value of the accelerometer
11
int16_t accel_z;
12
bool running = false;
13

    
14

    
15
/**
16
 * Set speed.
17
 *
18
 * @param rpmSpeed speed for left and right wheel in rounds/min
19
 */
20
void UserThread::setRpmSpeedFuzzy(const int (&rpmSpeed)[2]) {
21
  global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
22
}
23

    
24
void UserThread::setRpmSpeed(const int (&rpmSpeed)[2]) {
25
  global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]);
26
}
27

    
28
void UserThread::lightOneLed(Color color, int idx){
29
  global.robot.setLightColor(idx, Color(color));
30
}
31

    
32
void UserThread::lightAllLeds(Color color){
33
  int led = 0;
34
  for(led=0; led<8; led++){
35
        lightOneLed(color, led);
36
      }
37
}
38

    
39
void UserThread::showChargingState(){
40
  uint8_t numLeds = global.robot.getPowerStatus().state_of_charge / 12;
41
  Color color = Color::GREEN;
42
  if (numLeds <= 2){
43
    color = Color::RED;
44
  }else if(numLeds <= 6){
45
    color = Color::YELLOW;
46
  }
47
  for (int i=0; i<numLeds; i++){
48
    lightOneLed(color, i);
49
    this->sleep(300);
50
  }
51
  this->sleep(1000);
52
  lightAllLeds(Color::BLACK);
53
}
54

    
55
void UserThread::chargeAsLED(){
56
  uint8_t numLeds = global.robot.getPowerStatus().state_of_charge / 12;
57
  Color color = Color::GREEN;
58
  if (numLeds <= 2){
59
    color = Color::RED;
60
  }else if(numLeds <= 6){
61
    color = Color::YELLOW;
62
  }
63
  for (int i=0; i<numLeds; i++){
64
    lightOneLed(color, i);
65
    // this->sleep(300);
66
  }
67
  // this->sleep(1000);
68
  // lightAllLeds(Color::BLACK);
69
}
70

    
71
// ----------------------------------------------------------------
72

    
73
void UserThread::getProxySectorVals(uint16_t (&proxVals)[8], uint16_t (&sProx)[8]){
74
  for (int i=0; i<8; i++){
75
    sProx[i] = (proxVals[i] < proxVals[(i+1) % 8]) ? proxVals[i] : proxVals[(i+1) % 8];
76
    // chprintf((BaseSequentialStream*)&global.sercanmux1, "%d: %d, ", i, sProx[i]);
77

    
78
  }
79
    // chprintf((BaseSequentialStream*)&global.sercanmux1, "\n");
80

    
81
}
82

    
83

    
84
void UserThread::getMaxFrontSectorVal(uint16_t (&sProx)[8], int32_t &sPMax){
85
  for (int i=2; i<5; i++){
86
    sPMax = (sPMax < sProx[i]) ? sProx[i] : sPMax;
87
  }
88
}
89

    
90
void UserThread::proxSectorSpeedCorrection(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]){
91
  int i;
92
  uint16_t sProx[8];
93
  int32_t sPMax = 0;
94
  getProxySectorVals(proxVals, sProx);
95
  getMaxFrontSectorVal(sProx, sPMax);
96

    
97
  int32_t speedL = rpmSpeed[0] - (sPMax * pCtrl.pFactor);
98
  int32_t speedR = rpmSpeed[1] - (sPMax * pCtrl.pFactor);
99

    
100

    
101

    
102
  if(sPMax > pCtrl.threshMid){
103
      rpmSpeed[0] = 0;
104
      rpmSpeed[1] = 0;
105
      pCtrl.staticCont++;
106
  }else if((speedL > 0) || (speedR > 0)){
107
    pCtrl.staticCont = 0;
108
    rpmSpeed[0] = speedL;
109
    rpmSpeed[1] = speedR;
110
  }else{
111
    rpmSpeed[0] = 4000000 + (rpmSpeed[0] - global.rpmForward[0] * 1000000);
112
    rpmSpeed[1] = 4000000 + (rpmSpeed[1] - global.rpmForward[0] * 1000000);
113
  }
114

    
115
  for(i=4; i<5; i++){
116
    if ((proxVals[i] > pCtrl.threshMid) && (proxVals[i+1] > pCtrl.threshLow)){
117
      rpmSpeed[0] = -5000000 ;
118
      rpmSpeed[1] = -5000000 ;
119
      // pCtrl.staticCont++;
120
      break;
121
    }
122
  }
123
  chargeAsLED();
124

    
125
  // chprintf((BaseSequentialStream*)&global.sercanmux1, "Max: %d factor: %d, Panel: %d SpeedL: %d SpeedR: %d ActualL: %d ActualR: %d\n",sPMax,  pCtrl.pFactor,  sPMax * pCtrl.pFactor, speedL, speedR, rpmSpeed[0], rpmSpeed[1]);
126

    
127

    
128
}
129
// -------------------------------------------------------------------
130

    
131

    
132
void UserThread::preventCollision( int (&rpmSpeed)[2], uint16_t (&proxVals)[8]) {
133

    
134
  if((proxVals[3] > pCtrl.threshLow) || (proxVals[4] > pCtrl.threshLow)){
135
      rpmSpeed[0] = rpmSpeed[0] / 2;
136
      rpmSpeed[1] = rpmSpeed[1] / 2;
137
  }
138

    
139
  if((proxVals[3] > pCtrl.threshMid) || (proxVals[4] > pCtrl.threshMid)){
140
      rpmSpeed[0] = rpmSpeed[0] / 3;
141
      rpmSpeed[1] = rpmSpeed[1] / 3;
142
  }
143

    
144
  if((proxVals[3] > pCtrl.threshHigh) || (proxVals[4] > pCtrl.threshHigh)){
145
      rpmSpeed[0] = 0;
146
      rpmSpeed[1] = 0;
147
      utCount.ringProxCount++;
148
  }else{
149
    utCount.ringProxCount = 0;
150
  }
151

    
152
}
153

    
154

    
155
/**
156
 * Blocks as long as the position changes.
157
 */
158
void UserThread::checkForMotion(){
159
  bool motion = true;
160
  int led = 0;
161
  types::position oldPos = global.odometry.getPosition();
162
  while(motion){
163
    this->sleep(200);
164
    types::position tmp = global.odometry.getPosition();
165
    motion = oldPos.x != tmp.x; //abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z);
166
    oldPos = tmp;
167
    global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW));
168
    global.robot.setLightColor(led % 8, Color(Color::BLACK));
169
    led++;
170
  }
171
  lightAllLeds(Color::BLACK);
172
}
173

    
174
bool UserThread::checkFrontalObject(){
175
  uint32_t thresh = pCtrl.threshMid;
176
  uint32_t prox;
177
  for(int i=0; i<8; i++){
178
    prox = global.robot.getProximityRingValue(i);
179
    if((i == 3) || (i == 4)){
180
      if(prox < thresh){
181
        return false;
182
      }
183
    }else{
184
      if(prox > thresh){
185
        return false;
186
      }
187
    }
188
  }
189
  return true;
190
}
191

    
192
bool UserThread::checkPinVoltage(){
193
  return global.ltc4412.isPluggedIn();
194
}
195

    
196
bool UserThread::checkPinEnabled(){
197
  return global.ltc4412.isEnabled();
198
}
199

    
200
int UserThread::checkDockingSuccess(){
201
  // setRpmSpeed(stop);
202
  checkForMotion();
203
  int success = 0;
204
  // global.odometry.resetPosition();
205
  types::position start = global.startPos = global.odometry.getPosition();
206
  global.motorcontrol.setMotorEnable(false);
207
  this->sleep(1000);
208
  types::position stop_ = global.endPos = global.odometry.getPosition();
209

    
210
  // Amiro moved, docking was not successful
211
  // if ((start.x + stop_.x)  || (start.y + stop_.y)){
212
  if (abs(start.x - stop_.x) > 200 /* || (start.y + stop_.y) */){
213
    lightAllLeds(Color::RED);
214
    // Enable Motor again if docking was not successful
215
    global.motorcontrol.setMotorEnable(true);
216
    success = 0;
217
  }else{
218
    lightAllLeds(Color::GREEN);
219
    success = 1;
220
  }
221

    
222
  // this->sleep(500);
223
  lightAllLeds(Color::BLACK);
224
  return success;
225
}
226

    
227
int UserThread::getProxyRingSum(){
228
  int prox_sum = 0;
229
  for(int i=0; i<8;i++){
230
    prox_sum += global.robot.getProximityRingValue(i);;
231
  }
232
  return prox_sum;
233
}
234

    
235
int32_t UserThread::meanDeviation(uint16_t a, uint16_t b){
236
  int32_t diff = a - b;
237
  int32_t res = 0;
238
  devCor.proxbuf[devCor.pCount] = (diff*100)/((a+b)/2);
239
  for (int i = 0; i< PROX_DEVIATION_MEAN_WINDOW; i++){
240
    res += devCor.proxbuf[i];
241
  }
242
  devCor.pCount++;
243
  devCor.pCount = devCor.pCount % PROX_DEVIATION_MEAN_WINDOW;
244

    
245
  devCor.currentDeviation =  res / PROX_DEVIATION_MEAN_WINDOW;
246
  return devCor.currentDeviation;
247
}
248

    
249
void setAttributes(uint8_t (&map)[MAX_NODES][NODE_ATTRIBUTES],
250
                          uint8_t id, uint8_t l, uint8_t r, uint8_t att) {
251
  map[id][0] = l;
252
  map[id][1] = r;
253
  map[id][2] = att;
254
}
255

    
256
UserThread::UserThread() :
257
  chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()
258
{
259
}
260

    
261
UserThread::~UserThread()
262
{
263
}
264

    
265
msg_t
266
UserThread::main()
267
{
268
  /*
269
   * SETUP
270
   */
271
  // User thread state:
272

    
273
  for (uint8_t led = 0; led < 8; ++led) {
274
    global.robot.setLightColor(led, Color(Color::BLACK));
275
  }
276
  running = false;
277
  LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT;
278
  LineFollow lf(&global);
279
  AmiroMap map(&global);
280
  /*
281
   * LOOP
282
   */
283
  while (!this->shouldTerminate())
284
  {
285
    /*
286
    * read accelerometer z-value
287
    */
288
    accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);
289

    
290
    if (accel_z < -900 /*-0.9g*/) {
291
      // Start line following when AMiRo is rotated
292
      if(currentState == states::INACTIVE){
293
        newState = states::FOLLOW_LINE;
294
      }else{
295
        newState = states::IDLE;
296
      }
297
      lightAllLeds(Color::GREEN);
298
      this->sleep(1000);
299
      lightAllLeds(Color::BLACK);
300

    
301
    // If message was received handle it here:
302
    } else if(global.msgReceived){
303
      global.msgReceived = false;
304
      // running = true;
305
      switch(global.lfStrategy){
306
      case msg_content::MSG_START:
307
        newState = states::CALIBRATION_CHECK;
308
        break;
309
      case msg_content::MSG_STOP:
310
        newState = states::IDLE;
311
        break;
312
      case msg_content::MSG_EDGE_RIGHT:
313
        // newState = states::FOLLOW_LINE;
314
        lStrategy = LineFollowStrategy::EDGE_RIGHT;
315
        break;
316
      case  msg_content::MSG_EDGE_LEFT:
317
        // newState = states::FOLLOW_LINE;
318
        lStrategy = LineFollowStrategy::EDGE_LEFT;
319
        break;
320
      case msg_content::MSG_FUZZY:
321
        // newState = states::FOLLOW_LINE;
322
        lStrategy = LineFollowStrategy::FUZZY;
323
        break;
324
      case msg_content::MSG_DOCK:
325
        newState = states::DETECT_STATION;
326
        break;
327
      case msg_content::MSG_UNDOCK:
328
        newState = states::RELEASE;
329
        break;
330
      case msg_content::MSG_CHARGE:
331
        newState = states::CHARGING;
332
        break;
333
      case msg_content::MSG_RESET_ODOMETRY:
334
        global.odometry.resetPosition();
335
        break;
336
      case msg_content::MSG_CALIBRATE_BLACK:
337
        proxCalib.calibrateBlack = true;
338
        // global.odometry.resetPosition();
339
        newState = states::CALIBRATION;
340
        break;
341
      case msg_content::MSG_CALIBRATE_WHITE:
342
        proxCalib.calibrateBlack = false;
343
        newState = states::CALIBRATION;
344
        break;
345
      case msg_content::MSG_TEST_MAP_STATE:
346
        newState = states::TEST_MAP_STATE;
347
        break;
348

    
349
      default:
350
        newState = states::IDLE;
351
        break;
352
      }
353
    }
354
    // newState = currentState;
355

    
356
    // Get sensor data
357
    // uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
358
    // uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
359
    for(int i=0; i<8;i++){
360
      rProx[i] = global.robot.getProximityRingValue(i);
361
    }
362

    
363
    // Continously update devication values
364
    meanDeviation(rProx[0] & 0xFFF0, rProx[7] & 0xFFF0);
365
    // int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
366
    // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
367
    switch(currentState){
368
      case states::INACTIVE:
369
        // Dummy state to deactivate every interaction
370
      break;
371
      // ---------------------------------------
372
      case states::CALIBRATION_CHECK:
373
        // global.robot.calibrate();
374
        if(global.linePID.BThresh >= global.linePID.WThresh){
375
          newState = states::CALIBRATION_ERROR;
376
        }else{
377
          newState = states::FOLLOW_LINE;
378
        }
379
      break;
380
      // ---------------------------------------
381
      case states::CALIBRATION:
382
        /* Calibrate the global thresholds for black or white.
383
            This values will be used by the line follow object
384
        */
385

    
386
        proxCalib.buf = 0;
387
        if(proxCalib.calibrateBlack){
388
          chprintf((BaseSequentialStream*)&global.sercanmux1, "Black Calibration, Place AMiRo on black Surface!\n");
389
          global.robot.calibrate();
390
        }
391
        for(int i=0; i <= proxCalib.meanWindow; i++){
392
          proxCalib.buf += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset()
393
                          + global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
394
          this->sleep(CAN::UPDATE_PERIOD);
395
        }
396
        proxCalib.buf = proxCalib.buf / (2*proxCalib.meanWindow);
397

    
398
        if(proxCalib.calibrateBlack){
399
          global.linePID.BThresh = proxCalib.buf;
400
        }else  {
401
          global.linePID.WThresh  = proxCalib.buf;
402
        }
403
          chprintf((BaseSequentialStream*)&global.sercanmux1, "Black: %d, White: %d!\n", global.linePID.BThresh, global.linePID.WThresh);
404

    
405
        newState = states::IDLE;
406
      break;
407
      // ---------------------------------------
408
      case states::IDLE:
409
        global.motorcontrol.setMotorEnable(true);
410
        setRpmSpeed(stop);
411
        if(/* checkPinVoltage() && */ checkPinEnabled()){
412
          global.robot.requestCharging(0);
413
        }
414
        // pCtrl.pFactor = 0;
415
        pCtrl.staticCont = 0;
416
        utCount.whiteCount = 0;
417
        utCount.ringProxCount = 0;
418
        utCount.errorCount = 0;
419
        newState = states::INACTIVE;
420
      break;
421
      // ---------------------------------------
422
      case states::FOLLOW_LINE:
423
      // Set correct forward speed to every strategy
424
        if (global.forwardSpeed != global.rpmForward[0]){
425
          global.forwardSpeed = global.rpmForward[0];
426
        }
427

    
428
        if(lf.getStrategy() != lStrategy){
429
          lf.setStrategy(lStrategy);
430
        }
431

    
432
        if(lf.followLine(rpmSpeed)){
433
          utCount.whiteCount++;
434
          if(utCount.whiteCount >= WHITE_DETETION_TIMEOUT){
435
            setRpmSpeed(stop);
436
            utCount.whiteCount = 0;
437
            newState = states::WHITE_DETECTION_ERROR;
438
          }
439
        }else{
440
          utCount.whiteCount = 0;
441
        }
442

    
443
        preventCollision(rpmSpeed, rProx);
444
        // proxSectorSpeedCorrection(rpmSpeed, rProx);
445

    
446
        if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){
447
          utCount.ringProxCount = 0;
448

    
449

    
450
          checkForMotion();
451
          // Check if only front sensors are active
452
          if (checkFrontalObject()) {
453
            // global.distcontrol.setTargetPosition(0, 2792526, ROTATION_DURATION);
454
            // // BaseThread::sleep(8000);
455
            // checkForMotion();
456
            this->utCount.whiteCount = 0;
457
            newState = states::TURN;
458
            // lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
459
          } else {
460
            newState = states::PROXY_DETECTION_ERROR;
461
          }
462
        }
463

    
464
        if (lf.getStrategy() == LineFollowStrategy::FUZZY){
465
          setRpmSpeedFuzzy(rpmSpeed);
466
        }else{
467

    
468
          setRpmSpeed(rpmSpeed);
469
        }
470

    
471
      break;
472
      // ---------------------------------------
473
    case states::TURN:{
474
        // Check the line strategy in order to continue driving on the right side
475
      int factor = SPEED_CONVERSION_FACTOR;
476
      int frontL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
477
      int frontR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
478
      int blackSensor = 0;
479
      if (lf.getStrategy() == LineFollowStrategy::EDGE_RIGHT) {
480
        factor = -factor;
481
        blackSensor = frontL;
482
      }else{
483
        blackSensor = frontR;
484
      }
485

    
486
      rpmSpeed[0] = factor * CHARGING_SPEED;
487
      rpmSpeed[1] = -factor * CHARGING_SPEED;
488
      setRpmSpeed(rpmSpeed);
489

    
490
      if ((blackSensor >= global.linePID.WThresh )){
491
        utCount.whiteCount = 1;
492
      }else {
493
        if((utCount.whiteCount == 1) && (blackSensor <= global.linePID.BThresh)){
494
          utCount.whiteCount = 0;
495
          newState = states::FOLLOW_LINE;
496
          setRpmSpeed(stop);
497
        }
498
      }
499
      break;
500
    }
501
      // ---------------------------------------
502
    case states::DETECT_STATION:
503

    
504
        if (global.forwardSpeed != DETECTION_SPEED){
505
          global.forwardSpeed = DETECTION_SPEED;
506
        }
507
        if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
508
          lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
509
        }
510

    
511
        lf.followLine(rpmSpeed);
512
        setRpmSpeed(rpmSpeed);
513
        // // Detect marker before docking station
514
        // if ((WL+WR) < PROXY_WHEEL_THRESH){
515
        // Use proxy ring
516
        if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){
517

    
518
          setRpmSpeed(stop);
519
          checkForMotion();
520
          // 180° Rotation
521
          global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
522
          // BaseThread::sleep(8000);
523
          checkForMotion();
524
          newState = states::CORRECT_POSITIONING;
525
        }
526
      break;
527
      // ---------------------------------------
528
    case states::CORRECT_POSITIONING:
529
        if (global.forwardSpeed != CHARGING_SPEED){
530
          global.forwardSpeed = CHARGING_SPEED;
531
        }
532
        if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
533
          lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
534
        }
535
        lf.followLine(rpmSpeed);
536
        setRpmSpeed(rpmSpeed);
537

    
538
        utCount.stateTime++;
539
        if (utCount.stateTime >= DOCKING_CORRECTION_TIMEOUT){
540
          utCount.stateTime = 0;
541
          newState = states::REVERSE;
542
          setRpmSpeed(stop);
543
          checkForMotion();
544
        }
545
      break;
546
      // ---------------------------------------
547
    case states::REVERSE:
548
        if(lf.getStrategy() != LineFollowStrategy::REVERSE){
549
          lf.setStrategy(LineFollowStrategy::REVERSE);
550
        }
551
        lf.followLine(rpmSpeed);
552
        setRpmSpeed(rpmSpeed);
553
        // utCount.stateTime++;
554

    
555
        // Docking is only successful if Deviation is in range and sensors are at their max values.
556
        if((rProx[0] >= PROX_MAX_VAL)
557
           && (rProx[7] >= PROX_MAX_VAL)
558
           && ((devCor.currentDeviation > -MAX_DEVIATION_FACTOR) && (devCor.currentDeviation < MAX_DEVIATION_FACTOR) )){
559
          // setRpmSpeed(stop);
560
          // checkForMotion();
561
          utCount.stateTime = 0;
562
          newState = states::PUSH_BACK;
563
        }else if ((devCor.currentDeviation <= -MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){
564
          // Case R
565
          utCount.stateTime = 0;
566
          setRpmSpeed(stop);
567
          devCor.RCase = true;
568
          lightAllLeds(Color::YELLOW);
569
          newState = states::DEVIATION_CORRECTION;
570
        }else if ((devCor.currentDeviation >= MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){
571
          // Case L
572
          utCount.stateTime = 0;
573
          setRpmSpeed(stop);
574
          devCor.RCase = false;
575
          lightAllLeds(Color::WHITE);
576
          newState = states::DEVIATION_CORRECTION;
577
        }else if (utCount.stateTime >= REVERSE_DOCKING_TIMEOUT){
578
          setRpmSpeed(stop);
579
          utCount.stateTime = 0;
580
          utCount.errorCount++;
581
          if (utCount.errorCount >= DOCKING_ERROR_THRESH){
582
            newState = states::DOCKING_ERROR;
583
          }else{
584
            newState = states::CORRECT_POSITIONING;
585
          }
586
        }
587

    
588
        // if((devCor.currentDeviation <= -10)){
589
        //   rpmSpeed[0] -= 2000000;
590
        // }else if(devCor.currentDeviation >= 10){
591
        //   rpmSpeed[1] -= 2000000;
592
        // }
593
        // setRpmSpeed(rpmSpeed);
594
      break;
595
      // ---------------------------------------
596
    case states::DEVIATION_CORRECTION:
597
        // if(lf.getStrategy() != LineFollowStrategy::REVERSE){
598
        //   lf.setStrategy(LineFollowStrategy::REVERSE);
599
        // }
600
        // lf.followLine(rpmSpeed);
601
        // setRpmSpeed(rpmSpeed);
602
        if(utCount.stateTime < DEVIATION_CORRECTION_DURATION / 2 ){
603
          if(devCor.RCase){
604
            rpmSpeed[0] = 0;
605
            rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;
606
          }else {
607
            rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;
608
            rpmSpeed[1] = 0;
609
          }
610
          setRpmSpeed(rpmSpeed);
611
        }else if (((utCount.stateTime >= DEVIATION_CORRECTION_DURATION / 2) && (utCount.stateTime < DEVIATION_CORRECTION_DURATION +10)) ){
612
          if(devCor.RCase){
613
            rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;
614
            rpmSpeed[1] = 0;
615
          }else {
616
            rpmSpeed[0] = 0;
617
            rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;
618
          }
619
          setRpmSpeed(rpmSpeed);
620
          if(((devCor.currentDeviation >= -5) && (devCor.currentDeviation <= 5))){
621
            utCount.stateTime = 0;
622
            newState = states::REVERSE;
623
            setRpmSpeed(stop);
624
          }
625
        }else{
626
          utCount.stateTime = 0;
627
          newState = states::REVERSE;
628
          setRpmSpeed(stop);
629
        }
630

    
631
        utCount.stateTime++;
632

    
633

    
634
        // if (utCount.stateTime > PUSH_BACK_TIMEOUT){
635
        //   utCount.stateTime = 0;
636
        //   newState = states::CHECK_POSITIONING;
637
        // }
638
      break;
639
      // ---------------------------------------
640
    case states::PUSH_BACK:
641
        if(lf.getStrategy() != LineFollowStrategy::REVERSE){
642
          lf.setStrategy(LineFollowStrategy::REVERSE);
643
        }
644
        lf.followLine(rpmSpeed);
645
        setRpmSpeed(rpmSpeed);
646

    
647
        utCount.stateTime++;
648
        if (utCount.stateTime > PUSH_BACK_TIMEOUT){
649
          utCount.stateTime = 0;
650
          newState = states::CHECK_POSITIONING;
651
        }
652
      break;
653
      // ---------------------------------------
654
    case states::CHECK_POSITIONING:
655
        setRpmSpeed(stop);
656
        checkForMotion();
657
        if(checkDockingSuccess()){
658
          newState = states::CHECK_VOLTAGE;
659
        }else{
660
          utCount.errorCount++;
661
          newState = states::CORRECT_POSITIONING;
662
          if (utCount.errorCount >= DOCKING_ERROR_THRESH){
663
              newState = states::DOCKING_ERROR;
664
            }
665
        }
666
      break;
667
      // ---------------------------------------
668
    case states::CHECK_VOLTAGE:
669
        if(!checkPinEnabled()){
670
          global.robot.requestCharging(1);
671
        } else {
672
          if(checkPinVoltage()){
673
            // Pins are under voltage -> correctly docked
674

    
675
            newState = states::CHARGING;
676
          }else{
677
            utCount.errorCount++;
678
            // No voltage on pins -> falsely docked
679
            // deactivate pins
680
            global.motorcontrol.setMotorEnable(true);
681
            global.robot.requestCharging(0);
682
            // TODO: Soft release when docking falsely
683
            if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
684
              newState = states::RELEASE_TO_CORRECT;
685
            } else {
686
              newState = states::RELEASE_TO_CORRECT; //states::CORRECT_POSITIONING;
687
            }
688

    
689
            if (utCount.errorCount > DOCKING_ERROR_THRESH){
690
              newState = states::DOCKING_ERROR;
691
            }
692
          }
693
        }
694
      break;
695
      // ---------------------------------------
696
    case states::RELEASE_TO_CORRECT:
697

    
698
        global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
699
        checkForMotion();
700
        // move 1cm forward
701
        global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
702
        checkForMotion();
703
        // rotate back
704
        global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION);
705
        checkForMotion();
706

    
707
        global.distcontrol.setTargetPosition(1500, 0, ROTATION_DURATION);
708
        checkForMotion();
709
        newState = states::CORRECT_POSITIONING;
710
      break;
711
      // ---------------------------------------
712
    case states::CHARGING:
713
        global.motorcontrol.setMotorEnable(false);
714
        utCount.errorCount = 0;
715
        // Formulate Request to enable charging
716
        if(/* checkPinVoltage() && */ !checkPinEnabled()){
717
          global.robot.requestCharging(1);
718
        }
719
        if(checkPinEnabled()){
720
          showChargingState();
721
        }
722
      break;
723
      // ---------------------------------------
724
    case states::RELEASE:
725

    
726
      if (global.forwardSpeed != DETECTION_SPEED){
727
          global.rpmForward[0] = DETECTION_SPEED;
728
        }
729
        if(/* checkPinVoltage() && */ checkPinEnabled()){
730
          global.robot.requestCharging(0);
731
        }else{
732
          global.motorcontrol.setMotorEnable(true);
733
          // TODO: Use controlled
734
          //Rotate -20° to free from magnet
735
          global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
736
          checkForMotion();
737
          // move 1cm forward
738
          global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
739
          checkForMotion();
740
          // rotate back
741
          // global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
742
          // checkForMotion();
743

    
744
          // global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
745
          // checkForMotion();
746
          lStrategy = LineFollowStrategy::EDGE_RIGHT;
747
          newState = states::FOLLOW_LINE;
748
            // whiteBuf = -100;
749
          // lf.followLine(rpmSpeed);
750
          // setRpmSpeed(rpmSpeed);
751
        }
752
        // lightAllLeds(Color::BLACK);
753
      break;
754
      // ---------------------------------------
755
    case states::DOCKING_ERROR:
756
      newState = states::RELEASE;
757
      break;
758
      // ---------------------------------------
759
    case states::REVERSE_TIMEOUT_ERROR:
760
      newState = states::IDLE;
761
      break;
762
      // ---------------------------------------
763
    case states::CALIBRATION_ERROR:
764
      newState = states::IDLE;
765
      break;
766
      // ---------------------------------------
767
    case states::WHITE_DETECTION_ERROR:
768
      newState = states::IDLE;
769
      break;
770
      // ---------------------------------------
771
    case states::PROXY_DETECTION_ERROR:
772
      newState = states::IDLE;
773
      break;
774
      // ---------------------------------------
775
    case states::NO_CHARGING_POWER_ERROR:
776
      newState = states::IDLE;
777
      break;
778
      // ---------------------------------------
779
    case states::UNKNOWN_STATE_ERROR:
780
        newState = states::IDLE;
781
      break;
782
      // ---------------------------------------
783
    case states::TEST_MAP_STATE:{
784
      // Test suit for amiro map
785

    
786

    
787

    
788
      // AmiroMap map = AmiroMap(&global);
789

    
790
      // --------------------------------------------------
791

    
792
      global.tcase = 0;
793
      // Set basic valid map configuration
794
      setAttributes(global.testmap, 0, 1, 2, 1);
795
      setAttributes(global.testmap, 1, 2, 2, 0);
796
      setAttributes(global.testmap, 2, 1, 0, 0);
797
      setAttributes(global.testmap, 3, 0, 0, 0xff);
798
      chprintf((BaseSequentialStream *)&global.sercanmux1, "Init Case: %d, res: %d\n",global.tcase, map.initialize());
799
      global.testres[global.tcase] = map.get_state()->valid;
800

    
801
      global.tcase++; // 1
802
      // Test map fail if first node is flagged with end
803
      setAttributes(global.testmap, 0, 1, 2, 0xff);
804
      map.initialize();
805
      global.testres[global.tcase] = !map.get_state()->valid;
806

    
807
      global.tcase++; // 2
808
      // Test if node 2 is set as start node
809
      setAttributes(global.testmap, 0, 1, 2, 0);
810
      setAttributes(global.testmap, 2, 1, 0, 1);
811
      map.initialize();
812
      global.testres[global.tcase] = map.get_state()->current == 2;
813

    
814
      global.tcase++; // 3
815
      // Test if non reachable nodes will trigger invalid map
816
      setAttributes(global.testmap, 3, 0, 0, 0);
817
      setAttributes(global.testmap, 4, 0, 0, 0xff);
818
      map.initialize();
819
      global.testres[global.tcase] = !map.get_state()->valid;
820

    
821
      global.tcase++; // 4
822
      // Test Reinitialization
823
      setAttributes(global.testmap, 0, 1, 2, 1);
824
      setAttributes(global.testmap, 1, 2, 2, 0);
825
      setAttributes(global.testmap, 2, 1, 0, 0);
826
      setAttributes(global.testmap, 3, 0, 0, 0xff);
827
      map.initialize();
828
      global.testres[global.tcase] = map.get_state()->valid;
829

    
830
      global.odometry.resetPosition();
831
      uint8_t ret = 0;
832
      global.tcase++; // 5
833
      // Test update under normal linefollowing without fixpoint
834
      ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT);
835
      chprintf((BaseSequentialStream *)&global.sercanmux1,
836
               "Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,
837
               map.get_state()->current, map.get_state()->next);
838
      // No case should be true because neither was a node visited nor
839
      // was a fixpoint detected.
840
      global.testres[global.tcase] = (ret == 0x4);
841

    
842

    
843
      global.odometry.setPosition(1.0, 0.0, 0.0);
844
      chprintf((BaseSequentialStream *)&global.sercanmux1, "Current Point: %d\n", global.odometry.getPosition().x);
845
      global.tcase++; // 6
846
      // Fixpoint on left side
847
      ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT);
848
      chprintf((BaseSequentialStream *)&global.sercanmux1,
849
               "Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,
850
               map.get_state()->current, map.get_state()->next);
851
      // No case should be true because neither was a node visited nor
852
      // was a fixpoint detected.
853
      // global.odometry
854
      global.testres[global.tcase] = (ret == 0x1)
855
        && (map.get_state()->strategy == 0x01)
856
        && (map.get_state()->dist == 0)
857
        && (map.get_state()->current == 2);
858

    
859

    
860
      global.odometry.setPosition(1.5, 0.0, 0.0);
861
      chprintf((BaseSequentialStream *)&global.sercanmux1,
862
               "Current Point: %d\n", global.odometry.getPosition().x);
863
      global.tcase++; // 7
864
      // Fixpoint on left side, no update should appear because fixpoint already
865
      // marked
866
      ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT);
867
      chprintf((BaseSequentialStream *)&global.sercanmux1,
868
               "Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,
869
               map.get_state()->current, map.get_state()->next);
870
      // No case should be true because neither was a node visited nor
871
      // was a fixpoint detected.
872
      global.testres[global.tcase] = (ret == 0x00)
873
        && (map.get_state()->strategy == 0x01);
874
        // && (map.get_state()->dist == 0);
875

    
876
      global.odometry.setPosition(1.2, 0.0, 0.0);
877
      global.tcase++; // 8
878
      // Fixpoint on left side, no update should appear because fixpoint already
879
      // marked
880
      ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT);
881
      chprintf((BaseSequentialStream *)&global.sercanmux1,
882
               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n", global.tcase, ret,
883
               map.get_state()->current, map.get_state()->next, map.get_state()->dist, map.get_state()->eLength);
884
      // No case should be true because neither was a node visited nor
885
      // was a fixpoint detected.
886
      global.testres[global.tcase] =
887
          (ret == 0x04) && (map.get_state()->strategy == 0x01)
888
        && (map.get_state()->dist == 0);
889

    
890
      global.odometry.setPosition(.5, 0.0, 0.0);
891
      chprintf((BaseSequentialStream *)&global.sercanmux1,
892
               "Current Point: %d\n", global.odometry.getPosition().x);
893
      global.tcase++; // 9
894
      // Fixpoint on left side, no update should appear because fixpoint already
895
      // marked
896
      ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT);
897
      chprintf((BaseSequentialStream *)&global.sercanmux1,
898
               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n",
899
               global.tcase, ret, map.get_state()->current,
900
               map.get_state()->next, map.get_state()->dist,
901
               map.get_state()->eLength);
902
      // No case should be true because neither was a node visited nor
903
      // was a fixpoint detected.
904
      global.testres[global.tcase] =
905
        (ret == 9) &&
906
        (map.get_state()->strategy == 1) &&
907
        (map.get_state()->dist == 0) &&
908
        (map.get_state()->eLength == 50);
909

    
910
      global.odometry.setPosition(.75, 0.0, 0.0);
911
      chprintf((BaseSequentialStream *)&global.sercanmux1,
912
               "Current Point: %d\n", global.odometry.getPosition().x);
913
      global.tcase++; // 10
914
      // Fixpoint on left side, no update should appear because fixpoint already
915
      // marked
916
      ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT);
917
      chprintf((BaseSequentialStream *)&global.sercanmux1,
918
               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n",
919
               global.tcase, ret, map.get_state()->current,
920
               map.get_state()->next, map.get_state()->dist,
921
               map.get_state()->eLength);
922
      // No case should be true because neither was a node visited nor
923
      // was a fixpoint detected.
924
      global.testres[global.tcase] =
925
          (ret == 12) && (map.get_state()->strategy == 1) &&
926
          (map.get_state()->dist == 50) && (map.get_state()->eLength == 50);
927

    
928
      int failed = 0;
929
      int passed = 0;
930
      for (int i = 0; i <= global.tcase; i++) {
931
        if (global.testres[i]) {
932
          passed++;
933
          chprintf((BaseSequentialStream *)&global.sercanmux1,
934
                   "Test %d Passed!\n", i);
935
        } else {
936
          failed++;
937
          chprintf((BaseSequentialStream *)&global.sercanmux1,
938
                   "Test %d Failed\n", i);
939
        }
940
      }
941
      chprintf((BaseSequentialStream *)&global.sercanmux1,
942
               "Total: %d, Passed: %d, Failed: %d\n", global.tcase + 1, passed,
943
               failed);
944

    
945
      newState = states::IDLE;
946
      break;
947
    }
948
      // --------------------------------------------------
949
    default:
950
      newState = states::UNKNOWN_STATE_ERROR;
951
      break;
952
    }
953

    
954
    // In case a new state is set:
955
    // 1. Record the state transition
956
    if (currentState != newState){
957

    
958
      global.stateTransitionCounter++;
959
      // Clear all state transitions to prevent overflow
960
      if (global.stateTransitionCounter >= 255) {
961
        global.stateTransitionCounter = 0;
962
        for (int i = 0; i < 24; i++) {
963
          global.stateTracker[i] = 0;
964
        }
965
}
966
      // Transmit the new state over can
967
      chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
968
      global.robot.transmitState(newState);
969

    
970
      // Increase state count for specific state
971
      // TODO: Improve with dictionary or other than switch case
972
      if (newState == states::IDLE)
973
        {global.stateTracker[states::IDLE] += 1;}
974
      else if (newState == states::FOLLOW_LINE)
975
        {global.stateTracker[states::FOLLOW_LINE] += 1;}
976
      else if (newState == states::DETECT_STATION)
977
        {global.stateTracker[states::DETECT_STATION] += 1;}
978
      else if (newState == states::REVERSE)
979
        {global.stateTracker[states::REVERSE] += 1;}
980
      else if (newState == states::PUSH_BACK)
981
        {global.stateTracker[states::PUSH_BACK] += 1;}
982
      else if (newState == states::CHECK_POSITIONING)
983
        {global.stateTracker[states::CHECK_POSITIONING] += 1;}
984
      else if (newState == states::CHECK_VOLTAGE)
985
        {global.stateTracker[states::CHECK_VOLTAGE] += 1;}
986
      else if (newState == states::CHARGING)
987
        {global.stateTracker[states::CHARGING] += 1;}
988
      else if (newState == states::RELEASE)
989
        {global.stateTracker[states::RELEASE] += 1;}
990
      else if (newState == states::RELEASE_TO_CORRECT)
991
        {global.stateTracker[states::RELEASE_TO_CORRECT] += 1;}
992
      else if (newState == states::CORRECT_POSITIONING)
993
        {global.stateTracker[states::CORRECT_POSITIONING] += 1;}
994
      else if (newState == states::TURN)
995
        {global.stateTracker[states::TURN] += 1;}
996
      else if (newState == states::INACTIVE)
997
        {global.stateTracker[states::INACTIVE] += 1;}
998
      else if (newState == states::CALIBRATION)
999
        {global.stateTracker[states::CALIBRATION] += 1;}
1000
      else if (newState == states::CALIBRATION_CHECK)
1001
        {global.stateTracker[states::CALIBRATION_CHECK] += 1;}
1002
      else if (newState == states::DEVIATION_CORRECTION)
1003
        {global.stateTracker[states::DEVIATION_CORRECTION] += 1;}
1004
      else if (newState == states::DOCKING_ERROR)
1005
        {global.stateTracker[16+(-states::DOCKING_ERROR)] += 1;}
1006
      else if (newState == states::REVERSE_TIMEOUT_ERROR)
1007
        {global.stateTracker[16+(-states::REVERSE_TIMEOUT_ERROR)] += 1;}
1008
      else if (newState == states::CALIBRATION_ERROR)
1009
        {global.stateTracker[16+(-states::CALIBRATION_ERROR)] += 1;}
1010
      else if (newState == states::WHITE_DETECTION_ERROR)
1011
        {global.stateTracker[16+(-states::WHITE_DETECTION_ERROR)] += 1;}
1012
      else if (newState == states::PROXY_DETECTION_ERROR)
1013
        {global.stateTracker[16+(-states::PROXY_DETECTION_ERROR)] += 1;}
1014
      else if (newState == states::NO_CHARGING_POWER_ERROR)
1015
        {global.stateTracker[16+(-states::NO_CHARGING_POWER_ERROR)] += 1;}
1016
      else if (newState == states::UNKNOWN_STATE_ERROR)
1017
        {global.stateTracker[16+(-states::UNKNOWN_STATE_ERROR)] += 1;}
1018
    }
1019

    
1020
    // Keep track of last state and set the new state for next iteration
1021
    prevState = currentState;
1022
    currentState = newState;
1023

    
1024
    this->sleep(CAN::UPDATE_PERIOD);
1025
  }
1026

    
1027
  return RDY_OK;
1028
}