Project

General

Profile

Statistics
| Revision:

root / trunk / code / behaviors / formation_control / circle / circle.c @ 1509

History | View | Annotate | Download (3.3 KB)

1
#include <dragonfly_lib.h>
2
#include <encoders.h>
3

    
4
#define RADIUS 300
5

    
6
extern void blink(int);
7
extern void search(void);
8
extern void approach(void);
9
extern void lastDrive(void);
10

    
11
int main(void)
12
{
13

    
14
        /* Initialize dragonfly board */
15
        dragonfly_init(ALL_ON);
16
        encoders_init();
17

    
18
        orb_set_color(GREEN);
19
        delay_ms(300);
20
                
21
        search();
22
        
23
        approach();
24
        
25
        while(1); /* END HERE */
26

    
27
        return 0;
28
}
29

    
30

    
31

    
32
void blink(int num)
33
{
34
        for(int i = 0; i<num; i++)
35
        {
36
                orb_set_color(ORB_OFF);
37
                delay_ms(200);
38
                orb_set_color(RED);
39
                delay_ms(200);
40
        }
41
        orb_set_color(ORB_OFF);
42
}
43
void search()        //makes the robot face the beacon
44
{
45
        orb_set_color(YELLOW);        //show that the robot is searching
46
        bom_refresh(BOM_ALL);
47
        int top=bom_get_max();
48
        
49
        if ((top<4) || (top>11))        //determine which way to turn
50
        {
51
                motor_r_set(BACKWARD,200);
52
                motor_l_set(FORWARD,200);
53
        }
54
        else if((top>4) && (top<12))
55
        {
56
                motor_r_set(FORWARD,200);
57
                motor_l_set(BACKWARD,200);
58
        }
59
        while (top != 4)
60
        {
61
                bom_refresh(BOM_ALL);
62
                top=bom_get_max();
63
        }
64
        motors_off();
65
}
66
void approach()
67
{
68
        int dist = 0;
69
        int temp = 0;
70
        int counter = 0;
71
        int correct = 0;
72
        
73
        while(dist != RADIUS)        //puts the robots in the given distance RADIUS
74
        {
75
                temp=0;
76
                counter = 0;
77
                
78
                while(counter<8)
79
                {
80
                        if(range_read_distance(IR2) == -1){}
81
                        
82
                        else
83
                        {
84
                                temp += range_read_distance(IR2);
85
                                counter++;
86
                        }
87
                        delay_ms(20);
88
                }
89
                dist = temp/8;
90
                
91
                /*if (correct >2){
92
                        orb_set_color(RED);
93
                        break;
94
                        
95
                        }        // stop within the dead zone: it knows it is in the dead zone if it hits +- 15 from RADIUS 3 times*/
96
                
97
                if ( dist<(RADIUS+30) && dist>(RADIUS-30)) {        // the dead zone
98
                        orb_set_color(RED);
99
                        break;
100
                }
101
                
102
                else if ((dist>RADIUS) || (dist==-1))
103
                {                        
104
                        ///delay_ms(200);
105
                        search();
106
                        
107
                        motor_r_set(FORWARD,200);
108
                        motor_l_set(FORWARD,200);
109
                }
110
                else if ((dist<RADIUS))
111
                {
112
                        //delay_ms(200);
113
                        search();
114
                        
115
                        motor_r_set(BACKWARD,200);
116
                        motor_l_set(BACKWARD,200);
117
                }
118
                
119
        }
120
        
121
        orb_set_color(BLUE);
122
        
123
        motors_off();
124

    
125
        /*usb_puti(range_read_distance(IR2));
126
        usb_puts("\n\r");
127
        motor_r_set(FORWARD,200);
128
        motor_l_set(FORWARD,200);
129
        delay_ms(300);
130
        usb_puti(range_read_distance(IR2));
131
        usb_puts("\n\r");
132
        delay_ms(300);
133
        usb_puti(range_read_distance(IR2));
134
        usb_puts("\n\r");
135
        delay_ms(300);
136
        usb_puti(range_read_distance(IR2));
137
        usb_puts("\n\r");*/
138
}
139
void lastDrive()
140
{
141
        int lspeed = 190;
142
        int rspeed = 190;
143
        int firstldist = encoder_get_x(LEFT);
144
        int firstrdist = encoder_get_x(RIGHT);
145
        int ldist = firstldist;
146
        int rdist = firstrdist;
147
        while ( (ldist-firstldist<800) && (rdist-firstrdist<800) )
148
        {
149
                motor_l_set(FORWARD,lspeed);
150
                motor_r_set(FORWARD,rspeed);
151
                
152
                int lvel = encoder_get_v(LEFT);
153
                int rvel = encoder_get_v(RIGHT);
154
                
155
                usb_puts("\n\rLeft distance: ");
156
                usb_puti(ldist-firstldist);
157
                usb_puts("\n\rRight distance: ");
158
                usb_puti(rdist-firstrdist);
159
                usb_puts("\n\rLeft velocity: ");
160
                usb_puti(lvel);
161
                usb_puts("\n\rRight velocity: ");
162
                usb_puti(rvel);
163
                usb_puts("\n\r\n\r");
164
                
165
                if (rvel<lvel)
166
                {
167
                        rspeed += 1;
168
                        lspeed -= 1;
169
                }
170
                else if (rvel>lvel)
171
                {
172
                        rspeed -= 1;
173
                        lspeed += 1;
174
                }
175
                ldist = encoder_get_x(LEFT);
176
                rdist = encoder_get_x(RIGHT);
177
                
178
                delay_ms(25);
179
        }
180
        motors_off();
181
        orb_set_color(GREEN);
182
}