Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / linefollow.cpp @ e0c5d17a

History | View | Annotate | Download (16 KB)

1
// #include "global.hpp"
2
#include "linefollow.hpp" 
3
#include <cmath>
4
#include <limits>
5

    
6

    
7
LineFollow::LineFollow(Global *global){
8
    this->global = global;
9
}
10
LineFollow::LineFollow(Global *global, LineFollowStrategy strategy){
11
    this->global = global;
12
    this-> strategy = strategy;
13
}
14

    
15

    
16
int LineFollow::transitionError(int FL, int FR, int targetL, int targetR){
17
    // global->robot.setLightColor(0, Color::RED);
18
    // global->robot.setLightColor(7, Color::RED);
19
    int error = 0;
20

    
21
    switch (this->strategy)
22
    {
23
    case LineFollowStrategy::TRANSITION_R_L:
24
        error = -(FL -targetL + FR - targetR + this->trans);
25
        break;
26
    case LineFollowStrategy::TRANSITION_L_R:
27
        error = (FL -targetL + FR - targetR + this->trans);
28
        break;
29
    default:
30
        break;
31
    }
32
    this->trans += 400;
33
    if(FL+FR <= RAND_TRESH){
34
      // global->robot.setLightColor(0, Color::GREEN);
35
      // global->robot.setLightColor(7, Color::GREEN);
36
      switch (this->strategy)
37
      {
38
        case LineFollowStrategy::TRANSITION_R_L:
39
            this->strategy = LineFollowStrategy::EDGE_LEFT;
40
            break;
41
        case LineFollowStrategy::TRANSITION_L_R:
42
            this->strategy = LineFollowStrategy::EDGE_RIGHT;
43
            break;
44
        default:
45
            break;
46
      }
47
      this->trans = 0;
48
    }
49
  return error;
50
}
51

    
52
/**
53
 * Calculate the error from front proxi sensors and fixed threshold values for those sensors.
54
 */
55
int LineFollow::getError(){
56
  // global->robot.setLightColor(3, Color::YELLOW);
57
    // Get actual sensor data of both front sensors
58
    int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
59
    int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
60
    // int targetL = global->linePID.threshProxyL;
61
    // int targetR = global->linePID.threshProxyR;
62
    int targetL = global->linePID.BThresh;
63
    int targetR = global->linePID.WThresh;
64
    int error = 0;
65
    switch (this->strategy)
66
    {
67
    case LineFollowStrategy::EDGE_RIGHT:
68
        error = -(FL -targetL + FR - targetR);
69
        break;
70
    case LineFollowStrategy::EDGE_LEFT:
71
        error = (FL -targetL + FR - targetR);
72
        break;
73
    case LineFollowStrategy::MIDDLE:
74
        // Assume that the smallest value means driving in the middle
75
        // targetL = targetR = !(targetL<targetR)?targetR:targetL;
76
        error = (FL -targetL + FR - targetL);
77
        break;
78
    case LineFollowStrategy::TRANSITION_L_R: case LineFollowStrategy::TRANSITION_R_L:
79
      error = transitionError(FL, FR, targetL, targetR);
80
      break;
81
    default:
82
        break;
83
    }
84
    // Debugging stuff ------
85
    // if (global->enableRecord){
86
    //     global->senseRec[global->sensSamples].error = error;
87
    //     global->senseRec[global->sensSamples].FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
88
    //     global->senseRec[global->sensSamples].FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
89
    //     global->sensSamples++;
90
    //     }
91
    // ----------------------
92
    // Register white values
93
    if (FL+FR > global->threshWhite){
94
        whiteFlag = 1;
95
    }else{
96
        whiteFlag = 0;
97
    }
98
    return error;
99
}
100

    
101

    
102

    
103

    
104
/**
105
 * Depending on the strategy different behaviours will be triggered. 
106
 *  FUZZY - standard tracking of black area 
107
 *  REVERSE - drive back
108
 *  @param: rpmSpeed motor speed 
109
 */
