Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / userthread.cpp @ 84b4c632

History | View | Annotate | Download (24.1 KB)

1
// #include "userthread.hpp"
2
#include "global.hpp"
3
#include <cmath>
4
#include "linefollow.hpp" 
5
// #include <cmath>
6
// #include "global.hpp"
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] / 4;
142
      rpmSpeed[1] = rpmSpeed[1] / 4;
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
  int motion = 1;
161
  int led = 0;
162
  types::position oldPos = global.odometry.getPosition();
163
  while(motion){
164
    this->sleep(500);
165
    types::position tmp = global.odometry.getPosition();
166
    motion = 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

    
251
UserThread::UserThread() :
252
  chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()
253
{
254
}
255

    
256
UserThread::~UserThread()
257
{
258
}
259

    
260
msg_t
261
UserThread::main()
262
{
263
  /*
264
   * SETUP
265
   */
266
  // User thread state:
267

    
268
  for (uint8_t led = 0; led < 8; ++led) {
269
    global.robot.setLightColor(led, Color(Color::BLACK));
270
  }
271
  running = false;
272
  LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT;
273
  LineFollow lf(&global);
274
  /*
275
   * LOOP
276
   */
277
  while (!this->shouldTerminate())
278
  {
279
    /*
280
    * read accelerometer z-value
281
    */
282
    accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);
283
        
284
    if (accel_z < -900 /*-0.9g*/) { 
285
      // Start line following when AMiRo is rotated
286
      if(currentState == states::INACTIVE){
287
        newState = states::FOLLOW_LINE;
288
      }else{
289
        newState = states::IDLE;
290
      }
291
      lightAllLeds(Color::GREEN);
292
      this->sleep(1000);
293
      lightAllLeds(Color::BLACK);
294

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

    
346
    // Get sensor data 
347
    // uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
348
    // uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
349
    for(int i=0; i<8;i++){
350
      rProx[i] = global.robot.getProximityRingValue(i);
351
    }
352

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

    
376
        proxCalib.buf = 0;
377
        if(proxCalib.calibrateBlack){
378
          chprintf((BaseSequentialStream*)&global.sercanmux1, "Black Calibration, Place AMiRo on black Surface!\n");
379
          global.robot.calibrate();
380
        }
381
        for(int i=0; i <= proxCalib.meanWindow; i++){
382
          proxCalib.buf += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset() 
383
                          + global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); 
384
          this->sleep(CAN::UPDATE_PERIOD);
385
        }
386
        proxCalib.buf = proxCalib.buf / (2*proxCalib.meanWindow);
387
        
388
        if(proxCalib.calibrateBlack){
389
          global.linePID.BThresh = proxCalib.buf;
390
        }else  {
391
          global.linePID.WThresh  = proxCalib.buf;
392
        }
393
          chprintf((BaseSequentialStream*)&global.sercanmux1, "Black: %d, White: %d!\n", global.linePID.BThresh, global.linePID.WThresh);
394

    
395
        newState = states::IDLE;
396
      break;
397
      // ---------------------------------------
398
      case states::IDLE:
399
        global.motorcontrol.setMotorEnable(true);
400
        setRpmSpeed(stop);
401
        if(/* checkPinVoltage() && */ checkPinEnabled()){
402
          global.robot.requestCharging(0);
403
        }
404
        // pCtrl.pFactor = 0;
405
        pCtrl.staticCont = 0;
406
        utCount.whiteCount = 0;
407
        utCount.ringProxCount = 0;
408
        utCount.errorCount = 0;
409
        newState = states::INACTIVE;
410
      break;
411
      // ---------------------------------------
412
      case states::FOLLOW_LINE:
413
      // Set correct forward speed to every strategy
414
        if (global.forwardSpeed != global.rpmForward[0]){
415
          global.forwardSpeed = global.rpmForward[0];
416
        }
417
        
418
        if(lf.getStrategy() != lStrategy){
419
          lf.setStrategy(lStrategy);
420
        }
421

    
422
        if(lf.followLine(rpmSpeed)){
423
          utCount.whiteCount++;
424
          if(utCount.whiteCount >= WHITE_DETETION_TIMEOUT){
425
            setRpmSpeed(stop);
426
            utCount.whiteCount = 0;
427
            newState = states::WHITE_DETECTION_ERROR;
428
          }
429
        }else{
430
          utCount.whiteCount = 0;
431
        }
432

    
433
        preventCollision(rpmSpeed, rProx);
434
        // proxSectorSpeedCorrection(rpmSpeed, rProx);
435

    
436
        if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){
437
          utCount.ringProxCount = 0;
438
          newState = states::TURN;
439
        }
440

    
441
        if (lf.getStrategy() == LineFollowStrategy::FUZZY){
442
          setRpmSpeedFuzzy(rpmSpeed);
443
        }else{
444

    
445
          setRpmSpeed(rpmSpeed);
446
        }
447
        
448
      break;
449
      // ---------------------------------------
450
      case states::TURN:
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
          newState = states::FOLLOW_LINE;
458
          // lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
459
        }else{
460
          newState = states::PROXY_DETECTION_ERROR;
461
        }
