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