110
int LineFollow::followLine(int (&rpmSpeed)[2]){
111
  
112
    int correctionSpeed = 0;
113
    switch (this->strategy)
114
    {
115
    case LineFollowStrategy::FUZZY:
116
      for (int i = 0; i < 4; i++) {
117
          vcnl4020AmbientLight[i] = global->vcnl4020[i].getAmbientLight();
118
          vcnl4020Proximity[i] = global->vcnl4020[i].getProximityScaledWoOffset();
119
      }
120
      lineFollowing(vcnl4020Proximity, rpmSpeed);
121
      break;
122

    
123
    case LineFollowStrategy::REVERSE:
124
      correctionSpeed = -getPidCorrectionSpeed();
125
      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -1000000*global->forwardSpeed;
126

    
127
      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -1000000*global->forwardSpeed;
128
      
129
      break;
130

    
131
    default:
132
      correctionSpeed = getPidCorrectionSpeed();
133
      // chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed,  global->threshWhite);
134

    
135
      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   1000000*global->forwardSpeed + correctionSpeed;
136

    
137
      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = 1000000*global->forwardSpeed - correctionSpeed;
138
      break;
139
    }
140

    
141
    // Limit Speed
142
    if ((rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] > MAX_CORRECTED_SPEED) || (rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] > MAX_CORRECTED_SPEED)){
143
      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] /= 2;
144
      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] /= 2;
145
    }
146

    
147

    
148
    return whiteFlag;
149
}
150

    
151

    
152
/**
153
 * Pid controller which returns a corrections speed.
154
 */
155
int LineFollow::getPidCorrectionSpeed(){
156
    int32_t error = getError();
157
    int32_t sloap = oldError - error ;
158
    // int correctionSpeed = (int) (global->K_p*error + Ki*accumHist - global->K_d*sloap);
159
    int32_t correctionSpeed = (int32_t) (K_p*error + K_i*accumHist + K_d*sloap);
160
    oldError = error;
161
    
162
    // Overflow Protection
163
    if ((error > 0) && (accumHist > std::numeric_limits<int32_t>::max() - error)){
164
      // Overflow detected
165
      accumHist = std::numeric_limits<int32_t>::max();
166
    } else if ((error < 0) && (accumHist < std::numeric_limits<int32_t>::min() - error)) {
167
      // Underflow detected
168
      accumHist = std::numeric_limits<int32_t>::min();
169

    
170
    }else {
171
      accumHist += error;
172
    }
173
    if (abs(error) > global->maxDist.error){
174
        global->maxDist.error = error;
175
    }
176
    return correctionSpeed;
177
}
178

    
179

    
180
void LineFollow::setStrategy(LineFollowStrategy strategy){
181

    
182
  if(this->strategy == LineFollowStrategy::TRANSITION_R_L || this->strategy == LineFollowStrategy::TRANSITION_L_R){
183
    return;
184
  }
185

    
186
  switch(strategy){
187
    case LineFollowStrategy::EDGE_LEFT:
188
      if((this->strategy == LineFollowStrategy::EDGE_RIGHT) || (this->strategy == LineFollowStrategy::TRANSITION_R_L)){
189
        this->strategy = LineFollowStrategy::TRANSITION_R_L;
190
      }else{
191
        // In case of fuzzy or reverse
192
        this->strategy = strategy;
193
      }
194
      break;
195
    case LineFollowStrategy::EDGE_RIGHT:
196
    if((this->strategy == LineFollowStrategy::EDGE_LEFT) || (this->strategy == LineFollowStrategy::TRANSITION_L_R)){
197
        this->strategy = LineFollowStrategy::TRANSITION_L_R;
198
      }else{
199
        // In case of fuzzy or reverse
200
        this->strategy = strategy;
201
      }
202
      break;
203
    default:
204
    // From Fuzzy or Reverse state should work to transition automatically
205
      this->strategy = strategy;
206
      break;
207
  }
208
  // this->strategy = strategy;
209
}
210

    
211
void LineFollow::promptStrategyChange(LineFollowStrategy strategy){
212
  this->strategy = strategy;
213
}
214

    
215
LineFollowStrategy LineFollow::getStrategy(){
216
      return this->strategy;
217
}
218
void LineFollow::setGains(float Kp, float Ki, float Kd){
219
    this->K_p = Kp;
220
    this->K_i = Ki;
221
    this->K_d = Kd;
222
}
223

    
224

    
225

    
226

    
227

    
228
// Legacy code, fuzzy following-----------------------------------------
229
// Line following by a fuzzy controler
230
void LineFollow::lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]) {
231
  // FUZZYFICATION
232
  // First we need to get the fuzzy value for our 3 values {BLACK, GREY, WHITE}
233
  float leftWheelFuzzyMemberValues[3], leftFrontFuzzyMemberValues[3], rightFrontFuzzyMemberValues[3], rightWheelFuzzyMemberValues[3];
234
  fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], leftWheelFuzzyMemberValues);
