Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / userthread.cpp @ 4d54a507

History | View | Annotate | Download (36.9 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

    
277
  // State Variables
278
  ut_states prevState = ut_states::UT_IDLE;
279
  ut_states currentState = ut_states::UT_INACTIVE;
280
  ut_states newState = ut_states::UT_INACTIVE;
281

    
282

    
283
  running = false;
284
  LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT;
285
  LineFollow lf(&global);
286
  AmiroMap map(&global, lStrategy);
287
  /*
288
   * LOOP
289
   */
290
  while (!this->shouldTerminate())
291
  {
292
    /*
293
    * read accelerometer z-value
294
    */
295
    accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);
296

    
297
    if (accel_z < -900 /*-0.9g*/) {
298
      // Start line following when AMiRo is rotated
299
      if(currentState == ut_states::UT_INACTIVE){
300
        newState = ut_states::UT_FOLLOW_LINE;
301
      }else{
302
        newState = ut_states::UT_IDLE;
303
      }
304
      lightAllLeds(Color::GREEN);
305
      this->sleep(1000);
306
      lightAllLeds(Color::BLACK);
307

    
308
    // If message was received handle it here:
309
    } else if(global.msgReceived){
310
      global.msgReceived = false;
311
      // running = true;
312
      switch(global.lfStrategy){
313
      case msg_content::MSG_START:
314
        newState = ut_states::UT_CALIBRATION_CHECK;
315
        break;
316
      case msg_content::MSG_STOP:
317
        newState = ut_states::UT_IDLE;
318
        break;
319
      case msg_content::MSG_EDGE_RIGHT:
320
        // newState = ut_states::UT_FOLLOW_LINE;
321
        lStrategy = LineFollowStrategy::EDGE_RIGHT;
322
        break;
323
      case  msg_content::MSG_EDGE_LEFT:
324
        // newState = ut_states::UT_FOLLOW_LINE;
325
        lStrategy = LineFollowStrategy::EDGE_LEFT;
326
        break;
327
      case msg_content::MSG_FUZZY:
328
        // newState = ut_states::UT_FOLLOW_LINE;
329
        lStrategy = LineFollowStrategy::FUZZY;
330
        break;
331
      case msg_content::MSG_DOCK:
332
        newState = ut_states::UT_DETECT_STATION;
333
        break;
334
      case msg_content::MSG_UNDOCK:
335
        newState = ut_states::UT_RELEASE;
336
        break;
337
      case msg_content::MSG_CHARGE:
338
        newState = ut_states::UT_CHARGING;
339
        break;
340
      case msg_content::MSG_RESET_ODOMETRY:
341
        global.odometry.resetPosition();
342
        break;
343
      case msg_content::MSG_CALIBRATE_BLACK:
344
        proxCalib.calibrateBlack = true;
345
        // global.odometry.resetPosition();
346
        newState = ut_states::UT_CALIBRATION;
347
        break;
348
      case msg_content::MSG_CALIBRATE_WHITE:
349
        proxCalib.calibrateBlack = false;
350
        newState = ut_states::UT_CALIBRATION;
351
        break;
352
      case msg_content::MSG_TEST_MAP_STATE:
353
        if (AMIRO_MAP_AUTO_TRACKING){
354
          newState = ut_states::UT_TEST_MAP_AUTO_STATE;
355
        }else {
356
          newState = ut_states::UT_TEST_MAP_STATE;
357
        }
358
        break;
359
      case msg_content::MSG_SET_DIST_THRESH:
360

    
361

    
362
        break;
363
      case msg_content::MSG_GET_MAP_INFO:
364
        break;
365
      default:
366
        newState = ut_states::UT_IDLE;
367
        break;
368
      }
369
    }
370
    // newState = currentState;
371

    
372
    // Get sensor data
373
    uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
374
    uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
375
    for(int i=0; i<8;i++){
376
      rProx[i] = global.robot.getProximityRingValue(i);
377
    }
378

    
379
    // Continously update devication values
380
    meanDeviation(rProx[0] & 0xFFF0, rProx[7] & 0xFFF0);
381
    // int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
382
    // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
383
    switch(currentState){
384
      case ut_states::UT_INACTIVE:
385
        // Dummy state to deactivate every interaction
386
      break;
387
      // ---------------------------------------
388
      case ut_states::UT_CALIBRATION_CHECK:
389
        // global.robot.calibrate();
390
        if(global.linePID.BThresh >= global.linePID.WThresh){
391
          newState = ut_states::UT_CALIBRATION_ERROR;
392
        }else{
393
          newState = ut_states::UT_FOLLOW_LINE;
394
        }
395
      break;
396
      // ---------------------------------------
397
      case ut_states::UT_CALIBRATION:
398
        /* Calibrate the global thresholds for black or white.
399
            This values will be used by the line follow object
400
        */
401

    
402
        proxCalib.buf = 0;
403
        if(proxCalib.calibrateBlack){
404
          chprintf((BaseSequentialStream*)&global.sercanmux1, "Black Calibration, Place AMiRo on black Surface!\n");
405
          global.robot.calibrate();
406
        }
407
        for(int i=0; i <= proxCalib.meanWindow; i++){
408
          proxCalib.buf += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset()
409
                          + global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
410
          this->sleep(CAN::UPDATE_PERIOD);
411
        }
412
        proxCalib.buf = proxCalib.buf / (2*proxCalib.meanWindow);
413

    
414
        if(proxCalib.calibrateBlack){
415
          global.linePID.BThresh = proxCalib.buf;
416
        }else  {
417
          global.linePID.WThresh  = proxCalib.buf;
418
        }
419
          chprintf((BaseSequentialStream*)&global.sercanmux1, "Black: %d, White: %d!\n", global.linePID.BThresh, global.linePID.WThresh);
420

    
421
        newState = ut_states::UT_IDLE;
422
      break;
423
      // ---------------------------------------
424
      case ut_states::UT_IDLE:
425
        global.motorcontrol.setMotorEnable(true);
426
        setRpmSpeed(stop);
427
        if(/* checkPinVoltage() && */ checkPinEnabled()){
428
          global.robot.requestCharging(0);
429
        }
430
        // pCtrl.pFactor = 0;
431
        pCtrl.staticCont = 0;
432
        utCount.whiteCount = 0;
433
        utCount.ringProxCount = 0;
434
        utCount.errorCount = 0;
435
        newState = ut_states::UT_INACTIVE;
436
      break;
437
      // ---------------------------------------
438
      case ut_states::UT_FOLLOW_LINE:
439
      // Set correct forward speed to every strategy
440
        if (global.forwardSpeed != global.rpmForward[0]){
441
          global.forwardSpeed = global.rpmForward[0];
442
        }
443

    
444
        if(lf.getStrategy() != lStrategy){
445
          lf.setStrategy(lStrategy);
446
        }
447

    
448
        if(lf.followLine(rpmSpeed)){
449
          utCount.whiteCount++;
450
          if(utCount.whiteCount >= WHITE_DETETION_TIMEOUT){
451
            setRpmSpeed(stop);
452
            utCount.whiteCount = 0;
453
            newState = ut_states::UT_WHITE_DETECTION_ERROR;
454
          }
455
        }else{
456
          utCount.whiteCount = 0;
457
        }
458

    
459
        preventCollision(rpmSpeed, rProx);
460
        // proxSectorSpeedCorrection(rpmSpeed, rProx);
461

    
462
        if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){
463
          utCount.ringProxCount = 0;
464

    
465

    
466
          checkForMotion();
467
          // Check if only front sensors are active
468
          if (checkFrontalObject()) {
469
            // global.distcontrol.setTargetPosition(0, 2792526, ROTATION_DURATION);
470
            // // BaseThread::sleep(8000);
471
            // checkForMotion();
472
            this->utCount.whiteCount = 0;
473
            newState = ut_states::UT_TURN;
474
            // lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
475
          } else {
476
            newState = ut_states::UT_PROXY_DETECTION_ERROR;
477
          }
478
        }
479

    
480
        if (lf.getStrategy() == LineFollowStrategy::FUZZY){
481
          setRpmSpeedFuzzy(rpmSpeed);
482
        }else{
483

    
484
          setRpmSpeed(rpmSpeed);
485
        }
486

    
487
      break;
488
      // ---------------------------------------
489
    case ut_states::UT_TURN:{
490
        // Check the line strategy in order to continue driving on the right side
491
      int factor = SPEED_CONVERSION_FACTOR;
492
      int frontL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
493
      int frontR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
494
      int blackSensor = 0;
495
      if (lf.getStrategy() == LineFollowStrategy::EDGE_RIGHT) {
496
        factor = -factor;
497
        blackSensor = frontL;
498
      }else{
499
        blackSensor = frontR;
500
      }
501

    
502
      rpmSpeed[0] = factor * CHARGING_SPEED;
503
      rpmSpeed[1] = -factor * CHARGING_SPEED;
504
      setRpmSpeed(rpmSpeed);
505

    
506
      if ((blackSensor >= global.linePID.WThresh )){
507
        utCount.whiteCount = 1;
508
      }else {
509
        if((utCount.whiteCount == 1) && (blackSensor <= global.linePID.BThresh)){
510
          utCount.whiteCount = 0;
511
          newState = ut_states::UT_FOLLOW_LINE;
512
          setRpmSpeed(stop);
513
        }
514
      }
515
      break;
516
    }
517
      // ---------------------------------------
518
    case ut_states::UT_DETECT_STATION:
519

    
520
        if (global.forwardSpeed != DETECTION_SPEED){
521
          global.forwardSpeed = DETECTION_SPEED;
522
        }
523
        if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
524
          lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
525
        }
