Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / userthread.cpp @ 25388c2f

History | View | Annotate | Download (17.1 KB)

1
#include "userthread.hpp"
2

    
3
#include "global.hpp"
4

    
5
#include "linefollow2.hpp" 
6

    
7
using namespace amiro;
8

    
9
extern Global global;
10

    
11
// State machine states
12
enum states : uint8_t {
13
        IDLE,
14
        GO_RIGHT,
15
        GO_STRAIGHT,
16
        PARKING,
17
        PARKING_RIGHT,
18
        PARKING_LEFT,
19
        GO_LEFT,
20
        SPINNING_PARKING,
21
        SPINNING
22
};
23

    
24
// Policy
25
states policy[] = {
26
  GO_STRAIGHT,
27
  GO_RIGHT,
28
  GO_RIGHT,
29
  GO_STRAIGHT,
30
  GO_RIGHT,
31
  GO_STRAIGHT,
32
  GO_RIGHT,
33
  GO_STRAIGHT,
34
  GO_STRAIGHT,
35
  GO_RIGHT,
36
  GO_STRAIGHT,
37
  GO_RIGHT,
38
  GO_STRAIGHT
39
};
40

    
41
// The different classes (or members) of color discrimination
42
// BLACK is the line itselfe
43
// GREY is the boarder between the line and the surface
44
// WHITE is the common surface
45
enum colorMember : uint8_t {
46
        BLACK=0,
47
        GREY=1,
48
        WHITE=2
49
};
50

    
51
// a buffer for the z-value of the accelerometer
52
int16_t accel_z;
53
bool running;
54

    
55
// Get some information about the policy
56
const int sizeOfPolicy = sizeof(policy) / sizeof(states);
57
int policyCounter = 0; // Do not change this, it points to the beginning of the policy
58

    
59
// Different speed settings (all values in "rounds per minute")
60
const int rpmTurnLeft[2] = {-10, 10};
61
const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]};
62
const int rpmHalt[2] = {0, 0};
63

    
64
// Definition of the fuzzyfication function
65
//  | Membership
66
// 1|_B__   G    __W__
67
//  |    \  /\  /
68
//  |     \/  \/
69
//  |_____/\__/\______ Sensor values
70
// SEE MATLAB SCRIPT "fuzzyRule.m" for adjusting the values
71
// All values are "raw sensor values"
72
/* Use these values for white ground surface (e.g. paper) */
73

    
74
const int blackStartFalling = 0x1000; // Where the black curve starts falling
75
const int blackOff = 0x1800; // Where no more black is detected
76
const int whiteStartRising = 0x2800; // Where the white curve starts rising
77
const int whiteOn = 0x6000; // Where the white curve has reached the maximum value
78
const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
79
const int greyStartRising = blackStartFalling; // Where grey starts rising
80
const int greyOff = whiteOn; // Where grey is completely off again
81

    
82
/* Use these values for gray ground surfaces */
83
/*
84
const int blackStartFalling = 0x1000; // Where the black curve starts falling
85
const int blackOff = 0x2800; // Where no more black is detected
86
const int whiteStartRising = 0x4000; // Where the white curve starts rising
87
const int whiteOn = 0x5000; // Where the white curve starts rising
88
const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
89
const int greyStartRising = blackStartFalling; // Where grey starts rising
90
const int greyOff = whiteOn; // Where grey is completely off again
91
*/
92

    
93
int vcnl4020AmbientLight[4] = {0};
94
int vcnl4020Proximity[4] = {0};
95

    
96
// Border for the discrimination between black and white
97
const int discrBlackWhite = 16000; // border in "raw sensor values"
98
// Discrimination between black and white (returns BLACK or WHITE)
99
// The border was calculated by a MAP-decider
100
colorMember discrimination(int value) {
101
        if (value < discrBlackWhite)
102
                return BLACK;
103
        else
104
                return WHITE;
105
}
106

    
107
// Copy the speed from the source to the target array
108
void copyRpmSpeed(const int (&source)[2], int (&target)[2]) {
109
        target[constants::DiWheelDrive::LEFT_WHEEL] = source[constants::DiWheelDrive::LEFT_WHEEL];
110
        target[constants::DiWheelDrive::RIGHT_WHEEL] = source[constants::DiWheelDrive::RIGHT_WHEEL];
111
        // chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]);
112
}
113

    
114
// Fuzzyfication of the sensor values
115
void fuzzyfication(int sensorValue, float (&fuzziedValue)[3]) {
116
        if (sensorValue < blackStartFalling ) {
117
                // Only black value
118
                fuzziedValue[BLACK] = 1.0f;
119
                fuzziedValue[GREY] = 0.0f;
120
                fuzziedValue[WHITE] = 0.0f;
121
        } else if (sensorValue > whiteOn ) {
122
                // Only white value
123
                fuzziedValue[BLACK] = 0.0f;
124
                fuzziedValue[GREY] = 0.0f;
125
                fuzziedValue[WHITE] = 1.0f;
126
        } else if ( sensorValue < greyMax) {
127
                // Some greyisch value between black and grey
128

    
129
                // Black is going down
130
                if ( sensorValue > blackOff) {
131
                        fuzziedValue[BLACK] = 0.0f;
132
                } else {
133
                        fuzziedValue[BLACK] = static_cast<float>(sensorValue-blackOff) / (blackStartFalling-blackOff);
134
                }
135

    
136
                // Grey is going up
137
                if ( sensorValue < greyStartRising) {
138
                        fuzziedValue[GREY] = 0.0f;
139
                } else {
140
                        fuzziedValue[GREY] = static_cast<float>(sensorValue-greyStartRising) / (greyMax-greyStartRising);
141
                }
142

    
143
                // White is absent
144
                fuzziedValue[WHITE] = 0.0f;
145

    
146
        } else if ( sensorValue >= greyMax) {
147
                // Some greyisch value between grey white
148

    
149
                // Black is absent
150
                fuzziedValue[BLACK] = 0.0f;
151

    
152
                // Grey is going down
153
                if ( sensorValue < greyOff) {
154
                        fuzziedValue[GREY] = static_cast<float>(sensorValue-greyOff) / (greyMax-greyOff);
155
                } else {
156
                        fuzziedValue[GREY] = 0.0f;
157
                }
158

    
159
                // White is going up
160
                if ( sensorValue < whiteStartRising) {
161
                        fuzziedValue[WHITE] = 0.0f;
162
                } else {
163
                        fuzziedValue[WHITE] = static_cast<float>(sensorValue-whiteStartRising) / (whiteOn-whiteStartRising);
164
                }
165
        }
166
}
167

    
168
// Return the color, which has the highest fuzzy value
169
colorMember getMember(float (&fuzzyValue)[3]) {
170
        colorMember member;
171

    
172
        if (fuzzyValue[BLACK] > fuzzyValue[GREY])
173
                if (fuzzyValue[BLACK] > fuzzyValue[WHITE])
174
                        member = BLACK;
175
                else
176
                        member = WHITE;
177
        else
178
                if (fuzzyValue[GREY] > fuzzyValue[WHITE])
179
                        member = GREY;
180
                else
181
                        member = WHITE;
182

    
183
        return member;
184
}
185

    
186
// Get a crisp output for the steering commands
187
void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]) {
188

    
189
        // all sensors are equal
190
        if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_LEFT] &&