235
  fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], leftFrontFuzzyMemberValues);
236
  fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], rightFrontFuzzyMemberValues);
237
  fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT], rightWheelFuzzyMemberValues);
238

    
239
  // INFERENCE RULE DEFINITION
240
  // Get the member for each sensor
241
  colorMember member[4];
242
  member[constants::DiWheelDrive::PROX_WHEEL_LEFT] = getMember(leftWheelFuzzyMemberValues);
243
  member[constants::DiWheelDrive::PROX_FRONT_LEFT] = getMember(leftFrontFuzzyMemberValues);
244
  member[constants::DiWheelDrive::PROX_FRONT_RIGHT] = getMember(rightFrontFuzzyMemberValues);
245
  member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] = getMember(rightWheelFuzzyMemberValues);
246

    
247
  // visualize sensors via LEDs
248
  global->robot.setLightColor(constants::LightRing::LED_WNW, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_LEFT]));
249
  global->robot.setLightColor(constants::LightRing::LED_NNW, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_LEFT]));
250
  global->robot.setLightColor(constants::LightRing::LED_NNE, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_RIGHT]));
251
  global->robot.setLightColor(constants::LightRing::LED_ENE, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]));
252

    
253
  // chprintf((BaseSequentialStream*) &SD1, "Left: BLACK: %f, GREY: %f, WHITE: %f\r\n", leftWheelFuzzyMemberValues[BLACK], leftWheelFuzzyMemberValues[GREY], leftWheelFuzzyMemberValues[WHITE]);
254
  // chprintf((BaseSequentialStream*) &SD1, "Right: BLACK: %f, GREY: %f, WHITE: %f\r\n", rightFuzzyMemberValues[BLACK], rightFuzzyMemberValues[GREY], rightFuzzyMemberValues[WHITE]);
255

    
256
  // DEFUZZYFICATION
257
  defuzzyfication(member, rpmFuzzyCtrl);
258
  // defuzz(member, rpmFuzzyCtrl);
259
}
260

    
261

    
262
Color LineFollow::memberToLed(colorMember member) {
263
  switch (member) {
264
    case BLACK:
265
      return Color(Color::GREEN);
266
    case GREY:
267
      return Color(Color::YELLOW);
268
    case WHITE:
269
      return Color(Color::RED);
270
    default:
271
      return Color(Color::WHITE);
272
  }
273
}
274

    
275
void LineFollow::defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]) {
276
    whiteFlag = 0;
277
  // all sensors are equal
278
  if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_LEFT] &&
279
      member[constants::DiWheelDrive::PROX_FRONT_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_RIGHT] &&
280
      member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]) {
281
    // something is wrong -> stop
282
    copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
283
  // both front sensor detect a line
284
  } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK &&
285
      member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
286
    // straight
287
    copyRpmSpeed(global->rpmForward, rpmFuzzyCtrl);
288
  // exact one front sensor detects a line
289
  } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK ||
290
             member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
291
    // soft correction
292
    if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
293
      // soft right
294
      copyRpmSpeed(global->rpmSoftRight, rpmFuzzyCtrl);
295
    } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE) {
296
      // hard right
297
      copyRpmSpeed(global->rpmHardRight, rpmFuzzyCtrl);
298
    } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
299
      // soft left
300
      copyRpmSpeed(global->rpmSoftLeft, rpmFuzzyCtrl);
301
    } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) {
302
      // hard left
303
      copyRpmSpeed(global->rpmHardLeft, rpmFuzzyCtrl);
304
    }
305
  // both wheel sensors detect a line
306
  } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK &&
307
             member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
308
    // something is wrong -> stop
309
    copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
310
  // exactly one wheel sensor detects a line
311
  } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK ||
312
             member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
313
    if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK) {
314
      // turn left
315
      copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
316
    } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
317
      // turn right
318
      copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
319
    }
320
  // both front sensors may detect a line
321
  } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY &&
322
             member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
323
    if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
324
      // turn left
