Revision fb61cb17
Add first stage of charging behavoir
| scout/libscout/src/test_behaviors/ApproachEmitter.cpp | ||
|---|---|---|
| 24 | 24 |
*/ |
| 25 | 25 |
|
| 26 | 26 |
#include "ApproachEmitter.h" |
| 27 |
#define BOM_W1 5 |
|
| 28 |
#define BOM_W2 10 |
|
| 29 |
#define BOM_W3 15 |
|
| 30 |
#define BOM_W4 20 |
|
| 31 |
#define BOM_W5 30 |
|
| 27 |
#define TOP_BOM 0 |
|
| 28 |
#define BOT_BOM 1 |
|
| 32 | 29 |
|
| 33 | 30 |
using namespace std; |
| 34 | 31 |
|
| 35 |
ApproachEmitter::ApproachEmitter(string scoutname, Sensors* sensors)
|
|
| 32 |
ApproachEmitter::ApproachEmitter(string scoutname, Sensors* sensors) |
|
| 36 | 33 |
: Behavior(scoutname, "Approach Emitter", sensors) {
|
| 37 | 34 |
name = scoutname; |
| 38 | 35 |
scout_pos = new pos; |
| 36 |
|
|
| 37 |
// initallize the side index values :D UGLY!!! But best solution so far |
|
| 38 |
FRONT.push_back(0); |
|
| 39 |
LEFT.push_back(1); |
|
| 40 |
LEFT.push_back(2); |
|
| 41 |
LEFT.push_back(3); |
|
| 42 |
BACK.push_back(4); |
|
| 43 |
BACK.push_back(5); |
|
| 44 |
RIGHT.push_back(6); |
|
| 45 |
RIGHT.push_back(7); |
|
| 46 |
RIGHT.push_back(8); |
|
| 47 |
FRONT.push_back(9); |
|
| 48 |
|
|
| 49 |
} |
|
| 50 |
|
|
| 51 |
int ApproachEmitter::identifyPosition( |
|
| 52 |
vector<uint32_t> readings, vector<uint32_t> senders) {
|
|
| 53 |
bool identified = 0; |
|
| 54 |
int top_em_count = 0; |
|
| 55 |
int bot_em_count = 0; |
|
| 56 |
if (accumulate(&(readings[RIGHT.front()]), |
|
| 57 |
&(readings[RIGHT.back()])+1, 0) >= 2) {
|
|
| 58 |
for (int i=RIGHT.front(); i<RIGHT.back(); i++) {
|
|
| 59 |
top_em_count += (senders[i] == TOP_BOM); |
|
| 60 |
bot_em_count += (senders[i] == BOT_BOM); |
|
| 61 |
} |
|
| 62 |
if (top_em_count * bot_em_count != 0) {
|
|
| 63 |
//ROS_INFO("HAND OVER TO HUIJUN!");
|
|
| 64 |
} else if (top_em_count > 0) {
|
|
| 65 |
motors->set_sides(-50, -50, MOTOR_PERCENT); |
|
| 66 |
} else {
|
|
| 67 |
motors->set_sides(50, 50, MOTOR_PERCENT); |
|
| 68 |
} |
|
| 69 |
identified = 1; |
|
| 70 |
} else if (accumulate(&(readings[LEFT.front()]), |
|
| 71 |
&(readings[LEFT.back()])+1, 0) >= 2) {
|
|
| 72 |
|
|
| 73 |
for (int i=LEFT.front(); i<LEFT.back(); i++) {
|
|
| 74 |
top_em_count += (senders[i] == TOP_BOM); |
|
| 75 |
bot_em_count += (senders[i] == BOT_BOM); |
|
| 76 |
} |
|
| 77 |
if (top_em_count * bot_em_count != 0) {
|
|
| 78 |
} else if (top_em_count > 0) {
|
|
| 79 |
motors->set_sides(50, 50, MOTOR_PERCENT); |
|
| 80 |
} else {
|
|
| 81 |
motors->set_sides(-50, -50, MOTOR_PERCENT); |
|
| 82 |
} |
|
| 83 |
identified = 1; |
|
| 84 |
} |
|
| 85 |
return identified; |
|
| 39 | 86 |
} |
| 40 | 87 |
|
| 41 | 88 |
void ApproachEmitter::run() {
|
| 42 | 89 |
ROS_INFO("BOM Main Running");
|
| 43 | 90 |
BomReadings bomR; |
| 44 |
int r_weighted, l_weighted; |
|
| 45 |
int speed = 20; |
|
| 46 | 91 |
vector<uint32_t> r; |
| 47 | 92 |
vector<uint32_t> s; |
| 48 | 93 |
while (ok()) {
|
| 49 | 94 |
bomR = bom->query(); |
| 50 | 95 |
r = bomR.readings; |
| 51 | 96 |
s = bomR.senders; |
| 52 |
ROS_INFO("BOM Main Running");
|
|
| 53 |
if (accumulate(r.begin(), r.end(), 0)) {
|
|
| 54 |
for (int i=0; i<10; ++i) {
|
|
| 55 |
ROS_INFO("BOM %d reading is %d sender is %d", i, r[i], s[i]);
|
|
| 56 |
} |
|
| 57 |
|
|
| 58 |
//motors->set_sides(min(r_weighted+speed, 100), |
|
| 59 |
// min(l_weighted+speed, 102), MOTOR_PERCENT); |
|
| 97 |
if (identifyPosition(r, s)) {
|
|
| 98 |
// do nothing |
|
| 99 |
} else {
|
|
| 100 |
motors->set_sides(0, 50, MOTOR_PERCENT); |
|
| 60 | 101 |
} |
| 61 | 102 |
} |
| 103 |
while (ok()) {}
|
|
| 62 | 104 |
} |
Also available in: Unified diff