Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / linefollow.cpp @ 726fdc72

History | View | Annotate | Download (15.1 KB)

1
#include "global.hpp"
2
#include "linefollow2.hpp" 
3
#include <cmath>
4

    
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->threshProxyL;
61
    int targetR = global->threshProxyR;
62
    int error = 0;
63
    switch (this->strategy)
64
    {
65
    case LineFollowStrategy::EDGE_RIGHT:
66
        error = -(FL -targetL + FR - targetR);
67
        break;
68
    case LineFollowStrategy::EDGE_LEFT:
69
        error = (FL -targetL + FR - targetR);
70
        break;
71
    case LineFollowStrategy::MIDDLE:
72
        // Assume that the smallest value means driving in the middle
73
        targetL = targetR = !(targetL<targetR)?targetR:targetL;
74
        error = (FL -targetL + FR - targetR);
75
        break;
76
    case LineFollowStrategy::TRANSITION_L_R: case LineFollowStrategy::TRANSITION_R_L:
77
      error = transitionError(FL, FR, targetL, targetR);
78
      break;
79
    default:
80
        break;
81
    }
82
    // Debugging stuff ------
83
    // if (global->enableRecord){
84
    //     global->senseRec[global->sensSamples].error = error;
85
    //     global->senseRec[global->sensSamples].FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
86
    //     global->senseRec[global->sensSamples].FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
87
    //     global->sensSamples++;
88
    //     }
89
    // ----------------------
90
    // Register white values
91
    if (FL+FR > global->threshWhite){
92
        whiteFlag = 1;
93
    }else{
94
        whiteFlag = 0;
95
    }
96
    return error;
97
}
98

    
99

    
100

    
101

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

    
121
    case LineFollowStrategy::REVERSE:
122
      correctionSpeed = -getPidCorrectionSpeed();
123
      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -global->forwardSpeed;
124

    
125
      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -global->forwardSpeed;
126
      
127
      break;
128

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

    
133
      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   global->forwardSpeed + correctionSpeed;
134

    
135
      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed - correctionSpeed;
136
      break;
137
    }
138
    return whiteFlag;
139
}
140

    
141

    
142
/**
143
 * Pid controller which returns a corrections speed.
144
 */
145
int LineFollow::getPidCorrectionSpeed(){
146
    int error = getError();
147
    int sloap = oldError - error ;
148
    // int correctionSpeed = (int) (global->K_p*error + Ki*accumHist - global->K_d*sloap);
149
    int correctionSpeed = (int) (K_p*error + K_i*accumHist - K_d*sloap);
150
    oldError = error;
151
    // accumHist += (int) (0.01 * error);
152
    if (abs(error) > global->maxDist.error){
153
        global->maxDist.error = error;
154
    }
155
    return correctionSpeed;
156
}
157

    
158

    
159
void LineFollow::setStrategy(LineFollowStrategy strategy){
160

    
161
  if(this->strategy == LineFollowStrategy::TRANSITION_R_L || this->strategy == LineFollowStrategy::TRANSITION_L_R){
162
    return;
163
  }
164

    
165
  switch(strategy){
166
    case LineFollowStrategy::EDGE_LEFT:
167
      if((this->strategy == LineFollowStrategy::EDGE_RIGHT) || (this->strategy == LineFollowStrategy::TRANSITION_R_L)){
168
        this->strategy = LineFollowStrategy::TRANSITION_R_L;
169
      }else{
170
        // In case of fuzzy or reverse
171
        this->strategy = strategy;
172
      }
173
      break;
174
    case LineFollowStrategy::EDGE_RIGHT:
175
    if((this->strategy == LineFollowStrategy::EDGE_LEFT) || (this->strategy == LineFollowStrategy::TRANSITION_L_R)){
176
        this->strategy = LineFollowStrategy::TRANSITION_L_R;
177
      }else{
178
        // In case of fuzzy or reverse
179
        this->strategy = strategy;
180
      }
181
      break;
182
    default:
183
    // From Fuzzy or Reverse state should work to transition automatically
184
      this->strategy = strategy;
185
      break;
186
  }
187
  // this->strategy = strategy;
188
}
189

    
190
void LineFollow::promptStrategyChange(LineFollowStrategy strategy){
191
  this->strategy = strategy;
192
}
193

    
194
LineFollowStrategy LineFollow::getStrategy(){
195
      return this->strategy;
196
}
197
void LineFollow::setGains(float Kp, float Ki, float Kd){
198
    this->K_p = Kp;
199
    this->K_i = Ki;
200
    this->K_d = Kd;
201
}
202

    
203

    
204

    
205

    
206

    
207
// Legacy code, fuzzy following-----------------------------------------
208
// Line following by a fuzzy controler
209
void LineFollow::lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]) {
210
  // FUZZYFICATION
211
  // First we need to get the fuzzy value for our 3 values {BLACK, GREY, WHITE}
212
  float leftWheelFuzzyMemberValues[3], leftFrontFuzzyMemberValues[3], rightFrontFuzzyMemberValues[3], rightWheelFuzzyMemberValues[3];
213
  fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], leftWheelFuzzyMemberValues);
