Revision a94f870d
add initial searching for charger
scout/libscout/src/test_behaviors/ApproachEmitter.cpp | ||
---|---|---|
38 | 38 |
#define CENTRED 2 |
39 | 39 |
#define SEARCH 0 |
40 | 40 |
#define ADJUSTED 1 |
41 |
#define SIGNAL_ONLY 3 |
|
41 | 42 |
|
43 |
#define LOST_SIGNAL_THRESH 100 |
|
42 | 44 |
|
43 | 45 |
using namespace std; |
44 | 46 |
|
... | ... | |
46 | 48 |
: Behavior(scoutname, "Approach Emitter", sensors) { |
47 | 49 |
name = scoutname; |
48 | 50 |
scout_pos = new pos; |
51 |
lost_signal_time = 0; |
|
52 |
wheel_vel_diff = 60; |
|
49 | 53 |
|
50 | 54 |
// initallize the side index values :D UGLY!!! But best solution so far |
51 | 55 |
FRONT.push_back(0); |
... | ... | |
99 | 103 |
return identified; |
100 | 104 |
} |
101 | 105 |
|
106 |
void ApproachEmitter::searchForSignal() { |
|
107 |
lost_signal_time++; |
|
108 |
int mid = 50; |
|
109 |
if ((lost_signal_time) > LOST_SIGNAL_THRESH) { |
|
110 |
lost_signal_time = 0; |
|
111 |
wheel_vel_diff= (wheel_vel_diff>=0)?wheel_vel_diff-10:60; |
|
112 |
ROS_INFO("CHANGING SPEED! %d", wheel_vel_diff); |
|
113 |
} |
|
114 |
motors->set_sides(mid-wheel_vel_diff/2, |
|
115 |
mid+wheel_vel_diff/2, MOTOR_PERCENT); |
|
116 |
} |
|
117 |
|
|
102 | 118 |
void ApproachEmitter::run() { |
103 | 119 |
ROS_INFO("BOM Main Running"); |
104 | 120 |
BomReadings bomR; |
... | ... | |
111 | 127 |
s = bomR.senders; |
112 | 128 |
identified = identifyPosition(r, s); |
113 | 129 |
if (identified == CENTRED) { |
114 |
//ROS_INFO("break"); |
|
115 | 130 |
break; |
131 |
} else if (identified == ADJUSTED) { |
|
132 |
lost_signal_time = 0; |
|
116 | 133 |
} else if (identified == SEARCH) { |
117 |
motors->set_sides(10, 40, MOTOR_PERCENT); |
|
134 |
searchForSignal(); |
|
135 |
} else { |
|
136 |
ROS_INFO("WTF?"); |
|
118 | 137 |
} |
119 | 138 |
} |
120 | 139 |
|
... | ... | |
163 | 182 |
else if(checkBOM(4, LEFT_EMITTER_ID) && |
164 | 183 |
checkBOM(5, RIGHT_EMITTER_ID)) { |
165 | 184 |
|
166 |
motors->set_sides(-20, -20, MOTOR_PERCENT);
|
|
185 |
motors->set_sides(-40, -40, MOTOR_PERCENT);
|
|
167 | 186 |
} |
168 | 187 |
else if (get_side() == REAR) { |
169 | 188 |
if (checkBOM(4,LEFT_EMITTER_ID) && |
... | ... | |
177 | 196 |
motors->set_sides(-10, -20, MOTOR_PERCENT); |
178 | 197 |
} |
179 | 198 |
else { |
180 |
motors->set_sides(-20, -20, MOTOR_PERCENT);
|
|
199 |
motors->set_sides(-40, -40, MOTOR_PERCENT);
|
|
181 | 200 |
} |
182 | 201 |
} |
183 | 202 |
else { |
Also available in: Unified diff