DRC/NathansIdea.m at master · HalEdwa/DRC · 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
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
%% Initialization
delete(instrfindall);
clear all;
fclose('all');
%Serial On Off
serialf = 1;

if serialf ==1
    s = serial('/dev/cu.usbmodem1421','BaudRate',9600);
    fopen(s);
end
pause(1);
%for Cam 1 for video 0
CamVideo = 1;

if CamVideo ==1
    cam= webcam(1);
    %preview(cam)
    %cam.Brightness = 10;
    %cam.Saturation=100;
    %cam.Contrast= 95;
    %cam.BacklightCompensation=1;
else
    cam = VideoReader('QUT1.avi');
end

for po=1:1

    if CamVideo ==1
         rgbFrame = snapshot(cam);
         %rgbFrame = imresize(rgbFrame, 0.5);

       else
         rgbFrame = readFrame(cam);
         %rgbFrame = imresize(rgbFrame, 0.25);
    end

end

    figure(1);
while true
    tic
    if CamVideo ==1
         rgbFrame = snapshot(cam);
         rgbFrame = imresize(rgbFrame, 0.5);

       else
         rgbFrame = readFrame(cam);
         rgbFrame = imresize(rgbFrame, 0.25);
    end

    imHSV = rgb2hsv(rgbFrame);
    rgbFrame = hsv2rgb(imHSV);
    imBlue = (rgbFrame(:,:,3)-(rgbFrame(:,:,1)+rgbFrame(:,:,2))/2);
    sizeFrame = size(imBlue);
    Tb = mean(mean(imBlue(:,sizeFrame(2)/2:sizeFrame(2))))+0.06;% + std(std(imBlue));
    imBlue= imBlue- Tb;
    imBlue=imbinarize(imBlue);

    imHSV(:,:,1)= mod(imHSV(:,:,1)+(1/3),1);
    imYellow = hsv2rgb(imHSV);

    imYellow = imYellow(:,:,3)-(imYellow(:,:,1)+imYellow(:,:,2))/2;

    Ty = mean(mean(imYellow(:,sizeFrame(2)/2:sizeFrame(2))))+0.04; %+ std(std(imYellow));
    imYellow= (imYellow- Ty);
    imYellow=imbinarize(imYellow);


    %boxPosition =mean(mean(imMagenta))
    %imMagenta=colorSegment(imHSV, [0.89,0,0], [1,1,1]);


    %Steering Left Side
    count = 1;
    imSteerB =fliplr(imYellow(1:270,1:sizeFrame(2)/2));%(imBlue(1:170,1:sizeFrame(2)/2));
    [sel, c] = max( imSteerB ~=0, [], 2 );
    gradientb = 1.2; %c(end)/numel(c);
    interceptb = 100;
    for ir=1:size(c)
        if c(ir)>ir*gradientb+interceptb | c(ir)==1
            c(ir) = ir*gradientb+interceptb;
            count= count +1;
        end
    end
    count
    %STeering Right Side
    imSteerY =(imBlue(1:270,sizeFrame(2)/2:sizeFrame(2)));%(imYellow(1:170,sizeFrame(2)/2:sizeFrame(2)));
    [sel, f] = max( imSteerY ~=0, [], 2 );
    gradienty = 1.2;%c(end)/numel(c);
    intercepty = 100;
    count = 1;
    for ir=1:size(f)
        if f(ir)>ir*gradienty+intercepty | f(ir)==1
            f(ir) = ir*gradienty+intercepty;
            count= count +1;
        end
    end


    sum = (f-c);
    steerAngle=-(mean(sum(end-160:end)))/4+90%(mean(f))*-1.2+258
    if (steerAngle < 50)
          steerAngle = 50;
    end
    if (steerAngle > 120)
          steerAngle = 120;
    end

    %Visualisations
%     subplot(2, 2, 1);
%     imshow(fliplr(imYellow(1:270,1:sizeFrame(2)/2)));
%     subplot(2, 2, 2);
%     imshow((imBlue(1:270,sizeFrame(2)/2:sizeFrame(2))));
%     subplot(2, 2, 3);
%     imshow(rgbFrame);

%   steerAngle;
    if serialf ==1
        fwrite(s,steerAngle,'uint8');
    end
    pause(1);
    toc
end


function mask = colorSegment(I, minHSV, maxHSV)


    mask1 = I(:,:,1)>=minHSV(1) & I(:,:,1)<=maxHSV(1);


    mask2 = I(:,:,2)>=minHSV(2) & I(:,:,2)<=maxHSV(2);


    mask3 = I(:,:,3)>=minHSV(3) & I(:,:,3)<=maxHSV(3);


    mask = mask1.*mask2.*mask3;


    return
end