191
            member[constants::DiWheelDrive::PROX_FRONT_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_RIGHT] &&
192
            member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]) {
193
                // something is wrong -> stop
194
                copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
195
        // both front sensor detect a line
196
        } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK &&
197
            member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
198
                // straight
199
                copyRpmSpeed(global.rpmForward, rpmFuzzyCtrl);
200
        // exact one front sensor detects a line
201
        } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK ||
202
                   member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
203
                // soft correction
204
                if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
205
                        // soft right
206
                        copyRpmSpeed(global.rpmSoftRight, rpmFuzzyCtrl);
207
                } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE) {
208
                        // hard right
209
                        copyRpmSpeed(global.rpmHardRight, rpmFuzzyCtrl);
210
                } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
211
                        // soft left
212
                        copyRpmSpeed(global.rpmSoftLeft, rpmFuzzyCtrl);
213
                } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) {
214
                        // hard left
215
                        copyRpmSpeed(global.rpmHardLeft, rpmFuzzyCtrl);
216
                }
217
        // both wheel sensors detect a line
218
        } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK &&
219
                   member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
220
                // something is wrong -> stop
221
                copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
222
        // exactly one wheel sensor detects a line
223
        } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK ||
224
                   member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
225
                if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK) {
226
                        // turn left
227
                        copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
228
                } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