462
      break;
463
      // ---------------------------------------
464
      case states::DETECT_STATION:
465
        if (global.forwardSpeed != DETECTION_SPEED){
466
          global.forwardSpeed = DETECTION_SPEED;
467
        }
468
        if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
469
          lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
470
        }
471

    
472
        lf.followLine(rpmSpeed);
473
        setRpmSpeed(rpmSpeed);
474
        // // Detect marker before docking station
475
        // if ((WL+WR) < PROXY_WHEEL_THRESH){
476
        // Use proxy ring 
477
        if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){
478

    
479
          setRpmSpeed(stop);
480
          checkForMotion();
481
          // 180° Rotation 
482
          global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
483
          // BaseThread::sleep(8000);
484
          checkForMotion();
485
          newState = states::CORRECT_POSITIONING;
486
        }
487
      break;
488
      // ---------------------------------------
489
      case states::CORRECT_POSITIONING:
490
        if (global.forwardSpeed != CHARGING_SPEED){
491
          global.forwardSpeed = CHARGING_SPEED;
492
        }
493
        if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
494
          lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
495
        }
496
        lf.followLine(rpmSpeed);
497
        setRpmSpeed(rpmSpeed);
498

    
499
        utCount.stateTime++;
500
        if (utCount.stateTime >= DOCKING_CORRECTION_TIMEOUT){
501
          utCount.stateTime = 0;
502
          newState = states::REVERSE;
503
          setRpmSpeed(stop);
504
          checkForMotion();
505
        }
506
      break;
507
      // ---------------------------------------
508
      case states::REVERSE:
509
        if(lf.getStrategy() != LineFollowStrategy::REVERSE){
510
          lf.setStrategy(LineFollowStrategy::REVERSE);
511
        }
512
        lf.followLine(rpmSpeed);
513
        setRpmSpeed(rpmSpeed);
514
        // utCount.stateTime++;
515

    
516
        // Docking is only successful if Deviation is in range and sensors are at their max values.
