Statistics
| Branch: | Tag: | Revision:

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

History | View | Annotate | Download (32.2 KB)

1
#include "global.hpp"
2
#include <cmath>
3
#include "linefollow.hpp"
4
#include "userthread.hpp"
5
#include "amiro_map.hpp"
6

    
7
using namespace amiro;
8

    
9
extern Global global;
10

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

    
15

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

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

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

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

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

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

    
72
// ----------------------------------------------------------------
73

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

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

    
82
}
83

    
84

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

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

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

    
101

    
102

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

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

    
126
  // 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]);
127

    
128

    
129
}
130
// -------------------------------------------------------------------
131

    
132

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

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

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

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

    
153
}
154

    
155

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    
450

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

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

    
469
          setRpmSpeed(rpmSpeed);
470
        }
471

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

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

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

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

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

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

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

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

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

    
632
        utCount.stateTime++;
633

    
634

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

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

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

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

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

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

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

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

    
787

    
788

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

    
791
      // --------------------------------------------------
792

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

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

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

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

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

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

    
843

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

    
874

    
875

    
876

    
877

    
878
      int failed = 0;
879
      int passed = 0;
880
      for (int i = 0; i <= global.tcase; i++) {
881
        if (global.testres[i]) {
882
          passed++;
883
          chprintf((BaseSequentialStream *)&global.sercanmux1,
884
                   "Test %d Passed!\n", i);
885
        } else {
886
          failed++;
887
          chprintf((BaseSequentialStream *)&global.sercanmux1,
888
                   "Test %d Failed\n", i);
889
        }
890
      }
891
      chprintf((BaseSequentialStream *)&global.sercanmux1,
892
               "Total: %d, Passed: %d, Failed: %d\n", global.tcase + 1, passed,
893
               failed);
894

    
895
      newState = states::IDLE;
896
      break;
897
    }
898
      // --------------------------------------------------
899
    default:
900
      newState = states::UNKNOWN_STATE_ERROR;
901
      break;
902
    }
903

    
904
    // In case a new state is set:
905
    // 1. Record the state transition
906
    if (currentState != newState){
907

    
908
      global.stateTransitionCounter++;
909
      // Clear all state transitions to prevent overflow
910
      if (global.stateTransitionCounter >= 255) {
911
        global.stateTransitionCounter = 0;
912
        for (int i = 0; i < 24; i++) {
913
          global.stateTracker[i] = 0;
914
        }
915
}
916
      // Transmit the new state over can
917
      chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
918
      global.robot.transmitState(newState);
919

    
920
      // Increase state count for specific state
921
      // TODO: Improve with dictionary or other than switch case
922
      if (newState == states::IDLE)
923
        {global.stateTracker[states::IDLE] += 1;}
924
      else if (newState == states::FOLLOW_LINE)
925
        {global.stateTracker[states::FOLLOW_LINE] += 1;}
926
      else if (newState == states::DETECT_STATION)
927
        {global.stateTracker[states::DETECT_STATION] += 1;}
928
      else if (newState == states::REVERSE)
929
        {global.stateTracker[states::REVERSE] += 1;}
930
      else if (newState == states::PUSH_BACK)
931
        {global.stateTracker[states::PUSH_BACK] += 1;}
932
      else if (newState == states::CHECK_POSITIONING)
933
        {global.stateTracker[states::CHECK_POSITIONING] += 1;}
934
      else if (newState == states::CHECK_VOLTAGE)
935
        {global.stateTracker[states::CHECK_VOLTAGE] += 1;}
936
      else if (newState == states::CHARGING)
937
        {global.stateTracker[states::CHARGING] += 1;}
938
      else if (newState == states::RELEASE)
939
        {global.stateTracker[states::RELEASE] += 1;}
940
      else if (newState == states::RELEASE_TO_CORRECT)
941
        {global.stateTracker[states::RELEASE_TO_CORRECT] += 1;}
942
      else if (newState == states::CORRECT_POSITIONING)
943
        {global.stateTracker[states::CORRECT_POSITIONING] += 1;}
944
      else if (newState == states::TURN)
945
        {global.stateTracker[states::TURN] += 1;}
946
      else if (newState == states::INACTIVE)
947
        {global.stateTracker[states::INACTIVE] += 1;}
948
      else if (newState == states::CALIBRATION)
949
        {global.stateTracker[states::CALIBRATION] += 1;}
950
      else if (newState == states::CALIBRATION_CHECK)
951
        {global.stateTracker[states::CALIBRATION_CHECK] += 1;}
952
      else if (newState == states::DEVIATION_CORRECTION)
953
        {global.stateTracker[states::DEVIATION_CORRECTION] += 1;}
954
      else if (newState == states::DOCKING_ERROR)
955
        {global.stateTracker[16+(-states::DOCKING_ERROR)] += 1;}
956
      else if (newState == states::REVERSE_TIMEOUT_ERROR)
957
        {global.stateTracker[16+(-states::REVERSE_TIMEOUT_ERROR)] += 1;}
958
      else if (newState == states::CALIBRATION_ERROR)
959
        {global.stateTracker[16+(-states::CALIBRATION_ERROR)] += 1;}
960
      else if (newState == states::WHITE_DETECTION_ERROR)
961
        {global.stateTracker[16+(-states::WHITE_DETECTION_ERROR)] += 1;}
962
      else if (newState == states::PROXY_DETECTION_ERROR)
963
        {global.stateTracker[16+(-states::PROXY_DETECTION_ERROR)] += 1;}
964
      else if (newState == states::NO_CHARGING_POWER_ERROR)
965
        {global.stateTracker[16+(-states::NO_CHARGING_POWER_ERROR)] += 1;}
966
      else if (newState == states::UNKNOWN_STATE_ERROR)
967
        {global.stateTracker[16+(-states::UNKNOWN_STATE_ERROR)] += 1;}
968
    }
969

    
970
    // Keep track of last state and set the new state for next iteration
971
    prevState = currentState;
972
    currentState = newState;
973

    
974
    this->sleep(CAN::UPDATE_PERIOD);
975
  }
976

    
977
  return RDY_OK;
978
}