Project

General

Profile

Revision 1870

Added by Brad Neuman about 6 years ago

removed 16299_s10 branch

View differences:

branches/16299_s10/code/tools/eeprom/Makefile
1
# this is a local makefile
2

  
3
# Relative path to the root directory (containing lib directory)
4
#ifndef COLONYROOT
5
COLONYROOT = ..
6

  
7
# Target file name (without extension).
8
TARGET = main
9

  
10
# Uncomment this to use the wireless library
11
#USE_WIRELESS = 1
12

  
13
# com1 = serial port. Use lpt1 to connect to parallel port.
14
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
15

  
16
#else
17
COLONYROOT := ../$(COLONYROOT)
18
#endif
19

  
20
include $(COLONYROOT)/Makefile
branches/16299_s10/code/tools/eeprom/program_eeprom.c
1
#include <dragonfly_lib.h>
2
#include <eeprom.h>
3

  
4
int main()
5
{
6
  char str[5];
7
  int i=0,id;
8
  char c;
9

  
10
  dragonfly_init(ALL_ON);
11

  
12
  usb_puts("Here is the current setup:\r\n  ID: ");
13
  itoa(get_robotid(), str, 10);
14
  usb_puts(str);
15
  usb_puts("\r\n  BOM type: ");
16
  itoa(get_bom_type(), str, 10);
17
  usb_puts(str);
18
  usb_puts("\r\n\r\nEnter new ID:");
19

  
20
  while((c = usb_getc()) != '\n' && c != '\r')
21
    {
22
      usb_putc(c);
23
      if(i>=4)
24
	{
25
	  usb_puts("ERROR: filled buffer\n");
26
	  while(1);
27
	}
28
      str[i++] = c;
29
    }
30
  while(i<5)
31
    str[i++] = 0;
32

  
33
  id = atoi(str);
34

  
35
  usb_puts("\r\nsetting new id to: ");
36
  usb_puti(id);
37

  
38

  
39
  eeprom_put_byte(EEPROM_ROBOT_ID_ADDR, 'I');
40
  eeprom_put_byte(EEPROM_ROBOT_ID_ADDR+1, 'D');
41
  eeprom_put_byte(EEPROM_ROBOT_ID_ADDR+2, id);
42

  
43

  
44
  usb_puts("\r\nEnter BOM type:\r\b"
45
	   "1 for BOM 1.0\r\n"
46
	   "5 for BOM 1.5\r\n"
47
	   "r for RBOM\r\n>");
48

  
49
  c = usb_getc();
50
  usb_putc(c);
51

  
52
  switch(c)
53
    {
54
    case '1': id = BOM10; break;
55
    case '5': id = BOM15; break;
56
    case 'r': id = RBOM; break;
57
    default:
58
      usb_puts("ERROR: invalid input");
59
      while(1);
60
    }
61

  
62
  eeprom_put_byte(EEPROM_BOM_TYPE_ADDR, 'B');
63
  eeprom_put_byte(EEPROM_BOM_TYPE_ADDR+1, 'O');
64
  eeprom_put_byte(EEPROM_BOM_TYPE_ADDR+2, 'M');
65
  eeprom_put_byte(EEPROM_BOM_TYPE_ADDR+3, id);
66

  
67

  
68

  
69
  usb_puts("\r\ndone! have a nice day\r\n");
70

  
71

  
72
  while(1);
73

  
74
  return 0;
75
}
76
  
