Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / userthread.cpp @ f336542d

History | View | Annotate | Download (14.8 KB)

1
#include "userthread.hpp"
2

    
3
#include "global.hpp"
4

    
5
using namespace amiro;
6

    
7
extern Global global;
8

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

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

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

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

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

    
57
// Different speed settings (all values in "rounds per minute")
58
const int rpmForward[2] = {25,25};
59
const int rpmSoftLeft[2] = {15,25};
60
const int rpmHardLeft[2] = {10,25};
61
const int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]};
62
const int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};
63
const int rpmTurnLeft[2] = {-10, 10};
64
const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]};
65
const int rpmHalt[2] = {0, 0};
66

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

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

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

    
96
int vcnl4020AmbientLight[4] = {0};
97
int vcnl4020Proximity[4] = {0};
98

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

    
110
// Copy the speed from the source to the target array
111
void copyRpmSpeed(const int (&source)[2], int (&target)[2]) {
112
        target[constants::DiWheelDrive::LEFT_WHEEL] = source[constants::DiWheelDrive::LEFT_WHEEL];
113
        target[constants::DiWheelDrive::RIGHT_WHEEL] = source[constants::DiWheelDrive::RIGHT_WHEEL];
114
}
115

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

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

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

    
145
                // White is absent
146
                fuzziedValue[WHITE] = 0.0f;
147

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

    
151
                // Black is absent
152
                fuzziedValue[BLACK] = 0.0f;
153

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

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

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

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

    
185
        return member;
186
}
187

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

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

    
275
        return;
276
}
277

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

    
291
// Line following by a fuzzy controler
292
void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]) {
293
        // FUZZYFICATION
294
        // First we need to get the fuzzy value for our 3 values {BLACK, GREY, WHITE}
295
        float leftWheelFuzzyMemberValues[3], leftFrontFuzzyMemberValues[3], rightFrontFuzzyMemberValues[3], rightWheelFuzzyMemberValues[3];
296
        fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], leftWheelFuzzyMemberValues);
297
        fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], leftFrontFuzzyMemberValues);
298
        fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], rightFrontFuzzyMemberValues);
299
        fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT], rightWheelFuzzyMemberValues);
300

    
301
        // INFERENCE RULE DEFINITION
302
        // Get the member for each sensor
303
        colorMember member[4];
304
        member[constants::DiWheelDrive::PROX_WHEEL_LEFT] = getMember(leftWheelFuzzyMemberValues);
305
        member[constants::DiWheelDrive::PROX_FRONT_LEFT] = getMember(leftFrontFuzzyMemberValues);
306
        member[constants::DiWheelDrive::PROX_FRONT_RIGHT] = getMember(rightFrontFuzzyMemberValues);
307
        member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] = getMember(rightWheelFuzzyMemberValues);
308

    
309
        // visualize sensors via LEDs
310
        global.robot.setLightColor(constants::LightRing::LED_WNW, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_LEFT]));
311
        global.robot.setLightColor(constants::LightRing::LED_NNW, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_LEFT]));
312
        global.robot.setLightColor(constants::LightRing::LED_NNE, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_RIGHT]));
313
        global.robot.setLightColor(constants::LightRing::LED_ENE, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]));
314

    
315
//        chprintf((BaseSequentialStream*) &SD1, "Left: BLACK: %f, GREY: %f, WHITE: %f\r\n", leftFuzzyMemberValues[BLACK], leftFuzzyMemberValues[GREY], leftFuzzyMemberValues[WHITE]);
316
//        chprintf((BaseSequentialStream*) &SD1, "Right: BLACK: %f, GREY: %f, WHITE: %f\r\n", rightFuzzyMemberValues[BLACK], rightFuzzyMemberValues[GREY], rightFuzzyMemberValues[WHITE]);
317

    
318
        // DEFUZZYFICATION
319
        defuzzyfication(member, rpmFuzzyCtrl);
320
}
321

    
322
// Set the speed by the array
323
void setRpmSpeed(const int (&rpmSpeed)[2]) {
324
        global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
325
}
326

    
327
// Get the next policy rule
328
states getNextPolicy() {
329
        // If the policy is over, start again
330
        if (policyCounter >= sizeOfPolicy)
331
                policyCounter = 3;
332

    
333
        return policy[policyCounter++];
334
}
335

    
336

    
337

    
338
UserThread::UserThread() :
339
  chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()
340
{
341
}
342

    
343
UserThread::~UserThread()
344
{
345
}
346

    
347
msg_t
348
UserThread::main()
349
{
350
        /*
351
         * SETUP
352
         */
353
        int rpmFuzzyCtrl[2] = {0};
354
    for (uint8_t led = 0; led < 8; ++led) {
355
                global.robot.setLightColor(led, Color(Color::BLACK));
356
    }
357
    running = false;
358

    
359
        /*
360
         * LOOP
361
         */
362
        while (!this->shouldTerminate())
363
        {
364
        /*
365
         * read accelerometer z-value
366
         */
367
        accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);
368

    
369
        /*
370
         * evaluate the accelerometer
371
         */
372
        if (accel_z < -900 /*-0.9g*/) {
373
            if (running) {
374
                // stop the robot
375
                running = false;
376
                global.motorcontrol.setTargetRPM(0, 0);
377
            } else {
378
                // start the robot
379
                running = true;
380
            }
381
            // set the front LEDs to blue for one second
382
            global.robot.setLightColor(constants::LightRing::LED_SSW, Color(Color::BLACK));
383
            global.robot.setLightColor(constants::LightRing::LED_WSW, Color(Color::BLACK));
384
            global.robot.setLightColor(constants::LightRing::LED_WNW, Color(Color::WHITE));
385
            global.robot.setLightColor(constants::LightRing::LED_NNW, Color(Color::WHITE));
386
            global.robot.setLightColor(constants::LightRing::LED_NNE, Color(Color::WHITE));
387
            global.robot.setLightColor(constants::LightRing::LED_ENE, Color(Color::WHITE));
388
            global.robot.setLightColor(constants::LightRing::LED_ESE, Color(Color::BLACK));
389
            global.robot.setLightColor(constants::LightRing::LED_SSE, Color(Color::BLACK));
390
            this->sleep(MS2ST(1000));
391
            global.robot.setLightColor(constants::LightRing::LED_WNW, Color(Color::BLACK));
392
            global.robot.setLightColor(constants::LightRing::LED_NNW, Color(Color::BLACK));
393
            global.robot.setLightColor(constants::LightRing::LED_NNE, Color(Color::BLACK));
394
            global.robot.setLightColor(constants::LightRing::LED_ENE, Color(Color::BLACK));
395
        }
396

    
397
        if (running) {
398
            // Read the proximity values
399
            for (int i = 0; i < 4; i++) {
400
                vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
401
                vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
402
            }
403

    
404
//            chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n",
405
//                     vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
406
//                     vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
407
//                     vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
408
//                     vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]);
409

    
410
            lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl);
411
            setRpmSpeed(rpmFuzzyCtrl);
412
        }
413

    
414
                this->sleep(CAN::UPDATE_PERIOD);
415
        }
416

    
417
  return RDY_OK;
418
}
419