517
        if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL) && ((devCor.currentDeviation > -MAX_DEVIATION_FACTOR) &&  (devCor.currentDeviation < MAX_DEVIATION_FACTOR) )){
518
          // setRpmSpeed(stop);
519
          // checkForMotion();
520
          utCount.stateTime = 0;
521
          newState = states::PUSH_BACK;
522
        }else if ((devCor.currentDeviation <= -MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){
523
          // Case R 
524
          utCount.stateTime = 0;
525
          setRpmSpeed(stop);
526
          devCor.RCase = true;
527
          lightAllLeds(Color::YELLOW);
528
          newState = states::DEVIATION_CORRECTION;
529
        }else if ((devCor.currentDeviation >= MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){
530
          // Case L 
531
          utCount.stateTime = 0;
532
          setRpmSpeed(stop);
533
          devCor.RCase = false;
534
          lightAllLeds(Color::WHITE);
535
          newState = states::DEVIATION_CORRECTION;
536
        }else if (utCount.stateTime >= REVERSE_DOCKING_TIMEOUT){
537
          setRpmSpeed(stop);
538
          utCount.stateTime = 0;
539
          utCount.errorCount++;
540
          if (utCount.errorCount >= DOCKING_ERROR_THRESH){
541
            newState = states::DOCKING_ERROR;
542
          }else{
543
            newState = states::CORRECT_POSITIONING;
544
          }
545
        }
546

    
547
        // if((devCor.currentDeviation <= -10)){
548
        //   rpmSpeed[0] -= 2000000;
549
        // }else if(devCor.currentDeviation >= 10){
550
        //   rpmSpeed[1] -= 2000000;
551
        // }
552
        // setRpmSpeed(rpmSpeed);
553
      break;
554
      // ---------------------------------------
555
      case states::DEVIATION_CORRECTION:
556
        // if(lf.getStrategy() != LineFollowStrategy::REVERSE){
557
        //   lf.setStrategy(LineFollowStrategy::REVERSE);
558
        // }
559
        // lf.followLine(rpmSpeed);
560
        // setRpmSpeed(rpmSpeed);
561
        if(utCount.stateTime < DEVIATION_CORRECTION_DURATION / 2 ){
562
          if(devCor.RCase){
563
            rpmSpeed[0] = 0;
564
            rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;
565
          }else {
566
            rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;
567
            rpmSpeed[1] = 0;
568
          }
569
          setRpmSpeed(rpmSpeed);
570
        }else if (((utCount.stateTime >= DEVIATION_CORRECTION_DURATION / 2) && (utCount.stateTime < DEVIATION_CORRECTION_DURATION +10)) ){
571
          if(devCor.RCase){
572
            rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;
573
            rpmSpeed[1] = 0;
574
          }else {
575
            rpmSpeed[0] = 0;
576
            rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;
577
          }
578
          setRpmSpeed(rpmSpeed);
579
          if(((devCor.currentDeviation >= -5) && (devCor.currentDeviation <= 5))){
580
            utCount.stateTime = 0;
581
            newState = states::REVERSE;
582
            setRpmSpeed(stop);
583
          }
584
        }else{
585
          utCount.stateTime = 0;
586
          newState = states::REVERSE;
587
          setRpmSpeed(stop);
588
        }
589

    
590
        utCount.stateTime++;
591

    
592

    
593
        // if (utCount.stateTime > PUSH_BACK_TIMEOUT){
594
        //   utCount.stateTime = 0;
595
        //   newState = states::CHECK_POSITIONING;
596
        // }
597
      break;
598
      // ---------------------------------------
599
      case states::PUSH_BACK:
600
        if(lf.getStrategy() != LineFollowStrategy::REVERSE){
601
          lf.setStrategy(LineFollowStrategy::REVERSE);
602
        }
603
        lf.followLine(rpmSpeed);
604
        setRpmSpeed(rpmSpeed);
605

    
606
        utCount.stateTime++;
607
        if (utCount.stateTime > PUSH_BACK_TIMEOUT){
608
          utCount.stateTime = 0;
609
          newState = states::CHECK_POSITIONING;
610
        }
611
      break;
612
      // ---------------------------------------
613
      case states::CHECK_POSITIONING:
614
        setRpmSpeed(stop);
615
        checkForMotion();
616
        if(checkDockingSuccess()){
617
          newState = states::CHECK_VOLTAGE;
618
        }else{
619
          utCount.errorCount++;
620
          newState = states::CORRECT_POSITIONING;
621
          if (utCount.errorCount >= DOCKING_ERROR_THRESH){
622
              newState = states::DOCKING_ERROR;
623
            }
624
        }
625
      break;
626
      // ---------------------------------------
627
      case states::CHECK_VOLTAGE:
628
        if(!checkPinEnabled()){
629
          global.robot.requestCharging(1);
630
        } else {
631
          if(checkPinVoltage()){
632
            // Pins are under voltage -> correctly docked 
633
            
634
            newState = states::CHARGING;
635
          }else{
636
            utCount.errorCount++;
637
            // No voltage on pins -> falsely docked
638
            // deactivate pins
639
            global.motorcontrol.setMotorEnable(true);
640
            global.robot.requestCharging(0);
641
            // TODO: Soft release when docking falsely
642
            if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
643
              newState = states::RELEASE_TO_CORRECT;
644
            } else {
645
              newState = states::RELEASE_TO_CORRECT; //states::CORRECT_POSITIONING; 
646
            }
647

    
648
            if (utCount.errorCount > DOCKING_ERROR_THRESH){
649
              newState = states::DOCKING_ERROR;
650
            }
651
          }
652
        }
653
      break;
654
      // ---------------------------------------
655
      case states::RELEASE_TO_CORRECT:
656

    
657
        global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
658
        checkForMotion();
659
        // move 1cm forward
660
        global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
661
        checkForMotion();
662
        // rotate back
663
        global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION);