526

    
527
        lf.followLine(rpmSpeed);
528
        setRpmSpeed(rpmSpeed);
529
        // // Detect marker before docking station
530
        // if ((WL+WR) < PROXY_WHEEL_THRESH){
531
        // Use proxy ring
532
        if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){
533

    
534
          setRpmSpeed(stop);
535
          checkForMotion();
536
          // 180° Rotation
537
          global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
538
          // BaseThread::sleep(8000);
539
          checkForMotion();
540
          newState = ut_states::UT_CORRECT_POSITIONING;
541
        }
542
      break;
543
      // ---------------------------------------
544
    case ut_states::UT_CORRECT_POSITIONING:
545
        if (global.forwardSpeed != CHARGING_SPEED){
546
          global.forwardSpeed = CHARGING_SPEED;
547
        }
548
        if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
549
          lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
550
        }
551
        lf.followLine(rpmSpeed);
552
        setRpmSpeed(rpmSpeed);
553

    
554
        utCount.stateTime++;
555
        if (utCount.stateTime >= DOCKING_CORRECTION_TIMEOUT){
556
          utCount.stateTime = 0;
557
          newState = ut_states::UT_REVERSE;
558
          setRpmSpeed(stop);
559
          checkForMotion();
560
        }
561
      break;
562
      // ---------------------------------------
