RobotRace/camera_test at master · pbking1/RobotRace · 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
void test(int Hmax, int Hmin, int Smax, int Smin) {      //find the center of the object
    MFCapSetH(Hmax,Hmin);
    MFCapSetS(Smax,Smin);
    while(1){
        if(X() >= 100 && X() <= 300 && color_block() > 100)
			LED('b');
    }
}

void camera_back() {             //find the yellow
    int Hmax = 100;
    int Hmin = 45;
    int Smax = 75;
    int Smin = 23;
    MFCapSetH(Hmax,Hmin);
    MFCapSetS(Smax,Smin);
    move_back(1023, 100);

    stop();
    turn_head('r');
    set_posture(-40, 50, 45, 50, 500);

    while (1) {
		if (X() < 160) LED('l');
		if (color_block() > 500) LED('r');
		if (X() < 160 && color_block() > 500) {
            LED('b');
            stop();
            break;
		} else {
            move_back(400, 50);
		}
    }

    while (1) {
		if (X() < 240)
			LED('l');
		else if (color_block() > 200)
			LED('r');
		if (X() < 240 && color_block() > 200) {
            LED('b');
            stop();
            break;
		} else {
            turn_right_slowly_2(100);
		}
    }

    stop();


}

void camera(int Hmax, int Hmin, int Smax, int Smin) {   //find the green
    MFCapSetH(Hmax,Hmin);
    MFCapSetS(Smax,Smin);
    move_forward(1023, 4000);

    stop();
    turn_head('r');
    set_posture(-40, 50, 45, 50, 500);

    while (1) {
		if (X() > 160) LED('l');
		if (color_block() > 500) LED('r');
		if (X() > 160 && color_block() > 500) {
            LED('b');
            stop();
            break;
		} else {
            move_forward(400, 50);
		}
    }

    stop();
    turn_head('m');

    while (1) {
		if (X() < 320)
			LED('l');
		else if (color_block() > 200)
			LED('r');
		if (X() < 320 && color_block() > 200) {
            LED('b');
            stop();
            break;
		} else {
            turn_right_slowly_2(100);
		}
    }

    stop();


}