214
  fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], leftFrontFuzzyMemberValues);
215
  fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], rightFrontFuzzyMemberValues);
216
  fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT], rightWheelFuzzyMemberValues);
217

    
218
  // INFERENCE RULE DEFINITION
219
  // Get the member for each sensor
220
  colorMember member[4];
221
  member[constants::DiWheelDrive::PROX_WHEEL_LEFT] = getMember(leftWheelFuzzyMemberValues);
222
  member[constants::DiWheelDrive::PROX_FRONT_LEFT] = getMember(leftFrontFuzzyMemberValues);
223
  member[constants::DiWheelDrive::PROX_FRONT_RIGHT] = getMember(rightFrontFuzzyMemberValues);
224
  member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] = getMember(rightWheelFuzzyMemberValues);
225

    
226
  // visualize sensors via LEDs
227
  global->robot.setLightColor(constants::LightRing::LED_WNW, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_LEFT]));
228
  global->robot.setLightColor(constants::LightRing::LED_NNW, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_LEFT]));
229
  global->robot.setLightColor(constants::LightRing::LED_NNE, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_RIGHT]));
230
  global->robot.setLightColor(constants::LightRing::LED_ENE, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]));
231

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

    
235
  // DEFUZZYFICATION
236
  defuzzyfication(member, rpmFuzzyCtrl);
237
  // defuzz(member, rpmFuzzyCtrl);
238
}
239

    
240

    
241
Color LineFollow::memberToLed(colorMember member) {
242
  switch (member) {
243
    case BLACK:
244
      return Color(Color::GREEN);
245
    case GREY:
246
      return Color(Color::YELLOW);
247
    case WHITE:
248
      return Color(Color::RED);
249
    default:
250
      return Color(Color::WHITE);
251
  }
252
}
253

    
254
void LineFollow::defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]) {
255
    whiteFlag = 0;
256
  // all sensors are equal
257
  if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_LEFT] &&
258
      member[constants::DiWheelDrive::PROX_FRONT_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_RIGHT] &&
259
      member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]) {
260
    // something is wrong -> stop
261
    copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
262
  // both front sensor detect a line
263
  } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK &&
264
      member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
265
    // straight
266
    copyRpmSpeed(global->rpmForward, rpmFuzzyCtrl);
267
  // exact one front sensor detects a line
268
  } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK ||
269
             member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
270
    // soft correction
271
    if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
272
      // soft right
273
      copyRpmSpeed(global->rpmSoftRight, rpmFuzzyCtrl);
274
    } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE) {
275
      // hard right
276
      copyRpmSpeed(global->rpmHardRight, rpmFuzzyCtrl);
277
    } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
278
      // soft left
279
      copyRpmSpeed(global->rpmSoftLeft, rpmFuzzyCtrl);
280
    } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) {
281
      // hard left
282
      copyRpmSpeed(global->rpmHardLeft, rpmFuzzyCtrl);
283
    }
284
  // both wheel sensors detect a line
285
  } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK &&
286
             member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
287
    // something is wrong -> stop
288
    copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
289
  // exactly one wheel sensor detects a line
290
  } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK ||
291
             member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
292
    if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK) {
293
      // turn left
294
      copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
295
    } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
296
      // turn right
297
      copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
298
    }
299
  // both front sensors may detect a line
300
  } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY &&
301
             member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
302
    if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
303
      // turn left
304
      copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
305
    } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
