root / trunk / code / behaviors / bfs_fsm / orbit_fsm.c @ 1469
History | View | Annotate | Download (3.96 KB)
1 | 672 | dsschult | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <wireless.h> |
||
3 | #include <wl_token_ring.h> |
||
4 | #include "orbit_fsm.h" |
||
5 | #include "bfs_fsm.h" |
||
6 | |||
7 | /* Used to orbit a robot
|
||
8 | |||
9 | Must be within BOM range of robot before activating
|
||
10 | |||
11 | orbit_theta_stop requires encoders, and is incomplete
|
||
12 | |||
13 | Assumes:
|
||
14 | both robots are already in a token ring
|
||
15 | */
|
||
16 | |||
17 | |||
18 | /* private function prototype */
|
||
19 | void orbit_evaluate_state(void); |
||
20 | |||
21 | |||
22 | /* primary init */
|
||
23 | void orbit_init(int robot) { |
||
24 | //range_init();
|
||
25 | //analog_init(1);
|
||
26 | //motors_init();
|
||
27 | //orb_init();
|
||
28 | //orb_enable();
|
||
29 | //usb_init();
|
||
30 | |||
31 | /*Start in the start state, ORBIT_SEEK */
|
||
32 | orbit_state = ORBIT_INSERTION; |
||
33 | |||
34 | orbit_otherRobot = robot; // set which robot to seek and orbit
|
||
35 | |||
36 | orbit_pControl=0;
|
||
37 | orbit_bom = bfs_bom; |
||
38 | orbit_theta = 0;
|
||
39 | orbit_theta_stop = 1000;
|
||
40 | |||
41 | /*Initialize distances to zero.*/
|
||
42 | orbit_d1=1000; orbit_d2=1000; orbit_d3=1000; orbit_d4=1000; orbit_d5=1000; |
||
43 | |||
44 | } |
||
45 | /* secondary init */
|
||
46 | void orbit_init_theta(int robot,int theta) { |
||
47 | //range_init();
|
||
48 | //analog_init(1);
|
||
49 | //motors_init();
|
||
50 | //orb_init();
|
||
51 | //orb_enable();
|
||
52 | //usb_init();
|
||
53 | |||
54 | /*Start in the start state, ORBIT_INSERTION */
|
||
55 | orbit_state = ORBIT_INSERTION; |
||
56 | |||
57 | orbit_otherRobot = robot; // set which robot to seek and orbit
|
||
58 | |||
59 | orbit_pControl=0;
|
||
60 | orbit_bom = bfs_bom; |
||
61 | orbit_theta = 0;
|
||
62 | orbit_theta_stop = (theta>0)?theta:-1; // check theta_stop |
||
63 | |||
64 | /*Initialize distances to zero.*/
|
||
65 | orbit_d1=1000; orbit_d2=1000; orbit_d3=1000; orbit_d4=1000; orbit_d5=1000; |
||
66 | |||
67 | } |
||
68 | |||
69 | /*The main function, call this to update states as frequently as possible.*/
|
||
70 | void orbit_fsm(void) { |
||
71 | |||
72 | /*The following lines ensure that undefined (-1) values
|
||
73 | will not update the distances.*/
|
||
74 | int temp;
|
||
75 | |||
76 | //temp=range_read_distance(IR1);
|
||
77 | //orbit_d1=(temp == -1) ? orbit_d1 : temp;
|
||
78 | |||
79 | temp=range_read_distance(IR2); |
||
80 | orbit_d2=(temp == -1) ? orbit_d2 : temp;
|
||
81 | |||
82 | //temp=range_read_distance(IR3);
|
||
83 | //orbit_d3=(temp == -1) ? orbit_d3 : temp;
|
||
84 | |||
85 | //temp=range_read_distance(IR4);
|
||
86 | //orbit_d4=(temp == -1) ? orbit_d4 : temp;
|
||
87 | |||
88 | //temp=range_read_distance(IR5);
|
||
89 | //orbit_d5=(temp == -1) ? orbit_d5 : temp;
|
||
90 | |||
91 | wl_do(); // update wireless
|
||
92 | |||
93 | // get bom reading
|
||
94 | temp = wl_token_get_my_sensor_reading(orbit_otherRobot); |
||
95 | orbit_bom = (temp == -1) ? orbit_bom : temp;
|
||
96 | |||
97 | // modify bom reading so right is negative, left is positive
|
||
98 | if (orbit_bom <= 12) |
||
99 | orbit_bom -= 4;
|
||
100 | else
|
||
101 | orbit_bom -= 20;
|
||
102 | |||
103 | |||
104 | if (orbit_state == ORBIT_SEEK && orbit_d2 < ORBIT_DISTANCE)
|
||
105 | orbit_state = ORBIT_INSERTION; // begin to orbit
|
||
106 | if (orbit_state == ORBIT_INSERTION &&
|
||
107 | (orbit_bom == ORBIT_DIRECTION || (orbit_bom + 1) == ORBIT_DIRECTION || (orbit_bom - 1) == ORBIT_DIRECTION)) |
||
108 | orbit_state = ORBIT_ORBITING; // orbit achieved
|
||
109 | 740 | dsschult | if (orbit_state == ORBIT_ORBITING && ((orbit_d2 < ORBIT_STOP_DISTANCE)
|
110 | 672 | dsschult | || (orbit_theta >= orbit_theta_stop))) |
111 | orbit_state = ORBIT_STOP; // orbit obstructed
|
||
112 | |||
113 | // evaluate state
|
||
114 | orbit_evaluate_state(); |
||
115 | } |
||
116 | |||
117 | |||
118 | //Acts on state change.
|
||
119 | void orbit_evaluate_state(){
|
||
120 | switch(orbit_state){
|
||
121 | case(ORBIT_SEEK): orb_set_color(RED);
|
||
122 | // move towards robot
|
||
123 | orbit_pControl = orbit_bom*10;
|
||
124 | move(ORBIT_STRAIGHT_SPEED,orbit_pControl); |
||
125 | break;
|
||
126 | |||
127 | case(ORBIT_INSERTION): orb_set_color(GREEN);
|
||
128 | // rotate into orbit, perpendicular to other robot
|
||
129 | orbit_pControl = -ORBIT_DIRECTION*10;
|
||
130 | 740 | dsschult | move(ORBIT_STRAIGHT_SPEED/2,orbit_pControl);
|
131 | 672 | dsschult | break;
|
132 | |||
133 | case(ORBIT_ORBITING): orb_set_color(BLUE);
|
||
134 | // go straight with slight rotation
|
||
135 | if (orbit_bom == ORBIT_DIRECTION)
|
||
136 | orbit_pControl = ORBIT_DIRECTION; |
||
137 | else if (orbit_bom < ORBIT_DIRECTION) |
||
138 | orbit_pControl = ORBIT_DIRECTION-ORBIT_CORRECTION; |
||
139 | else
|
||
140 | orbit_pControl = ORBIT_DIRECTION+ORBIT_CORRECTION; |
||
141 | |||
142 | move(ORBIT_STRAIGHT_SPEED,orbit_pControl); |
||
143 | break;
|
||
144 | |||
145 | case(ORBIT_STOP): orb_set_color(YELLOW);
|
||
146 | move(0,0); |
||
147 | break;
|
||
148 | |||
149 | default: orb_set_color(YELLOW);
|
||
150 | /*Should never get here, so stop.*/
|
||
151 | move(0,0); |
||
152 | break;
|
||
153 | } |
||
154 | } |
||
155 |