branches/16299_s10/code/tools/rangefinders/Makefile
1
# this is a local makefile
2

  
3
# Relative path to the root directory (containing lib directory)
4
ifndef COLONYROOT
5
COLONYROOT := ..
6

  
7
# Target file name (without extension).
8
TARGET = 
9

  
10
# Uncomment this to use the wireless library
11
USE_WIRELESS = 1
12

  
13
# com1 = serial port. Use lpt1 to connect to parallel port.
14
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
15

  
16
else
17
COLONYROOT := ../$(COLONYROOT)
18
endif
19

  
20
include $(COLONYROOT)/Makefile
branches/16299_s10/code/tools/rangefinders/robot/testIRcycle.c
1
#include <dragonfly_lib.h>
2

  
3
int main(void)
4
{
5
	/* initialize components, set wireless channel */
6
	dragonfly_init(ALL_ON);
7
	
8
  //Replace the next two lines with your own code
9
  //You can add as many lines as you want
10
  
11
  uint8_t rangeNumber[] = {IR1,IR2,IR3,IR4,IR5}; //0 indexed
12
  uint8_t cIndex;
13
  
14
  //initial setup (IR1)
15
  cIndex=0;
16
  orb1_set_color(BLUE);
17
  orb2_set_color(GREEN);
18
  
19
	while(1) {
20
		
21
		usb_puti(range_read_distance(rangeNumber[cIndex]));
22
		usb_putc('\r');
23
		
24
		if (button1_click()) {
25
			switch (cIndex){ //button1 chooses a color channel
26
				case 0: //IR1
27
					orb1_set_color(RED);
28
					orb2_set_color(RED);
29
					cIndex=1;
30
					break;
31
				case 1: //IR2
32
					
33
					orb1_set_color(GREEN);
34
					orb2_set_color(GREEN);
35
					cIndex=2;
36
					break;
37
				case 2: //IR3
38
					
39
					orb1_set_color(BLUE);
40
					orb2_set_color(BLUE);
41
					cIndex=3;
42
					break;
43
				case 3: //IR4
44
					orb1_set_color(YELLOW);
45
					orb2_set_color(YELLOW);
46
					cIndex=4;
47
					break;	
48
				case 4: //IR5
49
					orb1_set_color(BLUE);
50
					orb2_set_color(GREEN);
51
					cIndex=0;
52
					break;
53
				default: //undefined
54
					return 0;
55
					break;
56
			}
57
			delay_ms(50); //allows button press to release
58
		}
59
		delay_ms(50); //refresh frequency: 1000/(50ms) refresh rate
60

  
61
	}
62
		
63
		
64
	
65
  //this tell the robot to just chill out forever. don't put anything after this
66
  while(1);
67
	return 0;
68
}
branches/16299_s10/code/tools/rangefinders/robot/Makefile
1
# this is a local makefile
2

  
3
# Relative path to the root directory (containing lib directory)
4
ifndef COLONYROOT
5
COLONYROOT := ..
6

  
7
# Target file name (without extension).
8
TARGET = testIRcycle
9

  
10
# Uncomment this to use the wireless library
11
USE_WIRELESS = 1
12

  
13
# com1 = serial port. Use lpt1 to connect to parallel port.
14
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
15

  
16
else
17
COLONYROOT := ../$(COLONYROOT)
18
endif
19

  
20
include $(COLONYROOT)/Makefile
branches/16299_s10/code/tools/Makefile
1
# this is a local makefile
2

  
3
# Relative path to the root directory (containing lib directory)
4
ifndef COLONYROOT
5
COLONYROOT := ..
6

  
7
# Target file name (without extension).
8
TARGET = main
9

  
10
# Uncomment this to use the wireless library
11
USE_WIRELESS = 1
12

  
13
# com1 = serial port. Use lpt1 to connect to parallel port.
14
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
15

  
16
else
17
COLONYROOT := ../$(COLONYROOT)
18
endif
19

  
20
include $(COLONYROOT)/Makefile
branches/16299_s10/code/behaviors/formation_control/hive/hive.c
1
#include <dragonfly_lib.h>
2
#include <wl_basic.h>
3
#include <encoders.h>
4
#include "hive.h"
5

  
6
int timeout = 0;
7
int sending = 0;
8
int desired_max_bom;
9
int bom_max_counter;
10

  
11

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

  
33
void forward(int speed){			// set the motors to this forward speed.
34
	motor_l_set(FORWARD,speed);
35
	motor_r_set(FORWARD,speed);
36
}
37
void left(int speed){				// turn left at this speed.
38
	motor_l_set(FORWARD,speed);
39
	motor_r_set(BACKWARD,speed);
40
}
41
void right(int speed){
42
	motor_l_set(BACKWARD,speed);
43
	motor_r_set(FORWARD,speed);
44
}
45
void stop(void){					// could be set to motors_off(), or just use this as an alternative.
46
	motor_l_set(BACKWARD,0);			// stop() is better - motors_off() creates a slight delay to turn them back on.
47
	motor_r_set(FORWARD,0);
48
}
49
void setforward(int spd1, int spd2){
50
	motor_l_set(FORWARD,spd1);
51
	motor_r_set(FORWARD,spd2);
52
}
53
void backward(int speed){
54
	motor_l_set(BACKWARD, speed);
55
	motor_r_set(BACKWARD, speed);
56
}
57

  
58

  
59
void switch_sending(void)
60
{
61
    if(sending)
62
    {
63
	sending = 0;
64
	bom_off();
65
    }
66
    else
67
    {
68
	sending = 1;
69
	bom_on();
70
    }
71
}
72

  
73

  
74

  
75
int get_distance(void){				// takes an averaged reading of the front rangefinder
76
    int temp,distance,kk=5;			// kk sets this to 5 readings.
77
    distance =0;
78
    for (int i=0; i<kk; i++){
79
	temp = range_read_distance(IR2);
80
	if (temp == -1)
81
	{
82
	    //temp=0;
83
	    i--;
84
	}
85
	else
86
	    distance+= temp;
87
	delay_ms(3);
88
    }
89
    if (kk>0)
90
	return (int)(distance/kk);
91
    else 
92
	return 0;
93
}
94

  
95

  
96

  
97
/*
98
  Orients the robot so that it is facing the beacon (or the broadcasting BOM).
99
	
100
*/
101
void correctTurn(void)
102
{
103
    orb1_set_color(BLUE);			// BLUE and PURPLE
104
    left(220);
105
    while(1)
106
    {
107
	int bomNum = 0;				// bomNum is the current maximum reading
108
	bom_refresh(BOM_ALL);
109
	bomNum = bom_get_max();
110
	usb_puti(bomNum);
111
	if(bomNum == 4)				// when it's turned the right way, stop
112
	{
113
	    timeout = 0;
114
	    //motor_l_set(1, 200);
115
	    //motor_r_set(1, 200);
116
	    break;				// exits the while() loop to stop the method
117
	}
118
	else					// facing the wrong way
119
	{
120
	    if(bomNum == -1)
121
	    {
122
		timeout++;
123
				
124
		if(timeout > 5000)	// if it's been looking too long, move a little bit as it turns
125
		{
126
		    motor_r_set(FORWARD, 210);
127
		    motor_l_set(BACKWARD, 190);
128
		}
129
	    }
130
	    else if((bomNum >= 12) || (bomNum < 4))
131
	    {
132
		motor_l_set(FORWARD, 200);
133
		motor_r_set(BACKWARD, 200);
134
		timeout = 0;
135
	    }
136
	    else
137
	    {
138
		motor_l_set(BACKWARD, 200);
139
		motor_r_set(FORWARD, 200);
140
		timeout = 0;
141
	    }
142
	}
143
    }
144
    return;
145
}
146

  
147

  
148

  
149
/* 
150
   drives forward a hardcoded distance. May not be useful.
151
*/
152
void go_straight(void){
153
    forward(200);
154
    encoder_rst_dx(LEFT);
155
    encoder_rst_dx(RIGHT);
156
    delay_ms(100); 
157
    int x_left = encoder_get_x(LEFT), x_right = encoder_get_x(RIGHT);
158
    int count = 0;
159
    int d;
160
    while (count<25){						//count = 25 when bot6; count <12
161
	x_left = encoder_get_x(LEFT);
162
	x_right = encoder_get_x(RIGHT);
163
	d = x_right-x_left;
164
	if (d>13 || d<-13){
165
	    if(d<50 && d>-50){
166
		d = round(1.0*d/4);
167
		setforward(200+d, 200-d);
168
	    }
169
	}
170
	delay_ms(32);
171
	count++;
172
    }
173
}
174

  
175

  
176
/* 
177
   BLINK the given number times
178
*/
179
void blink(int num) {
180
    for(int i = 0; i<num; i++)
181
    {
182
	orb_set_color(ORB_OFF);
183
	delay_ms(350);
184
	orb_set_color(RED);
185
	delay_ms(200);
186
    }
187
    orb_set_color(ORB_OFF);
188
}
189

  
190

  
191
int main(void)
192
{
193
    /* Initialize dragonfly board */
194
    dragonfly_init(ALL_ON);
195
    /* Initialize the basic wireless library */
196
    wl_basic_init_default();
197
    /* Set the XBee channel to your assigned channel */	/* Set the XBee channel to your assigned channel */
198
    wl_set_channel(24);
199

  
200
	orb_set_color(BLUE);
201
	
202
    encoder_rst_dx(LEFT);
203
    encoder_rst_dx(RIGHT);
204

  
205
	forward(200);
206
	delay_ms(3000);
207

  
208

  
209
    orb_set_color(RED);
210
    while(1); /* END HERE */
211

  
212
    //return 0;
213
}
branches/16299_s10/code/behaviors/formation_control/hive/Makefile
1
# this is a local makefile
2

  
3
# Relative path to the root directory (containing lib directory)
4
ifndef COLONYROOT
5
COLONYROOT := ..
6

  
7
# Target file name (without extension).
8
TARGET = main
9

  
10
# Uncomment this to use the wireless library
11
USE_WIRELESS = 1
12

  
13
# com1 = serial port. Use lpt1 to connect to parallel port.
14
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
15
#AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
16
else
17
COLONYROOT := ../$(COLONYROOT)
18
endif
19

  
20
include $(COLONYROOT)/Makefile
branches/16299_s10/code/behaviors/formation_control/hive/hive.h
1
#ifndef _CIRCLE_H
2
#define _CIRCLE_H
3

  
4
#include <inttypes.h>
5

  
6
/**** This file should not be edited! ****/
7

  
8
/*
9
 * The packet structure is 2 bytes
10
 * byte 0 is the action, which is one of the values below
11
 * byte 1 is the robot id
12
 */