306
      // turn right
307
      copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
308
    }
309
  // exactly one front sensor may detect a line
310
  } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY ||
311
             member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
312
    if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
313
      // turn left
314
      copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
315
    } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
316
      // turn right
317
      copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
318
    }
319
  // both wheel sensors may detect a line
320
  } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY &&
321
             member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
322
    // something is wrong -> stop
323
    copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
324
  // exactly one wheel sensor may detect a line
325
  } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY ||
326
             member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
327
    if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
328
      // turn left
329
      copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
330
    } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
331
      // turn right
332
      copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
333
    }
334
  // no sensor detects anything 
335
  } else {
336
    // line is lost -> stop
337
    whiteFlag = 1;
338
    copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
339
  }
340
    chprintf((BaseSequentialStream*) &SD1, "Fuzzy Speed: Left: %d, Right: %d\n", rpmFuzzyCtrl[0], rpmFuzzyCtrl[1]);
341
  return;
342
}
343

    
344
colorMember LineFollow::getMember(float (&fuzzyValue)[3]) {
345
  colorMember member;
346

    
347
  if (fuzzyValue[BLACK] > fuzzyValue[GREY])
348
    if (fuzzyValue[BLACK] > fuzzyValue[WHITE])
349
      member = BLACK;
350
    else
351
      member = WHITE;
352
  else
353
    if (fuzzyValue[GREY] > fuzzyValue[WHITE])
354
      member = GREY;
355
    else
356
      member = WHITE;
357

    
358
  return member;
359
}
360

    
361
// Fuzzyfication of the sensor values
362
void LineFollow::fuzzyfication(int sensorValue, float (&fuzziedValue)[3]) {
363
  if (sensorValue < blackStartFalling ) {
364
    // Only black value
365
    fuzziedValue[BLACK] = 1.0f;
366
    fuzziedValue[GREY] = 0.0f;
367
    fuzziedValue[WHITE] = 0.0f;
368
  } else if (sensorValue > whiteOn ) {
369
    // Only white value
370
    fuzziedValue[BLACK] = 0.0f;
371
    fuzziedValue[GREY] = 0.0f;
372
    fuzziedValue[WHITE] = 1.0f;
373
  } else if ( sensorValue < greyMax) {
374
    // Some greyisch value between black and grey
375

    
376
    // Black is going down
377
    if ( sensorValue > blackOff) {
378
      fuzziedValue[BLACK] = 0.0f;
379
    } else {
380
      fuzziedValue[BLACK] = static_cast<float>(sensorValue-blackOff) / (blackStartFalling-blackOff);
381
    }
382

    
383
    // Grey is going up
384
    if ( sensorValue < greyStartRising) {
385
      fuzziedValue[GREY] = 0.0f;
386
    } else {
387
      fuzziedValue[GREY] = static_cast<float>(sensorValue-greyStartRising) / (greyMax-greyStartRising);
388
    }
389

    
390
    // White is absent
391
    fuzziedValue[WHITE] = 0.0f;
392

    
393
  } else if ( sensorValue >= greyMax) {
394
    // Some greyisch value between grey white
395

    
396
    // Black is absent
397
    fuzziedValue[BLACK] = 0.0f;
398

    
399
    // Grey is going down
400
    if ( sensorValue < greyOff) {
401
      fuzziedValue[GREY] = static_cast<float>(sensorValue-greyOff) / (greyMax-greyOff);
402
    } else {
403
      fuzziedValue[GREY] = 0.0f;
404
    }
405

    
406
    // White is going up
407
    if ( sensorValue < whiteStartRising) {
408
      fuzziedValue[WHITE] = 0.0f;
409
    } else {
410
      fuzziedValue[WHITE] = static_cast<float>(sensorValue-whiteStartRising) / (whiteOn-whiteStartRising);
411
    }
412
  }
413
}
414

    
415
void LineFollow::copyRpmSpeed(const int (&source)[2], int (&target)[2]) {
416
  target[constants::DiWheelDrive::LEFT_WHEEL] = source[constants::DiWheelDrive::LEFT_WHEEL];
417
  target[constants::DiWheelDrive::RIGHT_WHEEL] = source[constants::DiWheelDrive::RIGHT_WHEEL];
418
  // chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]);
419
}