Revision 64a67b32
Merged ApproachEmitter behavior with ParkScout. Still buggy.
| scout/libscout/src/test_behaviors/ApproachEmitter.cpp | ||
|---|---|---|
| 26 | 26 |
#include "ApproachEmitter.h" |
| 27 | 27 |
#define TOP_BOM 0 |
| 28 | 28 |
#define BOT_BOM 1 |
| 29 |
#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 |
|
|
| 29 | 41 |
|
| 30 | 42 |
using namespace std; |
| 31 | 43 |
|
| ... | ... | |
| 50 | 62 |
|
| 51 | 63 |
int ApproachEmitter::identifyPosition( |
| 52 | 64 |
vector<uint32_t> readings, vector<uint32_t> senders) {
|
| 53 |
bool identified = 0;
|
|
| 65 |
bool identified = SEARCH;
|
|
| 54 | 66 |
int top_em_count = 0; |
| 55 | 67 |
int bot_em_count = 0; |
| 56 | 68 |
if (accumulate(&(readings[RIGHT.front()]), |
| ... | ... | |
| 60 | 72 |
bot_em_count += (senders[i] == BOT_BOM); |
| 61 | 73 |
} |
| 62 | 74 |
if (top_em_count * bot_em_count != 0) {
|
| 63 |
//ROS_INFO("HAND OVER TO HUIJUN!");
|
|
| 75 |
return CENTRED;
|
|
| 64 | 76 |
} else if (top_em_count > 0) {
|
| 65 | 77 |
motors->set_sides(-50, -50, MOTOR_PERCENT); |
| 66 | 78 |
} else {
|
| 67 | 79 |
motors->set_sides(50, 50, MOTOR_PERCENT); |
| 68 | 80 |
} |
| 69 |
identified = 1;
|
|
| 81 |
identified = ADJUSTED;
|
|
| 70 | 82 |
} else if (accumulate(&(readings[LEFT.front()]), |
| 71 | 83 |
&(readings[LEFT.back()])+1, 0) >= 2) {
|
| 72 | 84 |
|
| ... | ... | |
| 80 | 92 |
} else {
|
| 81 | 93 |
motors->set_sides(-50, -50, MOTOR_PERCENT); |
| 82 | 94 |
} |
| 83 |
identified = 1;
|
|
| 95 |
identified = ADJUSTED;
|
|
| 84 | 96 |
} |
| 85 | 97 |
return identified; |
| 86 | 98 |
} |
| ... | ... | |
| 90 | 102 |
BomReadings bomR; |
| 91 | 103 |
vector<uint32_t> r; |
| 92 | 104 |
vector<uint32_t> s; |
| 105 |
int identified = 0; |
|
| 93 | 106 |
while (ok()) {
|
| 94 | 107 |
bomR = bom->query(); |
| 95 | 108 |
r = bomR.readings; |
| 96 | 109 |
s = bomR.senders; |
| 97 |
if (identifyPosition(r, s)) {
|
|
| 98 |
// do nothing |
|
| 99 |
} else {
|
|
| 100 |
motors->set_sides(0, 50, MOTOR_PERCENT); |
|
| 110 |
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); |
|
| 101 | 116 |
} |
| 102 | 117 |
} |
| 103 |
while (ok()) {}
|
|
| 118 |
|
|
| 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; |
|
| 104 | 232 |
} |
| scout/libscout/src/test_behaviors/ApproachEmitter.h | ||
|---|---|---|
| 46 | 46 |
private: |
| 47 | 47 |
int identifyPosition(std::vector<uint32_t> readings, |
| 48 | 48 |
std::vector<uint32_t> senders); |
| 49 |
|
|
| 50 |
bool reverse_park(); |
|
| 51 |
int get_side(); |
|
| 52 |
bool checkBOM(int bNum, unsigned int ID); |
|
| 53 |
void update_readings(); |
|
| 54 |
bool at_destination(); |
|
| 55 |
|
|
| 49 | 56 |
std::string name; |
| 50 | 57 |
pos* scout_pos; |
| 51 | 58 |
std::vector<uint8_t> LEFT; |
| ... | ... | |
| 53 | 60 |
std::vector<uint8_t> FRONT; |
| 54 | 61 |
std::vector<uint8_t> BACK; |
| 55 | 62 |
|
| 63 |
int start_side; |
|
| 64 |
std::vector<uint32_t> readings; |
|
| 65 |
std::vector<uint32_t> senders; |
|
| 66 |
|
|
| 67 |
|
|
| 68 |
|
|
| 56 | 69 |
}; |
| 57 | 70 |
#endif |
Also available in: Unified diff