Project-Spyn---Handy-Chaps/MainCode.m at main · Ariya3/Project-Spyn---Handy-Chaps · 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
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
% NaC = 0
% black = 1
% blue = 2
% green = 3
% yellow = 4
% red = 5
% white = 6
% brown = 7
global key
InitKeyboard();

while 1

    color = brick.ColorCode(1);
    distance = brick.UltrasonicDist(2);
    touch = brick.TouchPressed(3);
    disp(color);

    %% starts keyboard control
    if(color == 2 || color == 3) %% change based on pickup and dropoff
        color = brick.ColorCode(1);
        while 1
            pause(0.1);
            switch key
                % Wheel Direction
                case 'uparrow'
                    brick.MoveMotor('A', -30);
                    brick.MoveMotor('C', -30);
                    disp ('Up Arrow Pressed!');

                case 'downarrow'
                    brick.MoveMotor('A', 30);
                    brick.MoveMotor('C', 30);
                    disp ('Down Arrow Pressed!');

                case 'leftarrow'
                    brick.MoveMotor('A', 30);
                    brick.MoveMotor('C', -30)
                    disp ('Left Arrow Pressed!');

                case 'rightarrow'
                    brick.MoveMotor('A', -30);
                    brick.MoveMotor('C', 30)
                    disp ('Right Arrow Pressed!');

                    % Lift Controls
                case 'w'
                    brick.MoveMotor('B', 15);
                    disp ('W Pressed!')

                case 's'
                    brick.MoveMotor('B', -15);
                    disp ('S Pressed!')

                    % Pause
                case 0
                    brick.MoveMotor('A', 0);
                    brick.MoveMotor('B', 0);
                    brick.MoveMotor('C', 0);
                    brick.MoveMotor('D', 0);
                    disp ('No Key Pressed!');

                    %switches back to auton
                case'p'
                    brick.MoveMotor('A', -70);
                    brick.MoveMotor('C', -70);
                    pause(1);
                    brick.StopAllMotors();
                    break;

            end
        end

    end

    %% autonomous code
    %stop sign
    if (color == 5)
        brick.StopAllMotors();
        pause(2);
        brick.MoveMotor('A', -50);
        brick.MoveMotor('C', -50);
        disp(color);

    %break condition
    elseif (color == 6) %exit
        brick.MoveMotor('A', 0);
        brick.MoveMotor('C', 0);
        break;
    end

    %moves forward
    if (distance <= 45)
        brick.MoveMotor('B', 0);
        brick.MoveMotor('A', -70);
        brick.MoveMotor('C', -70);
        disp(distance);

    %turn right
    elseif (distance > 45)
        brick.MoveMotor('A', -60);
        brick.MoveMotor('C', -60);
        pause(1.4);
        brick.MoveMotor('A', -25);
        brick.MoveMotor('C', 25);
        pause(1.67);
        brick.MoveMotor('A', -60);
        brick.MoveMotor('C', -60);
        pause(2.5);
        brick.StopMotor('A');
        brick.StopMotor('C');
        disp(distance);
    end

    %turns left
    if (touch == 1)
        brick.MoveMotor('A', 60);
        brick.MoveMotor('C', 60);
        pause(1.25);
        brick.MoveMotor('A', 25);
        brick.MoveMotor('C', -25);
        pause(1.35);
    end


end

CloseKeyboard();