scoutos / scout / libscout / src / test_behaviors / ApproachEmitter.cpp @ 64a67b32
History | View | Annotate | Download (6.18 KB)
1 | 9b5d3fff | Hui Jun Tay | /**
|
---|---|---|---|
2 | * Copyright (c) 2011 Colony Project
|
||
3 | *
|
||
4 | * Permission is hereby granted, free of charge, to any person
|
||
5 | * obtaining a copy of this software and associated documentation
|
||
6 | * files (the "Software"), to deal in the Software without
|
||
7 | * restriction, including without limitation the rights to use,
|
||
8 | * copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||
9 | * copies of the Software, and to permit persons to whom the
|
||
10 | * Software is furnished to do so, subject to the following
|
||
11 | * conditions:
|
||
12 | *
|
||
13 | * The above copyright notice and this permission notice shall be
|
||
14 | * included in all copies or substantial portions of the Software.
|
||
15 | *
|
||
16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
||
17 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
||
18 | * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
||
19 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||
20 | * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
|
||
21 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||
22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
||
23 | * OTHER DEALINGS IN THE SOFTWARE.
|
||
24 | */
|
||
25 | |||
26 | #include "ApproachEmitter.h" |
||
27 | fb61cb17 | Yuyang (Misty) Guo | #define TOP_BOM 0 |
28 | #define BOT_BOM 1 |
||
29 | 64a67b32 | Hui Jun Tay | #define LEFT_EMITTER_ID 1 |
30 | #define RIGHT_EMITTER_ID 0 |
||
31 | |||
32 | |||
33 | #define LEFTSIDE 1 |
||
34 | #define RIGHTSIDE 2 |
||
35 | #define NEITHER 0 |
||
36 | |||
37 | #define CENTRED 2 |
||
38 | #define SEARCH 0 |
||
39 | #define ADJUSTED 1 |
||
40 | |||
41 | 9b5d3fff | Hui Jun Tay | |
42 | using namespace std; |
||
43 | |||
44 | fb61cb17 | Yuyang (Misty) Guo | ApproachEmitter::ApproachEmitter(string scoutname, Sensors* sensors)
|
45 | 9b5d3fff | Hui Jun Tay | : Behavior(scoutname, "Approach Emitter", sensors) {
|
46 | name = scoutname; |
||
47 | scout_pos = new pos;
|
||
48 | fb61cb17 | Yuyang (Misty) Guo | |
49 | // initallize the side index values :D UGLY!!! But best solution so far
|
||
50 | FRONT.push_back(0);
|
||
51 | LEFT.push_back(1);
|
||
52 | LEFT.push_back(2);
|
||
53 | LEFT.push_back(3);
|
||
54 | BACK.push_back(4);
|
||
55 | BACK.push_back(5);
|
||
56 | RIGHT.push_back(6);
|
||
57 | RIGHT.push_back(7);
|
||
58 | RIGHT.push_back(8);
|
||
59 | FRONT.push_back(9);
|
||
60 | |||
61 | } |
||
62 | |||
63 | int ApproachEmitter::identifyPosition(
|
||
64 | vector<uint32_t> readings, vector<uint32_t> senders) { |
||
65 | 64a67b32 | Hui Jun Tay | bool identified = SEARCH;
|
66 | fb61cb17 | Yuyang (Misty) Guo | int top_em_count = 0; |
67 | int bot_em_count = 0; |
||
68 | if (accumulate(&(readings[RIGHT.front()]),
|
||
69 | &(readings[RIGHT.back()])+1, 0) >= 2) { |
||
70 | for (int i=RIGHT.front(); i<RIGHT.back(); i++) { |
||
71 | top_em_count += (senders[i] == TOP_BOM); |
||
72 | bot_em_count += (senders[i] == BOT_BOM); |
||
73 | } |
||
74 | if (top_em_count * bot_em_count != 0) { |
||
75 | 64a67b32 | Hui Jun Tay | return CENTRED;
|
76 | fb61cb17 | Yuyang (Misty) Guo | } else if (top_em_count > 0) { |
77 | motors->set_sides(-50, -50, MOTOR_PERCENT); |
||
78 | } else {
|
||
79 | motors->set_sides(50, 50, MOTOR_PERCENT); |
||
80 | } |
||
81 | 64a67b32 | Hui Jun Tay | identified = ADJUSTED; |
82 | fb61cb17 | Yuyang (Misty) Guo | } else if (accumulate(&(readings[LEFT.front()]), |
83 | &(readings[LEFT.back()])+1, 0) >= 2) { |
||
84 | |||
85 | for (int i=LEFT.front(); i<LEFT.back(); i++) { |
||
86 | top_em_count += (senders[i] == TOP_BOM); |
||
87 | bot_em_count += (senders[i] == BOT_BOM); |
||
88 | } |
||
89 | if (top_em_count * bot_em_count != 0) { |
||
90 | } else if (top_em_count > 0) { |
||
91 | motors->set_sides(50, 50, MOTOR_PERCENT); |
||
92 | } else {
|
||
93 | motors->set_sides(-50, -50, MOTOR_PERCENT); |
||
94 | } |
||
95 | 64a67b32 | Hui Jun Tay | identified = ADJUSTED; |
96 | fb61cb17 | Yuyang (Misty) Guo | } |
97 | return identified;
|
||
98 | 9b5d3fff | Hui Jun Tay | } |
99 | |||
100 | void ApproachEmitter::run() {
|
||
101 | b71f5bca | Yuyang (Misty) Guo | ROS_INFO("BOM Main Running");
|
102 | 8ae3771a | Yuyang (Misty) Guo | BomReadings bomR; |
103 | vector<uint32_t> r; |
||
104 | vector<uint32_t> s; |
||
105 | 64a67b32 | Hui Jun Tay | int identified = 0; |
106 | 2380e2a4 | Yuyang (Misty) Guo | while (ok()) {
|
107 | 8ae3771a | Yuyang (Misty) Guo | bomR = bom->query(); |
108 | r = bomR.readings; |
||
109 | s = bomR.senders; |
||
110 | 64a67b32 | Hui Jun Tay | identified = identifyPosition(r, s); |
111 | if (identified == CENTRED) {
|
||
112 | ROS_INFO("break");
|
||
113 | break;
|
||
114 | } else if (identified == SEARCH) { |
||
115 | motors->set_sides(10, 40, MOTOR_PERCENT); |
||
116 | ac55b507 | Yuyang (Misty) Guo | } |
117 | 2380e2a4 | Yuyang (Misty) Guo | } |
118 | 64a67b32 | Hui Jun Tay | |
119 | update_readings(); |
||
120 | start_side = get_side(); |
||
121 | |||
122 | while (ok() && ((start_side == LEFTSIDE && checkBOM(3, RIGHT_EMITTER_ID)) || |
||
123 | (start_side == RIGHTSIDE && checkBOM(6, LEFT_EMITTER_ID)))) {
|
||
124 | motors->set_sides(25, 25, MOTOR_PERCENT); |
||
125 | update_readings(); |
||
126 | |||
127 | } |
||
128 | |||
129 | motors->set_sides(0, 0, MOTOR_PERCENT); |
||
130 | |||
131 | |||
132 | while(ok() && reverse_park()) {
|
||
133 | update_readings(); |
||
134 | } |
||
135 | |||
136 | |||
137 | //motors->set_sides(0,0,MOTOR_PERCENT);
|
||
138 | ROS_INFO("done");
|
||
139 | /*
|
||
140 | while(ok()) {
|
||
141 | update_readings();
|
||
142 | if (at_destination()) {
|
||
143 | motors->set_sides(0, 0, MOTOR_PERCENT);
|
||
144 | ROS_INFO("done");
|
||
145 | break;
|
||
146 | }
|
||
147 | else {
|
||
148 | motors->set_sides(-20,-20,MOTOR_PERCENT);
|
||
149 | |||
150 | }
|
||
151 | }
|
||
152 | */
|
||
153 | } |
||
154 | |||
155 | bool ApproachEmitter::reverse_park() {
|
||
156 | if(checkBOM(5, LEFT_EMITTER_ID) || |
||
157 | checkBOM(4, RIGHT_EMITTER_ID)) { //move forward |
||
158 | |||
159 | motors->set_sides(10, 10, MOTOR_PERCENT); |
||
160 | } |
||
161 | else if(checkBOM(4, LEFT_EMITTER_ID) && |
||
162 | checkBOM(5, RIGHT_EMITTER_ID)) {
|
||
163 | |||
164 | motors->set_sides(-20, -20, MOTOR_PERCENT); |
||
165 | if (at_destination()) {
|
||
166 | return false; |
||
167 | } |
||
168 | } |
||
169 | else {
|
||
170 | if (start_side == RIGHTSIDE) { //reverseright |
||
171 | motors->set_sides(-40, -10, MOTOR_PERCENT); |
||
172 | } |
||
173 | else { //reverseleft |
||
174 | motors->set_sides(-10, -40, MOTOR_PERCENT); |
||
175 | |||
176 | } |
||
177 | } |
||
178 | return true; |
||
179 | } |
||
180 | |||
181 | int ApproachEmitter::get_side() {
|
||
182 | |||
183 | if ((readings[1] > 0 || readings[2] > 0|| readings[3] > 0) && |
||
184 | (readings[6] == 0 && readings[7] == 0 && readings[8] == 0)) { |
||
185 | return LEFTSIDE;
|
||
186 | } |
||
187 | else if ((readings[6] > 0 || readings[7] > 0|| readings[8] > 0) && |
||
188 | (readings[1] == 0 && readings[2] == 0 && readings[3] == 0)) { |
||
189 | return RIGHTSIDE;
|
||
190 | } |
||
191 | else {
|
||
192 | return NEITHER;
|
||
193 | } |
||
194 | } |
||
195 | |||
196 | bool ApproachEmitter::checkBOM(int bNum, unsigned int ID) { |
||
197 | if(readings[bNum] == true) { |
||
198 | //ROS_INFO("Emitter ID: %d", senders[bNum]);
|
||
199 | return senders[bNum] == ID;
|
||
200 | } |
||
201 | return false; |
||
202 | |||
203 | } |
||
204 | |||
205 | void ApproachEmitter::update_readings() {
|
||
206 | BomReadings bomR = bom->query(); |
||
207 | readings = bomR.readings; |
||
208 | senders = bomR.senders; |
||
209 | |||
210 | } |
||
211 | |||
212 | bool ApproachEmitter::at_destination()
|
||
213 | { |
||
214 | vector<uint32_t> readings = linesensor->query(); |
||
215 | |||
216 | /* ROS_INFO("%d %d %d %d %d %d %d %d", readings[0], readings[1],
|
||
217 | readings[2],readings[3],readings[4],readings[5],readings[6],readings[7]);
|
||
218 | */
|
||
219 | //changed values so it detects solution
|
||
220 | if ( readings[0] > 75 || |
||
221 | readings[1] > 75 || |
||
222 | readings[2] > 75 || |
||
223 | readings[3] > 75 || |
||
224 | readings[4] > 75 || |
||
225 | readings[5] > 75 || |
||
226 | readings[6] > 75 || |
||
227 | readings[7] > 75 ) |
||
228 | { |
||
229 | return true; |
||
230 | } |
||
231 | return false; |
||
232 | 9b5d3fff | Hui Jun Tay | } |