325
      copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
326
    } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
327
      // turn right
328
      copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
329
    }
330
  // exactly one front sensor may detect a line
331
  } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY ||
332
             member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
333
    if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
334
      // turn left
335
      copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
336
    } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
337
      // turn right
338
      copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
339
    }
340
  // both wheel sensors may detect a line
341
  } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY &&
342
             member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
343
    // something is wrong -> stop
344
    copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
345
  // exactly one wheel sensor may detect a line
346
  } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY ||
347
             member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
348
    if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
349
      // turn left
350
      copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
351
    } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
352
      // turn right
353
      copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
354
    }
355
  // no sensor detects anything 
356
  } else {
357
    // line is lost -> stop
358
    whiteFlag = 1;
359
    copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
360
  }
361
    chprintf((BaseSequentialStream*) &SD1, "Fuzzy Speed: Left: %d, Right: %d\n", rpmFuzzyCtrl[0], rpmFuzzyCtrl[1]);
362
  return;
363
}
364

    
365
colorMember LineFollow::getMember(float (&fuzzyValue)[3]) {
366
  colorMember member;
367

    
368
  if (fuzzyValue[BLACK] > fuzzyValue[GREY])
369
    if (fuzzyValue[BLACK] > fuzzyValue[WHITE])
370
      member = BLACK;
371
    else
372
      member = WHITE;
373
  else
374
    if (fuzzyValue[GREY] > fuzzyValue[WHITE])
375
      member = GREY;
376
    else
377
      member = WHITE;
378

    
379
  return member;
380
}
381

    
382
// Fuzzyfication of the sensor values
383
void LineFollow::fuzzyfication(int sensorValue, float (&fuzziedValue)[3]) {
384
  if (sensorValue < blackStartFalling ) {
385
    // Only black value
386
    fuzziedValue[BLACK] = 1.0f;
387
    fuzziedValue[GREY] = 0.0f;
388
    fuzziedValue[WHITE] = 0.0f;
389
  } else if (sensorValue > whiteOn ) {
390
    // Only white value
391
    fuzziedValue[BLACK] = 0.0f;
392
    fuzziedValue[GREY] = 0.0f;
393
    fuzziedValue[WHITE] = 1.0f;
394
  } else if ( sensorValue < greyMax) {
395
    // Some greyisch value between black and grey
396

    
397
    // Black is going down
398
    if ( sensorValue > blackOff) {
399
      fuzziedValue[BLACK] = 0.0f;
400
    } else {
401
      fuzziedValue[BLACK] = static_cast<float>(sensorValue-blackOff) / (blackStartFalling-blackOff);
402
    }
403

    
404
    // Grey is going up
405
    if ( sensorValue < greyStartRising) {
406
      fuzziedValue[GREY] = 0.0f;
407
    } else {
408
      fuzziedValue[GREY] = static_cast<float>(sensorValue-greyStartRising) / (greyMax-greyStartRising);
409
    }
410

    
411
    // White is absent
412
    fuzziedValue[WHITE] = 0.0f;
413

    
414
  } else if ( sensorValue >= greyMax) {
415
    // Some greyisch value between grey white
416

    
417
    // Black is absent
418
    fuzziedValue[BLACK] = 0.0f;
419

    
420
    // Grey is going down
421
    if ( sensorValue < greyOff) {
422
      fuzziedValue[GREY] = static_cast<float>(sensorValue-greyOff) / (greyMax-greyOff);
423
    } else {
424
      fuzziedValue[GREY] = 0.0f;
425
    }
426

    
427
    // White is going up
428
    if ( sensorValue < whiteStartRising) {
429
      fuzziedValue[WHITE] = 0.0f;
430
    } else {
431
      fuzziedValue[WHITE] = static_cast<float>(sensorValue-whiteStartRising) / (whiteOn-whiteStartRising);
432
    }
433
  }
434
}
435

    
436
void LineFollow::copyRpmSpeed(const int (&source)[2], int (&target)[2]) {
437
  target[constants::DiWheelDrive::LEFT_WHEEL] = source[constants::DiWheelDrive::LEFT_WHEEL];
438
  target[constants::DiWheelDrive::RIGHT_WHEEL] = source[constants::DiWheelDrive::RIGHT_WHEEL];
439
  // chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]);
440
}