balanceBot/Helper.cpp at master · DozerEng/balanceBot · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
/*!
 * \file I2CHelper.cpp
 *
 * \author Michael Pillon
 *
 */

#include "Helper.hpp"


namespace Helper {

/*!
    \fn void knightRider(void)

    Knight Rider style LED routine.
    LEDs illiminate back and forth from left to right

 */
void knightRider(DigitalOut* led1, DigitalOut* led2, DigitalOut* led3, DigitalOut* led4)
{
    static int position = 0; //! position + 1 = next active LED
    static bool direction = 0; //!left to right (0) or right to left (1)

    //LED Position logic
    if (direction == 0) { //! Forward Direction
        switch (position) {
            case 0:
                *led1 = 1;
                *led2 = 0;
                *led3 = 0;
                *led4 = 0;
                position++;
                break;
            case 1:
                *led1 = 0;
                *led2 = 1;
                *led3 = 0;
                *led4 = 0;
                position++;
                break;
            case 2:
                *led1 = 0;
                *led2 = 0;
                *led3 = 1;
                *led4 = 0;
                position++;
                break;
            case 3:
                *led1 = 0;
                *led2 = 0;
                *led3 = 0;
                *led4 = 1;
                direction = 1; //reverse direction
                position--;
                break;
            default:
                position = 0; //Should NEVER get here
        }
    } else { //!Reverse direction == 1
        switch (position) {
            case 0:
                *led1 = 1;
                *led2 = 0;
                *led3 = 0;
                *led4 = 0;
                direction = 0;
                position++;
                break;
            case 1:
                *led1 = 0;
                *led2 = 1;
                *led3 = 0;
                *led4 = 0;
                position--;
                break;
            case 2:
                *led1 = 0;
                *led2 = 0;
                *led3 = 1;
                *led4 = 0;
                position--;
                break;
            case 3:
                *led1 = 0;
                *led2 = 0;
                *led3 = 0;
                *led4 = 1;
                position--; //reverse direction
                break;
            default:
                position = 1; //Should NEVER get here
        }
    }
}

}