563
    case ut_states::UT_REVERSE:
564
        if(lf.getStrategy() != LineFollowStrategy::REVERSE){
565
          lf.setStrategy(LineFollowStrategy::REVERSE);
566
        }
567
        lf.followLine(rpmSpeed);
568
        setRpmSpeed(rpmSpeed);
569
        // utCount.stateTime++;
570

    
571
        // Docking is only successful if Deviation is in range and sensors are at their max values.
572
        if((rProx[0] >= PROX_MAX_VAL)
573
           && (rProx[7] >= PROX_MAX_VAL)
574
           && ((devCor.currentDeviation > -MAX_DEVIATION_FACTOR) && (devCor.currentDeviation < MAX_DEVIATION_FACTOR) )){
575
          // setRpmSpeed(stop);
576
          // checkForMotion();
577
          utCount.stateTime = 0;
578
          newState = ut_states::UT_PUSH_BACK;
579
        }else if ((devCor.currentDeviation <= -MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){
580
          // Case R
581
          utCount.stateTime = 0;
582
          setRpmSpeed(stop);
583
          devCor.RCase = true;
584
          lightAllLeds(Color::YELLOW);
585
          newState = ut_states::UT_DEVIATION_CORRECTION;
586
        }else if ((devCor.currentDeviation >= MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){
587
          // Case L
588
          utCount.stateTime = 0;
589
          setRpmSpeed(stop);
590
          devCor.RCase = false;
591
          lightAllLeds(Color::WHITE);
592
          newState = ut_states::UT_DEVIATION_CORRECTION;
593
        }else if (utCount.stateTime >= REVERSE_DOCKING_TIMEOUT){
594
          setRpmSpeed(stop);
595
          utCount.stateTime = 0;
596
          utCount.errorCount++;
597
          if (utCount.errorCount >= DOCKING_ERROR_THRESH){
598
            newState = ut_states::UT_DOCKING_ERROR;
599
          }else{
600
            newState = ut_states::UT_CORRECT_POSITIONING;
601
          }
602
        }
603

    
604
        // if((devCor.currentDeviation <= -10)){
605
        //   rpmSpeed[0] -= 2000000;
606
        // }else if(devCor.currentDeviation >= 10){
607
        //   rpmSpeed[1] -= 2000000;
608
        // }
609
        // setRpmSpeed(rpmSpeed);
610
      break;
611
      // ---------------------------------------
612
    case ut_states::UT_DEVIATION_CORRECTION:
613
        // if(lf.getStrategy() != LineFollowStrategy::REVERSE){
614
        //   lf.setStrategy(LineFollowStrategy::REVERSE);
615
        // }
616
        // lf.followLine(rpmSpeed);
617
        // setRpmSpeed(rpmSpeed);
618
        if(utCount.stateTime < DEVIATION_CORRECTION_DURATION / 2 ){
619
          if(devCor.RCase){
620
            rpmSpeed[0] = 0;
621
            rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;
622
          }else {
623
            rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;
624
            rpmSpeed[1] = 0;
625
          }
626
          setRpmSpeed(rpmSpeed);
627
        }else if (((utCount.stateTime >= DEVIATION_CORRECTION_DURATION / 2) && (utCount.stateTime < DEVIATION_CORRECTION_DURATION +10)) ){
628
          if(devCor.RCase){
629
            rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;
630
            rpmSpeed[1] = 0;
631
          }else {
632
            rpmSpeed[0] = 0;
633
            rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;
634
          }
635
          setRpmSpeed(rpmSpeed);
636
          if(((devCor.currentDeviation >= -5) && (devCor.currentDeviation <= 5))){
637
            utCount.stateTime = 0;
638
            newState = ut_states::UT_REVERSE;
639
            setRpmSpeed(stop);
640
          }
641
        }else{
642
          utCount.stateTime = 0;
643
          newState = ut_states::UT_REVERSE;
644
          setRpmSpeed(stop);
645
        }
646

    
647
        utCount.stateTime++;
648

    
649

    
650
        // if (utCount.stateTime > PUSH_BACK_TIMEOUT){
651
        //   utCount.stateTime = 0;
652
        //   newState = ut_states::UT_CHECK_POSITIONING;
653
        // }
654
      break;
655
      // ---------------------------------------
656
    case ut_states::UT_PUSH_BACK:
657
        if(lf.getStrategy() != LineFollowStrategy::REVERSE){
658
          lf.setStrategy(LineFollowStrategy::REVERSE);
659
        }
660
        lf.followLine(rpmSpeed);
661
        setRpmSpeed(rpmSpeed);
662

    
663
        utCount.stateTime++;
664
        if (utCount.stateTime > PUSH_BACK_TIMEOUT){
665
          utCount.stateTime = 0;
666
          newState = ut_states::UT_CHECK_POSITIONING;
667
        }
668
      break;
669
      // ---------------------------------------
670
    case ut_states::UT_CHECK_POSITIONING:
671
        setRpmSpeed(stop);
672
        checkForMotion();
673
        if(checkDockingSuccess()){
674
          newState = ut_states::UT_CHECK_VOLTAGE;
675
        }else{
676
          utCount.errorCount++;
677
          newState = ut_states::UT_CORRECT_POSITIONING;
678
          if (utCount.errorCount >= DOCKING_ERROR_THRESH){
679
              newState = ut_states::UT_DOCKING_ERROR;
680
            }
681
        }
682
      break;
683
      // ---------------------------------------
684
    case ut_states::UT_CHECK_VOLTAGE:
685
        if(!checkPinEnabled()){
686
          global.robot.requestCharging(1);
687
        } else {
688
          if(checkPinVoltage()){
689
            // Pins are under voltage -> correctly docked
690

    
691
            newState = ut_states::UT_CHARGING;
692
          }else{
693
            utCount.errorCount++;
694
            // No voltage on pins -> falsely docked
695
            // deactivate pins
696
            global.motorcontrol.setMotorEnable(true);
697
            global.robot.requestCharging(0);
698
            // TODO: Soft release when docking falsely
699
            if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
700
              newState = ut_states::UT_RELEASE_TO_CORRECT;
701
            } else {
702
              newState = ut_states::UT_RELEASE_TO_CORRECT; //ut_states::UT_CORRECT_POSITIONING;
703
            }
704

    
705
            if (utCount.errorCount > DOCKING_ERROR_THRESH){
706
              newState = ut_states::UT_DOCKING_ERROR;
707
            }
708
          }
709
        }
710
      break;
711
      // ---------------------------------------
712
    case ut_states::UT_RELEASE_TO_CORRECT:
713

    
714
        global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
715
        checkForMotion();
716
        // move 1cm forward
717
        global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
718
        checkForMotion();
719
        // rotate back
720
        global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION);