13

  
14
#define CIRCLE_ACTION_EXIST 'E'
15
#define CIRCLE_ACTION_POSITION 'P'
16
#define CIRCLE_ACTION_ACK 'A'
17
#define CIRCLE_ACTION_DONE 'D'
18
//#define EDGE 0;
19
//#define BEACON 1;
20

  
21
#endif
branches/16299_s10/code/behaviors/formation_control/push_pull/vectorSizeSpecs.m
1
% vectorSizeSpecs - Script used to help calculate the max scalar value by
2
% 		which to multiply the BOM unit vectors (thus increasing the
3
%		precision of the scaled vectors) such that in the worst case, the
4
%		total net sum will not overflow the data type used to represent
5
%		the net vector components.
6
%
7
%	Author: John Sexton, Colony Project, CMU Robotics Club
8

  
9

  
10
% Parameters
11
maxIntensity = 255;
12
dataBits = 16;
13
scalar = 25;
14

  
15
n = 0:8;
16
angle = n * 2 * pi / 16;
17

  
18
vector_components = sin(angle);
19
max_net_component = sum(vector_components);
20

  
21
% Formula for max_net_component (mnc), obtained by summing
22
% values from the unit circle at angles of 0 : pi/8 : pi
23
% in quadrants I and II
24
mnc = 1;									%sin(pi/2)
25
mnc = mnc + (2*  (1/2)*sqrt(2+sqrt(2)));	%sin(3pi/8) and sin(5pi/8)
26
mnc = mnc + (2*  sqrt(2)/2);				%sin(pi/4) and sin(3pi/4)
27
mnc = mnc + (2*  (1/2)*sqrt(2-sqrt(2)));	%sin(pi/8) and sin(7pi/8)
28

  
29

  
30
calc_scalar = (2^(dataBits-1)-1) / (max_net_component * maxIntensity);
31
% calc_scalar = 25.560 for 16 bits => use 25 as scalar for int data type
32

  
33
fprintf('With %d data bits, calculated scalar value: %.3f\n\n', dataBits, calc_scalar);
34

  
35

  
36
% Check worst case
37
scaled_vector_components = scalar * vector_components;
38

  
39
worst_case = fix(scaled_vector_components) * maxIntensity;
40
worst_sum = sum(worst_case);
41
fprintf('With scalar %d, max worst case sum: %d\n', scalar, round(worst_sum));
42
fprintf('Max number: 2^(%d-1) - 1 = %d\n', dataBits, (2^(dataBits-1)-1));
43

  
44

  
45
% Calculate the x and y component arrays which should be used in the
46
% BOM Vector Component Tables in push_pull.c
47
N = 0:15;
48
x_comp = fix(scalar * cos(2 * pi / 16 * N))
49
y_comp = fix(scalar * sin(2 * pi / 16 * N))
branches/16299_s10/code/behaviors/formation_control/push_pull/push_pull.c
1
#include <stdint.h>
2
#include <dragonfly_lib.h>
3
#include <wl_basic.h>
4
#include <stdlib.h>
5

  
6
int main (void) {
7

  
8
	Vector v;
9

  
10
	dragonfly_init(ALL_ON);
11
	xbee_init();
12
	encoders_init();
13

  
14
	orbs_set_color(BLUE, GREEN);
15
	delay_ms(1000);
16
	orbs_set_color(GREEN, BLUE);
17
	delay_ms(1000);
18
	orbs_set_color(RED, RED);
19

  
20
	while (1) {
21
		bom_print_usb(NULL);
22

  
23
		bom_get_vector(&v, NULL);
24

  
25
		usb_puts("x: ");
26
		usb_puti(v.x);
27
		usb_puts("\ty: ");
28
		usb_puti(v.y);
29
		usb_puts("\n");
30

  
31
		delay_ms(50);
32
	}
33

  
34
	while(1);
35

  
36
}
branches/16299_s10/code/behaviors/formation_control/push_pull/Makefile
1
# this is a local makefile
2

  
3
# Relative path to the root directory (containing lib directory)
4
ifndef COLONYROOT
5
COLONYROOT := ..
6

  
7
# Target file name (without extension).
8
TARGET = main
9

  
10
# Uncomment this to use the wireless library
11
USE_WIRELESS = 1
12

  
13
# com1 = serial port. Use lpt1 to connect to parallel port.
14
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
15

  
16
else
17
COLONYROOT := ../$(COLONYROOT)
18
endif
19

  
20
include $(COLONYROOT)/Makefile
branches/16299_s10/code/behaviors/formation_control/Makefile
1
# this is a local makefile
2

  
3
# Relative path to the root directory (containing lib directory)
4
ifndef COLONYROOT
5
COLONYROOT := ..
6

  
7
# Target file name (without extension).
8
TARGET = main
9

  
10
# Uncomment this to use the wireless library
11
USE_WIRELESS = 1
12

  
13
# com1 = serial port. Use lpt1 to connect to parallel port.
14
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
15

  
16
else
17
COLONYROOT := ../$(COLONYROOT)
18
endif
19

  
20
include $(COLONYROOT)/Makefile
branches/16299_s10/code/behaviors/formation_control/circle/circle.c
1
#include <dragonfly_lib.h>
2
#include <wl_basic.h>
3
#include <encoders.h>
4
#include "circle.h"
5

  
6
int EDGE = 0;
7
int BEACON = 1;
8
int timeout = 0;
9
int sending = 0;
10
int stop2 = 0;
11
struct vector slave_position;
12
int desired_max_bom;
13
int bom_max_counter;
14

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

  
36
void switch_sending(void)
37
{
38
	if(sending)
39
	{
40
		sending = 0;
41
		bom_off();
42
	}
43
	else
44
	{
45
		sending = 1;
46
		bom_on();
47
	}
48
}
49

  
50
/*
51
Current situation:
52
	go to the center. ends when each edge robto send "I am here" message
53

  
54
I think compasses are in the club, but not on the robots?
55

  
56
Assume we have the compasses, and write pseudocode, maybe?
57

  
58
TODO:
59
  Center:
60
    1. Make a move forward method/state to make the circle begin moving as a group.
61

  
62
  Edge: 
63
    2. Find a way to follow the middle robot and preserve circle structure, or follow a directional command given by the center robot.
64
    3. Fix the error case with wireless - if the center robot does not receive the first exist packet, the robot keeps sending it and will cause error later.
65

  
66
  In general:
67
    More testing.  Especially with robots that are actually working.
68
*/
69

  
70

  
71
/*
72
	This program is used to make robots target a center (leader) robot using the BOM,
73
	then drive toward it and stop at a certain distance away.
74
	
75
	The distance will eventually be adjustable.
76

  
77
	With adjustment, the leader robot will be able to turn and use its standardized
78
	rangefinder to reposition or space the robots evenly.
79
	
80
	AuTHORS: Nico, Alex, Reva, Echo, Steve
81
*/
82

  
83

  
84
/* 
85
TODO:
86
	Used: Bots 1, 7
87
	16 Performed Badly
88
	12 worked ok as beacon, not well as edge
89
	
90
		Fix orient code so the bot does not toggle back and forth when it tries to turn
91
		
92
		Use the center bot to check distances
93
Done-->	Count them ("spam" method)
94
		Use beacon to find relative positions
95
		Beacon tells them how to move to be at the right distance
96
		*done*Wireless communication, initialization
97
*/
98

  
99

  
100

  
101
void forward(int speed){			// set the motors to this forward speed.
102
	motor_l_set(FORWARD,speed);
103
	motor_r_set(FORWARD,speed);
104
}
105
void left(int speed){				// turn left at this speed.
106
	motor_l_set(FORWARD,speed);
107
	motor_r_set(BACKWARD,speed);
108
}
109
void right(int speed){
110
	motor_l_set(BACKWARD,speed);
111
	motor_r_set(FORWARD,speed);
112
}
113
void stop(void){					// could be set to motors_off(), or just use this as an alternative.
114
	motor_l_set(BACKWARD,0);			// stop() is better - motors_off() creates a slight delay to turn them back on.
115
	motor_r_set(FORWARD,0);
116
}
117
void setforward(int spd1, int spd2){
118
	motor_l_set(FORWARD,spd1);
119
	motor_r_set(FORWARD,spd2);
120
}
121
void backward(int speed){
122
	motor_l_set(BACKWARD, speed);
123
	motor_r_set(BACKWARD, speed);
124
}
125
int get_distance(void){				// takes an averaged reading of the front rangefinder
126
	int temp,distance,kk=5;			// kk sets this to 5 readings.
127
	distance =0;
128
	for (int i=0; i<kk; i++){
129
		temp = range_read_distance(IR2);
130
		if (temp == -1)
131
		{
132
			//temp=0;
133
			i--;
134
		}
135
		else
136
			distance+= temp;
137
		delay_ms(3);
138
	}
139
	if (kk>0)
140
		return (int)(distance/kk);
141
	else 
142
		return 0;
143
}
144

  
145

  
146

  
147
/*
148
	Orients the robot so that it is facing the beacon (or the broadcasting BOM).
149
	
150
*/
151
void correctTurn(void)
152
{
153
	orb1_set_color(BLUE);			// BLUE and PURPLE
154
	left(220);
155
	while(1)
156
	{
157
		int bomNum = 0;				// bomNum is the current maximum reading
158
		bom_refresh(BOM_ALL);
159
		bomNum = bom_get_max();
160
		usb_puti(bomNum);
161
		if(bomNum == 4)				// when it's turned the right way, stop
162
		{
163
			timeout = 0;
164
			//motor_l_set(1, 200);
165
			//motor_r_set(1, 200);
166
			break;				// exits the while() loop to stop the method
167
		}
168
		else					// facing the wrong way
169
		{
170
			if(bomNum == -1)
171
			{
172
				timeout++;
173
				
174
				if(timeout > 5000)	// if it's been looking too long, move a little bit as it turns
175
				{
176
					motor_r_set(FORWARD, 210);
177
					motor_l_set(BACKWARD, 190);
178
				}
179
			}
180
			else if((bomNum >= 12) || (bomNum < 4))
181
			{
182
				motor_l_set(FORWARD, 200);
183
				motor_r_set(BACKWARD, 200);
184
				timeout = 0;
185
			}
186
			else
187
			{
188
				motor_l_set(BACKWARD, 200);
189
				motor_r_set(FORWARD, 200);
190
				timeout = 0;
191
			}
192
		}
193
	}
194
	return;
195
}
196

  
197

  
198
/*
199
	This is supposed to (essentially) do a correct turn, then move forward or something.
200
	Actually, we should just get rid of this.
201
*/
202
void correctApproach(void)
203
{
204
	int bomNum = 0;
205
	bom_refresh(BOM_ALL);
206
	bomNum = bom_get_max();
207
	usb_puti(bomNum);
208
	if(bomNum == 4)
209
	{	
210
		motor_l_set(1, 200);
211
		motor_r_set(1, 200);
212
	}
213
	else
214
	{
215
		if(bomNum == -1){}
216
		else if((bomNum >= 12) || (bomNum < 4))
217
		{
218
			motor_l_set(FORWARD, 200);
219
			motor_r_set(BACKWARD, 200);
220
		}
221
		else
222
		{
223
			motor_l_set(BACKWARD, 200);
224
			motor_r_set(FORWARD, 200);
225
		}
226
		delay_ms(100);
227
	}
228
}
229

  
230
/* 
231
	drives forward a hardcoded distance. May not be useful.
232
*/
233
void go_straight(void){
234
	forward(200);
235
	encoder_rst_dx(LEFT);
236
	encoder_rst_dx(RIGHT);
237
	delay_ms(100); 
238
	int x_left = encoder_get_x(LEFT), x_right = encoder_get_x(RIGHT);
239
	int count = 0;
240
	int d;
241
	while (count<25){						//count = 25 when bot6; count <12
242
		x_left = encoder_get_x(LEFT);
243
		x_right = encoder_get_x(RIGHT);
244
		d = x_right-x_left;
245
		if (d>13 || d<-13){
246
			if(d<50 && d>-50){
247
				d = round(1.0*d/4);
248
				setforward(200+d, 200-d);
249
			}
250
		}
251
		delay_ms(32);
252
		count++;
253
	}
254
}
255

  
256

  
257
/* 
258
    BLINK the given number times
259
*/
260
void blink(int num) {
261
	for(int i = 0; i<num; i++)
262
	{
263
		orb_set_color(ORB_OFF);
264
		delay_ms(350);
265
		orb_set_color(RED);
266
		delay_ms(200);
267
	}
268
	orb_set_color(ORB_OFF);
269
}
270

  
271

  
272

  
273

  
274

  
275

  
276

  
277
//*****************************************************************************************************************************************************************************************
278

  
279

  
280

  
281

  
282

  
283

  
284
/*
285
	A double state machine.  The robot is either an "edge" or a "beacon" depending on the initial potentiometer reading.
286
*/
287
int main(void)
288
{
289
	/* Initialize dragonfly board */
290
    	dragonfly_init(ALL_ON);
291
    	/* Initialize the basic wireless library */
292
    	wl_basic_init_default();
293
    	/* Set the XBee channel to your assigned channel */	/* Set the XBee channel to your assigned channel */
294
	wl_set_channel(24);
295

  
296
	int sending_counter = 0;
297
	int robotid = get_robotid();
298
	int used[16];
299
	int usedPointer=0;
300
	for (int i=0; i<16; i++) used[i] = 0;
301
	char send_buffer[2];
302
	int data_length;
303
	unsigned char *packet_data=wl_basic_do_default(&data_length);
304
	
305
	
306
	int state = EDGE;
307
	int beacon_State=0;
308
	int edge_State=0;
309
	int waitingCounter=0;
310
	int robotsReceived=0;
311
	int offset = 20, time=0;
312
	
313
	if(wheel()<100)
314
	{
315
		state=EDGE;
316
	}
317
	else
318
	{
319
		state=BEACON;
320
	}
321
	
322
	int distance=1000;					// how far away to stop.
323
	int onefoot=250, speed=250;				// one foot is 490 for bot 1; one foot is 200 for bot6
324
	
325
	while(1)
326
	{
327
		bom_refresh(BOM_ALL);
328
		
329
		/*
330
		*******TERMinology**************
331
		EDGE=0 other names: slave. Definition: robots on the edge of  the circle;
332
		BEACON=1 other name: master. Definition: robots in the center of the circle;
333
		
334
		
335
		*******EXPECTED MOVES ********** 
336
		The designed movement:
337
		 1. one center robot, several edge robots are on;
338
		 2. center robots: button 1 is pressed;
339
		 3. center robots: send global package telling edges that he exists;
340
		 4. EDGE robots response with ACK. 
341
		 5. EDGE robots wait for center robots to finish counting (DONE package)
342
		 6. EDGE robtos approach the center robtot and stop at the "onefoot" distance, send message to the center
343
		*/
344
		
345
		
346
		// decide if its is center or not.
347
		switch(state)
348
		{
349
			/**********
350
				if  EDGE /slave robots
351
			*/
352
			case 0:	
353
				switch(edge_State)
354
				{
355
					/*
356
						0. EDGE robots are on. 
357
						1. They are waiting for ExiST pacakage from the Center robots
358
						2. After they receive the package, they send ACK package to center.
359
						3. Wait the center robot to finish counting all EDGE robots
360
					*/
361
					case 0:   
362
						bom_off();
363
						orb1_set_color(YELLOW);orb2_set_color(CYAN);
364
						packet_data=wl_basic_do_default(&data_length);
365
						if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST)
366
						{
367
							send_buffer[0]=CIRCLE_ACTION_ACK;
368
							send_buffer[1]=robotid;
369
							
370
							wl_basic_send_global_packet(42,send_buffer,2);
371
							edge_State=1;
372
						}
373
					break;
374
					/*
375
						1. Wait for DONE package 
376
						2. The counting process is DONE
377
					*/
378
					case 1:		
379
					
380
						orb_set_color(YELLOW);orb2_set_color(PURPLE);
381
						packet_data=wl_basic_do_default(&data_length);
382
						//wl_basic_send_global_packet(42,send_buffer,2);
383
						
384
						if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE)
385
						{
386
							edge_State=2;
387
						}
388
					break;
389
					
390
					case 2:
391
						// COLOR afer DONE ---> MAGENTA
392
						orb_set_color(MAGENTA);
393
						correctTurn();			// turn to face the beacon
394
						forward(175);
395
						//range_init();
396
						
397
						
398
						distance = get_distance();
399
						time=0;
400
						while ((distance-offset)>=onefoot || distance==0 || (distance+offset)<onefoot)
401
						{
402
							if(distance==0)
403
								orb_set_color(WHITE);
404
							else if(distance-offset>=onefoot)
405
								forward(175);
406
							else
407
								backward(175);
408
							//correctApproach();
409
							distance = get_distance();
410
							delay_ms(14);
411
							time+=14;
412
							if(time>500){
413
								correctTurn();
414
								time=0;
415
							}
416
						}
417
							
418
						stop();
419
						orb_set_color(LIME);
420

  
421
						send_buffer[0]=CIRCLE_ACTION_ACK;
422
						send_buffer[1]=robotid;	
423
						wl_basic_send_global_packet(42,send_buffer,2);
424

  
425
						edge_State=3;
426
					break;
427
					
428
					// wait until the beacon sends out its robot ID
429
/*
430
	// as we don't check distance to center one by one now,  
431
	no more private communication at this point. 
432
					case 3:
433
						orb_set_color(YELLOW);
434
						orb2_set_color(GREEN);
435

  
436
						packet_data=wl_basic_do_default(&data_length);
437
						if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK && packet_data[1]==robotid)
438
						{
439
							bom_on();
440
							orb_set_color(ORANGE);
441
						}
442
						if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE && packet_data[1]==robotid)
443
						{
444
							bom_off();
445
							orb_set_color(YELLOW);
446
						}
447
					break;
448
*/
449
				}
450

  
451
*/				
452
				// NEW CODE FROM JOEL
453
				/*
454
				while(1)
455
				{
456
					packet_data = wl_basic_do_default(&data_length);
457
					
458
					if(packet_data[0]=='s') stop2=1;
459
					if(packet_data[0]=='a') switch_sending();
460
						
461
					if(sending)
462
					{
463
					
464
					}
465
				
466
					else // recieving
467
					{
468
				
469
						if(stop2)
470
						{
471
							motor_l_set(FORWARD,0);
472
							motor_r_set(FORWARD,0);
473
							orb1_set_color(GREEN);
474
						}
475
				
476
						else
477
						{
478
							int max_bom = bom_get_max();
479
								/*usb_puts("bom_get_max : ");
480
							usb_puti(max_bom);
481
							usb_puts("/n/r");*/
482
					/*	
483
						
484
							if(max_bom == 8)
485
							{	
486
								orb2_set_color(BLUE);
487
								motor_r_set(FORWARD,180);
488
								motor_l_set(FORWARD,180);
489
								
490
							}
491
							else if(max_bom == 7 || max_bom == 6 || max_bom == 5)
492
							{
493
								motor_l_set(FORWARD,180);
494
								motor_r_set(FORWARD,0);
495
							}
496
							else if(max_bom == -1);
497
							else 
498
							{
499
								orb2_set_color(GREEN);
500
								motor_l_set(FORWARD,0);
501
								motor_r_set(FORWARD,180);
502
							}
503
						}
504
					
505
					}
506
				
507
				delay_ms(10);
508
				
509
				} //end while
510
				*/
511
				//break;
512
				
513
				// END for EDGE robots
514
			
515

  
516

  
517

  
518

  
519

  
520
	
521
//*****************************************************************************************************************************************************************************************
522
			
523

  
524

  
525

  
526

  
527

  
528

  
529
			/*
530
			   The CENTER/BEACON/MASTER robot
531
			*/	
532
			case 1:			// BEACON /master /enter robots
533
				switch(beacon_State) {
534
				/*
535
				   0. center  robots on wait for pressing button 1
536
				*/
537
				case 0:		
538
					bom_on();
539
					orb_set_color(PURPLE);
540
					if(button1_click()) beacon_State=1;
541
					break;	
542
				/*
543
					1. Send EXIST package to EDGE robots
544
				*/
545
				case 1:		// sends a global exist packet to see how many robots there are
546
					orb_set_color(RED);
547
					send_buffer[0]=CIRCLE_ACTION_EXIST;
548
					send_buffer[1]=get_robotid();
549
					wl_basic_send_global_packet(42,send_buffer,2);
550
					usedPointer = 0;
551
					beacon_State=2;
552
					// wait for the edge to send for 1sec. 
553
					delay_ms(1000);
554
					break;
555
				/*
556
					2. Count the number of the EDGE robots 
557
					*******NOTE: at most  1500  times of loop ******
558
				*/
559
				case 2: 	
560
					waitingCounter++;
561
					orb1_set_color(YELLOW);
562
					orb2_set_color(BLUE);
563
					packet_data=wl_basic_do_default(&data_length);
564
					
565
					if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
566
					{
567
						orb_set_color(RED);
568
						orb2_set_color(BLUE);
569
						//only add to robots seen if you haven't gotten an ACK from this robot
570
						if(used[packet_data[1]]==0){		
571
							robotsReceived++;
572
							used[usedPointer] = packet_data[1];
573
							usedPointer++;
574

  
575
							usb_puts("Added: ");
576
							usb_puti(packet_data[1]);
577
							usb_puts("\r\n");
578
						}
579
					}
580
					if(waitingCounter >= 2000){
581
						beacon_State=3;
582
					}
583
					break;
584
				/*
585
					COUNTing is DONE.
586
					Sending DONE package.
587
				*/	
588
				case 3:
589
					blink(robotsReceived);
590
					orb_set_color(GREEN);
591
					send_buffer[0]=CIRCLE_ACTION_DONE;
592
					wl_basic_send_global_packet(42,send_buffer,2);
593
					beacon_State=4;
594
					break;
595
				/*
596
					Wait for all the robots to get to right distance/position 
597
				*/
598
				case 4: 
599
					orb1_set_color(YELLOW);
600
					orb2_set_color(WHITE);
601
					
602
					int numOk = 0;
603
					
604
					while(numOk<robotsReceived)
605
					{
606
						packet_data=wl_basic_do_default(&data_length);
607
						if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
608
						{
609
							numOk++;
610
						}
611
					}
612
					
613
					beacon_State = 5;
614
					break; 
615
				
616
				/**
617
 TODO: no need for "private" communication if we don't check distance one by one  
618
   So comment out this part at least for now.
619

  
620
				    Starts repositioning the edge robots one by one
621
				*/
622
/*				case 5:
623
					orb_set_color(MAGENTA);
624
					bom_off();
625
					for(int i=0; i<robotsReceived; i++)
626
										
627
					{
628
						usb_puts("used = ");
629
						usb_puti(used[i]);
630
						usb_puts("\r\n");
631
						send_buffer[0]=CIRCLE_ACTION_ACK;
632
						send_buffer[1]=used[i];
633
						wl_basic_send_global_packet(42,send_buffer,2);
634
						
635
						delay_ms(1000);
636

  
637
						send_buffer[0]=CIRCLE_ACTION_DONE;
638
						send_buffer[1]=used[i];
639
						wl_basic_send_global_packet(42,send_buffer,2);
640

  
641
						delay_ms(1000);
642
					}
643
*/
644

  
645
					/*orb_set_color(BLUE);
646
					// clock for switching the BOMs between the master and slave
647
					if(sending_counter++>4)	
648
					{
649
						switch_sending();
650
						sending_counter = 0;
651
						send_buffer[0] = 'a';
652
						wl_basic_send_global_packet(42, send_buffer, 1);
653
					}
654
					
655
					
656
					if(sending)
657
					{
658
						
659
					}
660
					
661
					else // recieving
662
					{
663
						int max_bom = bom_get_max();
664
						usb_puts("bom_get_max : ");
665
						usb_puti(max_bom);
666
						usb_puts("\n\r");
667
						
668
						if(max_bom == desired_max_bom)
669
						{
670
							// only stops the slave if two consecutive boms 
671
							//     reading give the desired bom as the max one. Filters the noise.
672
							if(bom_max_counter)                 
673
							{
674
								send_buffer[0] = 's';
675
								wl_basic_send_global_packet(42, send_buffer, 2);
676
							}
677
							bom_max_counter =1;
678
						}
679
						else bom_max_counter = 0;
680
						
681
					}
682
					*/
683
					//break;
684
			}
685
		}