664
        checkForMotion();
665

    
666
        global.distcontrol.setTargetPosition(1500, 0, ROTATION_DURATION);
667
        checkForMotion();
668
        newState = states::CORRECT_POSITIONING;
669
      break;
670
      // ---------------------------------------
671
      case states::CHARGING:
672
        global.motorcontrol.setMotorEnable(false);
673
        utCount.errorCount = 0;
674
        // Formulate Request to enable charging
675
        if(/* checkPinVoltage() && */ !checkPinEnabled()){
676
          global.robot.requestCharging(1);
677
        }
678
        if(checkPinEnabled()){
679
          showChargingState();
680
        }
681
      break;
682
      // ---------------------------------------
683
      case states::RELEASE:
684
      if (global.forwardSpeed != DETECTION_SPEED){
685
          global.rpmForward[0] = DETECTION_SPEED;
686
        }
687
        if(/* checkPinVoltage() && */ checkPinEnabled()){
688
          global.robot.requestCharging(0);
689
        }else{
690
          global.motorcontrol.setMotorEnable(true);
691

    
692
          //Rotate -20° to free from magnet
693
          global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
694
          checkForMotion();
695
          // move 1cm forward
696
          global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
697
          checkForMotion();
698
          // rotate back
699
          // global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
700
          // checkForMotion();
701

    
702
          // global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
703
          // checkForMotion();
704
          lStrategy = LineFollowStrategy::EDGE_RIGHT;
705
          newState = states::FOLLOW_LINE;
706
            // whiteBuf = -100;
707
          // lf.followLine(rpmSpeed);
708
          // setRpmSpeed(rpmSpeed);
709
        }
710
        // lightAllLeds(Color::BLACK);
711
      break;
712
      // ---------------------------------------
713
      case states::DOCKING_ERROR:
714
        newState = states::RELEASE;
715
      break;
716
      // ---------------------------------------
717
      case states::REVERSE_TIMEOUT_ERROR:
718
        newState = states::IDLE;
719
      break;
720
      // ---------------------------------------
721
      case states::CALIBRATION_ERROR:
722
        newState = states::IDLE;
723
      break;
724
      // ---------------------------------------
725
      case states::WHITE_DETECTION_ERROR:
726
        newState = states::IDLE;
727
      break;
728
      // ---------------------------------------
729
      case states::PROXY_DETECTION_ERROR:
730
        newState = states::IDLE;
731
      break;
732
      // ---------------------------------------
733
      case states::NO_CHARGING_POWER_ERROR:
734
        newState = states::IDLE;
735
      break;
736
      // ---------------------------------------
737
      case states::UNKNOWN_STATE_ERROR:
738
        newState = states::IDLE;
739
      break;
740
      // ---------------------------------------
741
      default:
742
        newState = states::UNKNOWN_STATE_ERROR;
743
      break;
744
      }
745
      if (currentState != newState){
746
        chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
747
        global.robot.transmitState(newState);
748
      }
749
      prevState = currentState;
750
      currentState = newState;
751
      // if (utCount.stateCount > CAN_TRANSMIT_STATE_THRESH){
752
      //     utCount.stateCount = 0;
753
      //   // chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
754
      //   global.robot.transmitState(currentState);
755
      //   // global.robot.setOdometry(global.odometry.getPosition());
756
        
757
      // }else{
758
      //   utCount.stateCount++;
759
      // }
760
    this->sleep(CAN::UPDATE_PERIOD);
761
  }
762

    
763
  return RDY_OK;
764
}