721
        checkForMotion();
722

    
723
        global.distcontrol.setTargetPosition(1500, 0, ROTATION_DURATION);
724
        checkForMotion();
725
        newState = ut_states::UT_CORRECT_POSITIONING;
726
      break;
727
      // ---------------------------------------
728
    case ut_states::UT_CHARGING:
729
        global.motorcontrol.setMotorEnable(false);
730
        utCount.errorCount = 0;
731
        // Formulate Request to enable charging
732
        if(/* checkPinVoltage() && */ !checkPinEnabled()){
733
          global.robot.requestCharging(1);
734
        }
735
        if(checkPinEnabled()){
736
          showChargingState();
737
        }
738
      break;
739
      // ---------------------------------------
740
    case ut_states::UT_RELEASE:
741

    
742
      if (global.forwardSpeed != DETECTION_SPEED){
743
          global.rpmForward[0] = DETECTION_SPEED;
744
        }
745
        if(/* checkPinVoltage() && */ checkPinEnabled()){
746
          global.robot.requestCharging(0);
747
        }else{
748
          global.motorcontrol.setMotorEnable(true);
749
          // TODO: Use controlled
750
          //Rotate -20° to free from magnet
751
          global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
752
          checkForMotion();
753
          // move 1cm forward
754
          global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
755
          checkForMotion();
756
          // rotate back
757
          // global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
758
          // checkForMotion();
759

    
760
          // global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
761
          // checkForMotion();
762
          lStrategy = LineFollowStrategy::EDGE_RIGHT;
763
          newState = ut_states::UT_FOLLOW_LINE;
764
            // whiteBuf = -100;
765
          // lf.followLine(rpmSpeed);
766
          // setRpmSpeed(rpmSpeed);
767
        }
768
        // lightAllLeds(Color::BLACK);
769
      break;
770
      // ---------------------------------------
771
    case ut_states::UT_DOCKING_ERROR:
772
      newState = ut_states::UT_RELEASE;
773
      break;
774
      // ---------------------------------------
775
    case ut_states::UT_REVERSE_TIMEOUT_ERROR:
776
      newState = ut_states::UT_IDLE;
777
      break;
778
      // ---------------------------------------
779
    case ut_states::UT_CALIBRATION_ERROR:
780
      newState = ut_states::UT_IDLE;
781
      break;
782
      // ---------------------------------------
783
    case ut_states::UT_WHITE_DETECTION_ERROR:
784
      newState = ut_states::UT_IDLE;
785
      break;
786
      // ---------------------------------------
787
    case ut_states::UT_PROXY_DETECTION_ERROR:
788
      newState = ut_states::UT_IDLE;
789
      break;
790
      // ---------------------------------------
791
    case ut_states::UT_NO_CHARGING_POWER_ERROR:
792
      newState = ut_states::UT_IDLE;
793
      break;
794
      // ---------------------------------------
795
    case ut_states::UT_UNKNOWN_STATE_ERROR:
796
        newState = ut_states::UT_IDLE;
797
      break;
798
      // ---------------------------------------
799
    case ut_states::UT_TEST_MAP_AUTO_STATE:
800
      chprintf((BaseSequentialStream *)&global.sercanmux1,
801
               "Start autotracking: \n");