686
	}
687

  
688
	orb_set_color(RED);
689
	while(1); /* END HERE */
690

  
691
	//return 0;
692
}
branches/16299_s10/code/behaviors/formation_control/circle/circle.h
1
#ifndef _CIRCLE_H
2
#define _CIRCLE_H
3

  
4
#include <inttypes.h>
5

  
6
/**** This file should not be edited! ****/
7

  
8
/*
9
 * The packet structure is 2 bytes
10
 * byte 0 is the action, which is one of the values below
11
 * byte 1 is the robot id
12
 */
13

  
14
#define CIRCLE_ACTION_EXIST 'E'
15
#define CIRCLE_ACTION_POSITION 'P'
16
#define CIRCLE_ACTION_ACK 'A'
17
#define CIRCLE_ACTION_DONE 'D'
18
//#define EDGE 0;
19
//#define BEACON 1;
20

  
21
#endif
branches/16299_s10/code/behaviors/formation_control/circle/Makefile
1
# this is a local makefile
2

  
3
# Relative path to the root directory (containing lib directory)
4
ifndef COLONYROOT
5
COLONYROOT := ..
6

  
7
# Target file name (without extension).
8
TARGET = main
9

  
10
# Uncomment this to use the wireless library
11
USE_WIRELESS = 1
12

  
13
# com1 = serial port. Use lpt1 to connect to parallel port.
14
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
15
#AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
16
else
17
COLONYROOT := ../$(COLONYROOT)
18
endif
19

  
20
include $(COLONYROOT)/Makefile
branches/16299_s10/code/behaviors/formation_control/circle_spacing/Makefile
1
# this is a local makefile
2

  
3
# Relative path to the root directory (containing lib directory)
4
ifndef COLONYROOT
5
COLONYROOT := ..
6

  
7
# Target file name (without extension).
8
TARGET = main
9

  
10
# Uncomment this to use the wireless library
11
USE_WIRELESS = 1
12

  
13
# com1 = serial port. Use lpt1 to connect to parallel port.
14
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM7:'; else echo '/dev/ttyUSB0'; fi)
15

  
16
else
17
COLONYROOT := ../$(COLONYROOT)
18
endif
19

  
20
include $(COLONYROOT)/Makefile
branches/16299_s10/code/behaviors/formation_control/circle_spacing/circle_spacing.c
1
#include <dragonfly_lib.h>
2
#include <wl_basic.h>
3
#include<encoders.h>
... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff