Revision 1564 trunk/code/behaviors/formation_control/circle_spacing/circle_spacing.c

View differences:

circle_spacing.c
2 2
#include <wl_basic.h>
3 3
#include<encoders.h>
4 4

  
5

  
6
int master = 0;
7
int sending = 0;
8
int stop = 0;
9

  
10

  
11
void switch_sending ()
12
{
13
	if(sending)
14
	{
15
		sending = 0;
16
		bom_off();
17
	}
18
	else
19
	{
20
		sending = 1;
21
		bom_on();
22
	}
23
}
24

  
25

  
26

  
5 27
int main(void)
6 28
{
7 29

  
8 30
	
31
	
32
	int sending_counter = 0;
9 33

  
10 34
    /* Initialize dragonfly board */
11 35
	
......
14 38
	wl_basic_init_default();
15 39
	wl_set_channel(12);
16 40
    int data_length;
17
	unsigned char *packet_data=wl_basic_do_default(&data_length);
18
	int distance=24;//packet_data[0];
19
	encoder_rst_dx(RIGHT);
20
	while(encoder_get_x(RIGHT)<=distance){
41
	unsigned char *packet_data;
42
	char send_buffer;
43
	
44
	
45
	if(master) orb1_set_color(RED);
46
	else orb1_set_color(BLUE);
47
	
48
	while(1)
49
	{
50
	
51
	
52
		
53
	
21 54
		bom_refresh(BOM_ALL);
22
	  
23
		orb1_set_color(BLUE);
24 55
		
25
	   if(bom_get_max()+1==8){	
26
		orb1_set_color(RED);
27
		motor_r_set(FORWARD,180);
28
		motor_l_set(FORWARD,180);
29 56
		
30
	   }
31
	   else {
32
		motor_l_set(FORWARD,0);
33
		motor_r_set(FORWARD,180);
34
	   }
57
		
58
		if(master)
59
		{
60
		
61
			if(sending_counter++>100)
62
			{
63
				switch_sending();
64
				sending_counter = 0;
65
				send_buffer = 'a';
66
				wl_basic_send_global_packet(42, send_buffer, 1);
67
			}
68
		
69
			
70
			if(sending)
71
			{
72
		
73
				
74
			
75
			
76

  
77
			}
78
			
79
			else // recieving
80
			{
81
				if(bom_get_max()+1==5)
82
				{
83
				send_buffer = 's';
84
				wl_basic_send_global_packet(42, send_buffer, 1);
85
				}
86
			}
87
		}
88
		
89
		
90
		
91
		else // slave
92
		{
93
			if(packet_data[0]=='s') stop=1;
94
			if(packet_data[0]=='a') switch_sending();
95
		
96
			if(sending)
97
			{
98
				motor_l_set(FORWARD,0);
99
				motor_r_set(FORWARD,0);
100
			}
101
			
102
			else // recieving
103
			{
104
			
105
				if(stop)
106
				{
107
				}
108
				
109
				else
110
				{
111

  
112
					
113
				   if(bom_get_max()+1==8){	
114
					orb1_set_color(RED);
115
					motor_r_set(FORWARD,180);
116
					motor_l_set(FORWARD,180);
117
					
118
				   }
119
				   else {
120
					motor_l_set(FORWARD,0);
121
					motor_r_set(FORWARD,180);
122
					}
123
				}
124
			
125
			
126
			}
127
		
128
		
129
		}
130
		
131
		packet_data = wl_basic_do_default(&data_length);
132
		delay_ms(10);
133
	}
35 134
	
36
    } 
37
	motor_r_set(FORWARD,0);
38
	motor_l_set(FORWARD,0);
39
	while(1);
40 135
  
41 136
  return 0;
42 137
  

Also available in: Unified diff