802

    
803
      chprintf((BaseSequentialStream *)&global.sercanmux1,
804
               "NodeCount: %d\n", map.getNodeCount());
805
      for (int i=0; i<map.getNodeCount(); i++) {
806
        chprintf((BaseSequentialStream *)&global.sercanmux1,
807
                 "ID: %d, l: %d, r: %d \n", map.getNodeList()[i].id,
808
                 map.getNodeList()[i].left, map.getNodeList()[i].right);
809
      }
810
      // Start test suit for trackUpdate
811
      map.reset();
812
      global.tcase = 0;
813
      newState = UT_TEST_MAP_STATE;
814
      break;
815
      // --------------------------------------------------
816
    case ut_states::UT_TEST_MAP_STATE:{
817
      // Test suit for amiro map
818

    
819

    
820

    
821
      // AmiroMap map = AmiroMap(&global);
822

    
823
      // --------------------------------------------------
824

    
825
      global.tcase++;
826
      // Set basic valid map configuration
827
      setAttributes(global.testmap, 0, 1, 2, 1);
828
      setAttributes(global.testmap, 1, 2, 2, 0);
829
      setAttributes(global.testmap, 2, 1, 0, 0);
830
      setAttributes(global.testmap, 3, 0, 0, 0xff);
831
      chprintf((BaseSequentialStream *)&global.sercanmux1, "Init Case: %d, res: %d\n",global.tcase, map.initialize());
832
      global.testres[global.tcase] = map.get_state()->valid;
833

    
834
      global.tcase++; // 1
835
      // Test map fail if first node is flagged with end
836
      setAttributes(global.testmap, 0, 1, 2, 0xff);
837
      map.initialize();
838
      global.testres[global.tcase] = !map.get_state()->valid;
839

    
840
      global.tcase++; // 2
841
      // Test if node 2 is set as start node
842
      setAttributes(global.testmap, 0, 1, 2, 0);
843
      setAttributes(global.testmap, 2, 1, 0, 1);
844
      map.initialize();
845
      global.testres[global.tcase] = map.get_state()->current == 2;
846

    
847
      global.tcase++; // 3
848
      // Test if non reachable nodes will trigger invalid map
849
      setAttributes(global.testmap, 3, 0, 0, 0);
850
      setAttributes(global.testmap, 4, 0, 0, 0xff);
851
      map.initialize();
852
      global.testres[global.tcase] = !map.get_state()->valid;
853

    
854
      global.tcase++; // 4
855
      // Test Reinitialization
856
      setAttributes(global.testmap, 0, 1, 2, 1);
857
      setAttributes(global.testmap, 1, 2, 2, 0);
858
      setAttributes(global.testmap, 2, 1, 0, 0);
859
      setAttributes(global.testmap, 3, 0, 0, 0xff);
860
      map.initialize();
861
      global.testres[global.tcase] = map.get_state()->valid;
862

    
863
      global.odometry.resetPosition();
864
      uint8_t ret = 0;
865
      global.tcase++; // 5
866
      // Test update under normal linefollowing without fixpoint
867
      ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT);
868
      chprintf((BaseSequentialStream *)&global.sercanmux1,
869
               "Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,
870
               map.get_state()->current, map.get_state()->next);
871
      // No case should be true because neither was a node visited nor
872
      // was a fixpoint detected.
873
      global.testres[global.tcase] = (ret == 0x4);
874

    
875

    
876
      global.odometry.setPosition(1.0, 0.0, 0.0);
877
      chprintf((BaseSequentialStream *)&global.sercanmux1, "Current Point: %d\n", global.odometry.getPosition().x);
878
      global.tcase++; // 6
879
      // Fixpoint on left side
880
      ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT);
881
      chprintf((BaseSequentialStream *)&global.sercanmux1,
882
               "Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,
883
               map.get_state()->current, map.get_state()->next);
884
      // No case should be true because neither was a node visited nor
885
      // was a fixpoint detected.
886
      // global.odometry
887
      global.testres[global.tcase] = (ret == 0x2)
888
        && (map.get_state()->strategy == 0x02)
889
        && (map.get_state()->dist == 0)
890
        && (map.get_state()->current == 2);
891

    
892

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

    
909
      global.odometry.setPosition(1.2, 0.0, 0.0);
910
      global.tcase++; // 8
