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

View differences:

circle_spacing.c
7 7
int sending = 0;
8 8
int stop = 0;
9 9
struct vector slave_position;
10
int desired_max_bom;
11
int bom_max_counter;
10 12

  
13

  
14
void set_desired_max_bom(int desired_angle)
15
{
16
	if (desired_angle >= 348 || desired_angle < 11) desired_max_bom = 0;
17
	if (desired_angle >= 11 && desired_angle < 33) desired_max_bom = 1;
18
	if (desired_angle >= 33 && desired_angle < 56) desired_max_bom = 2;
19
	if (desired_angle >= 56 && desired_angle < 78) desired_max_bom = 3;
20
	if (desired_angle >= 78 && desired_angle < 101) desired_max_bom = 4;
21
	if (desired_angle >= 101 && desired_angle < 123) desired_max_bom = 5;
22
	if (desired_angle >= 123 && desired_angle < 145) desired_max_bom = 6;
23
	if (desired_angle >= 145 && desired_angle < 167) desired_max_bom = 7;
24
	if (desired_angle >= 167 && desired_angle < 190) desired_max_bom = 8;
25
	if (desired_angle >= 190 && desired_angle < 212) desired_max_bom = 9;
26
	if (desired_angle >= 212 && desired_angle < 235) desired_max_bom = 10;
27
	if (desired_angle >= 235 && desired_angle < 257) desired_max_bom = 11;
28
	if (desired_angle >= 257 && desired_angle < 280) desired_max_bom = 12;
29
	if (desired_angle >= 280 && desired_angle < 302) desired_max_bom = 13;
30
	if (desired_angle >= 302 && desired_angle < 325) desired_max_bom = 14;
31
	if (desired_angle >= 325 && desired_angle < 348) desired_max_bom = 15;
32
}
33

  
34

  
35

  
36

  
37

  
38

  
11 39
void switch_sending ()
12 40
{
13 41
	if(sending)
......
30 58
	
31 59
	
32 60
	int sending_counter = 0;
33

  
61
	
34 62
    /* Initialize dragonfly board */
35 63
	
36 64
    dragonfly_init(ALL_ON);
......
42 70
	char send_buffer[2];
43 71
	
44 72
	
45
	if(master) orb1_set_color(RED);
73
	if(master) {orb1_set_color(RED); set_desired_max_bom(30);}
46 74
	else orb1_set_color(BLUE);
47 75
	
76
	
48 77
	while(1)
49 78
	{
50 79
	
......
58 87
		if(master)
59 88
		{
60 89
		
61
			if(sending_counter++>2)
90
			if(sending_counter++>4)
62 91
			{
63 92
				switch_sending();
64 93
				sending_counter = 0;
......
78 107
			
79 108
			else // recieving
80 109
			{
110
				int max_bom = bom_get_max();
111
					usb_puts("bom_get_max : ");
112
					usb_puti(max_bom);
113
					usb_puts("\n\r");
81 114
			
82
				bom_get_vector(&slave_position, NULL);
83
				if(slave_position.y/slave_position.x > 10){
84
			
85
				//if(bom_get_max()+1==5){
115
				if(max_bom == desired_max_bom)
116
				{
117
					if(bom_max_counter)                 // only stops the slave if two consecutive boms reading give the desired bom as the max one. Filters the noise.
118
					{
119
					send_buffer[0] = 's';
120
					wl_basic_send_global_packet(42, send_buffer, 2);
121
					}
122
				bom_max_counter =1;
123
				}
124
				else bom_max_counter = 0;
86 125
				
87
				send_buffer[0] = 's';
88
				wl_basic_send_global_packet(42, send_buffer, 2);
89
				}
90 126
			}
91 127
		}
92 128
		
......
99 135
		
100 136
			if(sending)
101 137
			{
102
			/*
103
				motor_l_set(FORWARD,0);
104
				motor_r_set(FORWARD,0);
105
			*/
138
			
106 139
			}
107 140
			
108 141
			else // recieving
......
117 150
				
118 151
				else
119 152
				{
153
					int max_bom = bom_get_max();
154
					usb_puts("bom_get_max : ");
155
					usb_puti(max_bom);
156
					usb_puts("/n/r");
120 157
				
121
					
122
					
123
					
124
					//bom_get_vector(&slave_position, NULL);
125
					//if(abs(slave_position.x)>2 && (double)slave_position.y/(double)slave_position.x < 0.1){
126
					
127
				   if(bom_get_max()+1==8){	
158
				
159
				   if(max_bom == 8)
160
				   {	
128 161
					orb2_set_color(BLUE);
129 162
					motor_r_set(FORWARD,180);
130 163
					motor_l_set(FORWARD,180);
131 164
					
132 165
				   }
133
				   else {
134
				   orb2_set_color(GREEN);
166
				   else if(max_bom == 7 || max_bom == 6 || max_bom == 5)
167
				   {
168
					motor_l_set(FORWARD,180);
169
					motor_r_set(FORWARD,0);
170
					}
171
					else if(max_bom == -1);
172
				   else 
173
				   {
174
					orb2_set_color(GREEN);
135 175
					motor_l_set(FORWARD,0);
136 176
					motor_r_set(FORWARD,180);
137 177
					}
......
156 196
}
157 197

  
158 198

  
199

  
200

  
201

  
202

  

Also available in: Unified diff