229
                        // turn right
230
                        copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
231
                }
232
        // both front sensors may detect a line
233
        } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY &&
234
                   member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
235
                if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
236
                        // turn left
237
                        copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
238
                } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
239
                        // turn right
240
                        copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
241
                }
242
        // exactly one front sensor may detect a line
243
        } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY ||
244
                   member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
245
                if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
246
                        // turn left
247
                        copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
248
                } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
249
                        // turn right
250
                        copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
251
                }
252
        // both wheel sensors may detect a line
253
        } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY &&
254
                   member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
255
                // something is wrong -> stop
256
                copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
257
        // exactly one wheel sensor may detect a line
258
        } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY ||
259
                   member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
260
                if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
261
                        // turn left
262
                        copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
263
                } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
264
                        // turn right
265
                        copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
266
                }
267
        // no sensor detects anything 
268
        } else {
269
                // line is lost -> stop
270
                copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
271
        }
272

    
273
        return;
274
}
275

    
276
Color memberToLed(colorMember member) {
277
        switch (member) {
278
                case BLACK:
279
                        return Color(Color::GREEN);
280
                case GREY:
281
                        return Color(Color::YELLOW);
282
                case WHITE:
283
                        return Color(Color::RED);
284
                default:
285
                        return Color(Color::WHITE);
286
        }
287
}
288

    
289
//void lineFollowing_new(xyz) {}
290

    
291
void defuzz(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]){
292
        // all sensors are equal
293
        // if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_LEFT] &&
294
        //     member[constants::DiWheelDrive::PROX_FRONT_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_RIGHT] &&
295
        //     member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]) {
296
        //         // something is wrong -> stop
297
        //         copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
298
        // // both front sensor detect a line
299
        if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK &&
300
            (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY)) {
301
                // straight
302
                copyRpmSpeed(global.rpmForward, rpmFuzzyCtrl);
303
        // Deviation to right
304
        } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK
305
                && member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE){
306
                        copyRpmSpeed(global.rpmSoftLeft, rpmFuzzyCtrl);
307
        // Deviation to left
308
        }else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK
309
                && member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK){
310
                        copyRpmSpeed(global.rpmSoftRight, rpmFuzzyCtrl);
311
        // Hard deviatio to right
312
        }else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY
313
                && member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE){
314
                        copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
315
        // Hard deviation to left
316
        }else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY
317
                && member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK){
318
                        copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
319
        // stop if white
320
        }else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE
321
                && member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE ){
322
                        copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
323
        }
324
}
325

    
326
// Line following by a fuzzy controler
327
void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]) {
328
        // FUZZYFICATION
329
        // First we need to get the fuzzy value for our 3 values {BLACK, GREY, WHITE}
330
        float leftWheelFuzzyMemberValues[3], leftFrontFuzzyMemberValues[3], rightFrontFuzzyMemberValues[3], rightWheelFuzzyMemberValues[3];
331
        fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], leftWheelFuzzyMemberValues);
332
        fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], leftFrontFuzzyMemberValues);
333
        fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], rightFrontFuzzyMemberValues);
334
        fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT], rightWheelFuzzyMemberValues);
335

    
336
        // INFERENCE RULE DEFINITION
337
        // Get the member for each sensor
338
        colorMember member[4];
339
        member[constants::DiWheelDrive::PROX_WHEEL_LEFT] = getMember(leftWheelFuzzyMemberValues);
340
        member[constants::DiWheelDrive::PROX_FRONT_LEFT] = getMember(leftFrontFuzzyMemberValues);
341
        member[constants::DiWheelDrive::PROX_FRONT_RIGHT] = getMember(rightFrontFuzzyMemberValues);
342
        member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] = getMember(rightWheelFuzzyMemberValues);
343

    
344
        // visualize sensors via LEDs
345
        global.robot.setLightColor(constants::LightRing::LED_WNW, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_LEFT]));
346
        global.robot.setLightColor(constants::LightRing::LED_NNW, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_LEFT]));
347
        global.robot.setLightColor(constants::LightRing::LED_NNE, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_RIGHT]));
348
        global.robot.setLightColor(constants::LightRing::LED_ENE, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]));