911
      // Fixpoint on left side, no update should appear because fixpoint already
912
      // marked
913
      ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT);
914
      chprintf((BaseSequentialStream *)&global.sercanmux1,
915
               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n", global.tcase, ret,
916
               map.get_state()->current, map.get_state()->next, map.get_state()->dist, map.get_state()->eLength);
917
      // No case should be true because neither was a node visited nor
918
      // was a fixpoint detected.
919
      global.testres[global.tcase] =
920
          (ret == 0x04) && (map.get_state()->strategy == 0x02);
921

    
922
      global.odometry.setPosition(.5, 0.0, 0.0);
923
      chprintf((BaseSequentialStream *)&global.sercanmux1,
924
               "Current Point: %d\n", global.odometry.getPosition().x);
925
      global.tcase++; // 9
926
      // Fixpoint on left side, no update should appear because fixpoint already
927
      // marked
928
      ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT);
929
      chprintf((BaseSequentialStream *)&global.sercanmux1,
930
               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n",
931
               global.tcase, ret, map.get_state()->current,
932
               map.get_state()->next, map.get_state()->dist,
933
               map.get_state()->eLength);
934
      // No case should be true because neither was a node visited nor
935
      // was a fixpoint detected.
936
      global.testres[global.tcase] =
937
        (ret == 2) &&
938
        (map.get_state()->strategy == 2) &&
939
        // (map.get_state()->dist == 0) &&
940
        (map.get_state()->eLength == 50);
941

    
942
      global.odometry.setPosition(.75, 0.0, 0.0);
943
      chprintf((BaseSequentialStream *)&global.sercanmux1,
944
               "Current Point: %d\n", global.odometry.getPosition().x);
945
      global.tcase++; // 10
946
      // Fixpoint on left side, no update should appear because fixpoint already
947
      // marked
948
      ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT);
949
      chprintf((BaseSequentialStream *)&global.sercanmux1,
950
               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n",
951
               global.tcase, ret, map.get_state()->current,
952
               map.get_state()->next, map.get_state()->dist,
953
               map.get_state()->eLength);
954
      // No case should be true because neither was a node visited nor
955
      // was a fixpoint detected.
956
      global.testres[global.tcase] =
957
          (ret == 4) && (map.get_state()->strategy == 2) &&
958
          // (map.get_state()->dist == 50)
959
         (map.get_state()->eLength == 50);
960

    
961
      int failed = 0;
962
      int passed = 0;
963
      for (int i = 0; i <= global.tcase; i++) {
964
        if (global.testres[i]) {
965
          passed++;
966
          chprintf((BaseSequentialStream *)&global.sercanmux1,
967
                   "Test %d Passed!\n", i);
968
        } else {
969
          failed++;
970
          chprintf((BaseSequentialStream *)&global.sercanmux1,
971
                   "Test %d Failed\n", i);
972
        }
973
      }
974
      chprintf((BaseSequentialStream *)&global.sercanmux1,
975
               "Total: %d, Passed: %d, Failed: %d\n", global.tcase + 1, passed,
976
               failed);
977

    
978
      newState = ut_states::UT_IDLE;
979
      break;
980
    }
981
      // --------------------------------------------------
982
    default:
983
      newState = ut_states::UT_UNKNOWN_STATE_ERROR;
984
      break;
985
    }
986

    
987
    // In case a new state is set:
988
    // 1. Record the state transition
989
    if (AMIRO_MAP_AUTO_TRACKING) {
990
      // map.trackUpdate(WL, WR, lStrategy, currentState);
991
    }
992

    
993
    if (currentState != newState){
994

    
995
      global.stateTransitionCounter++;
996
      // Clear all state transitions to prevent overflow
997
      if (global.stateTransitionCounter >= 255) {
998
        global.stateTransitionCounter = 0;
999
        for (int i = 0; i < 24; i++) {
1000
          global.stateTracker[i] = 0;
1001
        }
1002
}
1003
      // Transmit the new state over can
1004
      chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
1005
      global.robot.transmitState(newState);
1006

    
1007
      // Increase state count for specific state
1008
      // TODO: Improve with dictionary or other than switch case
1009
      if (newState == ut_states::UT_IDLE)
1010
        {global.stateTracker[ut_states::UT_IDLE] += 1;}
1011
      else if (newState == ut_states::UT_FOLLOW_LINE)
1012
        {global.stateTracker[ut_states::UT_FOLLOW_LINE] += 1;}
1013
      else if (newState == ut_states::UT_DETECT_STATION)
1014
        {global.stateTracker[ut_states::UT_DETECT_STATION] += 1;}
1015
      else if (newState == ut_states::UT_REVERSE)
1016
        {global.stateTracker[ut_states::UT_REVERSE] += 1;}
1017
      else if (newState == ut_states::UT_PUSH_BACK)
1018
        {global.stateTracker[ut_states::UT_PUSH_BACK] += 1;}
1019
      else if (newState == ut_states::UT_CHECK_POSITIONING)
1020
        {global.stateTracker[ut_states::UT_CHECK_POSITIONING] += 1;}
1021
      else if (newState == ut_states::UT_CHECK_VOLTAGE)
1022
        {global.stateTracker[ut_states::UT_CHECK_VOLTAGE] += 1;}
1023
      else if (newState == ut_states::UT_CHARGING)
1024
        {global.stateTracker[ut_states::UT_CHARGING] += 1;}
1025
      else if (newState == ut_states::UT_RELEASE)
1026
        {global.stateTracker[ut_states::UT_RELEASE] += 1;}
1027
      else if (newState == ut_states::UT_RELEASE_TO_CORRECT)
1028
        {global.stateTracker[ut_states::UT_RELEASE_TO_CORRECT] += 1;}
1029
      else if (newState == ut_states::UT_CORRECT_POSITIONING)
1030
        {global.stateTracker[ut_states::UT_CORRECT_POSITIONING] += 1;}
1031
      else if (newState == ut_states::UT_TURN)
1032
        {global.stateTracker[ut_states::UT_TURN] += 1;}
1033
      else if (newState == ut_states::UT_INACTIVE)
1034
        {global.stateTracker[ut_states::UT_INACTIVE] += 1;}
1035
      else if (newState == ut_states::UT_CALIBRATION)
1036
        {global.stateTracker[ut_states::UT_CALIBRATION] += 1;}
1037
      else if (newState == ut_states::UT_CALIBRATION_CHECK)
1038
        {global.stateTracker[ut_states::UT_CALIBRATION_CHECK] += 1;}
1039
      else if (newState == ut_states::UT_DEVIATION_CORRECTION)
1040
        {global.stateTracker[ut_states::UT_DEVIATION_CORRECTION] += 1;}
1041
      else if (newState == ut_states::UT_DOCKING_ERROR)
1042
        {global.stateTracker[16+(-ut_states::UT_DOCKING_ERROR)] += 1;}
1043
      else if (newState == ut_states::UT_REVERSE_TIMEOUT_ERROR)
1044
        {global.stateTracker[16+(-ut_states::UT_REVERSE_TIMEOUT_ERROR)] += 1;}
1045
      else if (newState == ut_states::UT_CALIBRATION_ERROR)
1046
        {global.stateTracker[16+(-ut_states::UT_CALIBRATION_ERROR)] += 1;}
1047
      else if (newState == ut_states::UT_WHITE_DETECTION_ERROR)
1048
        {global.stateTracker[16+(-ut_states::UT_WHITE_DETECTION_ERROR)] += 1;}
1049
      else if (newState == ut_states::UT_PROXY_DETECTION_ERROR)
1050
        {global.stateTracker[16+(-ut_states::UT_PROXY_DETECTION_ERROR)] += 1;}
1051
      else if (newState == ut_states::UT_NO_CHARGING_POWER_ERROR)
1052
        {global.stateTracker[16+(-ut_states::UT_NO_CHARGING_POWER_ERROR)] += 1;}
1053
      else if (newState == ut_states::UT_UNKNOWN_STATE_ERROR)
1054
        {global.stateTracker[16+(-ut_states::UT_UNKNOWN_STATE_ERROR)] += 1;}
1055
    }
1056

    
1057
    // Keep track of last state and set the new state for next iteration
1058
    prevState = currentState;
1059
    currentState = newState;
1060

    
1061
    this->sleep(CAN::UPDATE_PERIOD);
1062
  }
1063

    
1064
  return RDY_OK;
1065
}