349

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

    
353
        // DEFUZZYFICATION
354
        // defuzzyfication(member, rpmFuzzyCtrl);
355
        defuzz(member, rpmFuzzyCtrl);
356
}
357

    
358

    
359

    
360

    
361
// Set the speed by the array
362
void setRpmSpeed(const int (&rpmSpeed)[2]) {
363
        global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
364
}
365

    
366
// Get the next policy rule
367
states getNextPolicy() {
368
        // If the policy is over, start again
369
        if (policyCounter >= sizeOfPolicy)
370
                policyCounter = 3;
371

    
372
        return policy[policyCounter++];
373
}
374

    
375

    
376

    
377
UserThread::UserThread() :
378
  chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()
379
{
380
}
381

    
382
UserThread::~UserThread()
383
{
384
}
385

    
386
msg_t
387
UserThread::main()
388
{
389
        /*
390
         * SETUP
391
         */
392
        int rpmFuzzyCtrl[2] = {0};
393
    for (uint8_t led = 0; led < 8; ++led) {
394
                global.robot.setLightColor(led, Color(Color::BLACK));
395
    }
396
    running = false;
397
        LineFollow lf;
398
        /*
399
         * LOOP
400
         */
401
        while (!this->shouldTerminate())
402
        {
403
        /*
404
         * read accelerometer z-value
405
         */
406
        accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);
407

    
408
        /*
409
         * evaluate the accelerometer
410
         */
411

    
412
        //Features over can: (Line Following)
413
        //SetKP
414
        //SetKI
415
        //SetKd ?
416
        //SetSleepTime
417
        //SeForwardSpeed? or SetMaxSpeed
418
        //DriveOnLeftLine
419
        //DriveOnRightLine
420
        
421
        //if accel_z<-900
422
                //send can event (one time)
423
                // Line following is started if amiro roteted
424
        if (accel_z < -900 /*-0.9g*/) { //new: (can.allowLineFollowOnTurn && accel_z < 900) || can.startLineFollow optional feature
425
            if (running) {
426
                // stop the robot
427
                running = false;
428
                global.motorcontrol.setTargetRPM(0, 0);
429
            } else {
430
                // start the robot
431
                running = true;
432
            }
433
            // set the front LEDs to blue for one second
434
            global.robot.setLightColor(constants::LightRing::LED_SSW, Color(Color::BLACK));
435
            global.robot.setLightColor(constants::LightRing::LED_WSW, Color(Color::BLACK));
436
            global.robot.setLightColor(constants::LightRing::LED_WNW, Color(Color::WHITE));
437
            global.robot.setLightColor(constants::LightRing::LED_NNW, Color(Color::WHITE));
438
            global.robot.setLightColor(constants::LightRing::LED_NNE, Color(Color::WHITE));
439
            global.robot.setLightColor(constants::LightRing::LED_ENE, Color(Color::WHITE));
440
            global.robot.setLightColor(constants::LightRing::LED_ESE, Color(Color::BLACK));
441
            global.robot.setLightColor(constants::LightRing::LED_SSE, Color(Color::BLACK));
442
            this->sleep(MS2ST(1000));
443
            global.robot.setLightColor(constants::LightRing::LED_WNW, Color(Color::BLACK));
444
            global.robot.setLightColor(constants::LightRing::LED_NNW, Color(Color::BLACK));
445
            global.robot.setLightColor(constants::LightRing::LED_NNE, Color(Color::BLACK));
446
            global.robot.setLightColor(constants::LightRing::LED_ENE, Color(Color::BLACK));
447
        }
448

    
449
        if (running) {
450
            // Read the proximity values
451
            for (int i = 0; i < 4; i++) {
452
                vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
453
                vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
454
            }
455
                        lf.stableFollow(vcnl4020Proximity, rpmFuzzyCtrl, &global);
456
            // chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n",
457
            //         vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
458
            //         vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
459
            //         vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
460
            //         vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]);
461
        //if (configLineFollowing==2)
462
        //   lineFollownew
463
        //else
464
            // lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl, &global);
465
            setRpmSpeed(rpmFuzzyCtrl);
466
        }
467

    
468
                // this->sleep(US2ST(5));
469
                this->sleep(CAN::UPDATE_PERIOD);
470
        }
471

    
472
  return RDY_OK;
473
}