Revision 68845e8e
ParkScout (2nd half of AutoParking Behavior) in Simular is working without end brakes.
| scout/libscout/src/test_behaviors/ParkScout.cpp | ||
|---|---|---|
| 24 | 24 |
*/ |
| 25 | 25 |
|
| 26 | 26 |
#include "ParkScout.h" |
| 27 |
#define LEFT_EMITTER_ID 110
|
|
| 28 |
#define RIGHT_EMITTER_ID 111
|
|
| 27 |
#define LEFT_EMITTER_ID 1 |
|
| 28 |
#define RIGHT_EMITTER_ID 0
|
|
| 29 | 29 |
|
| 30 | 30 |
|
| 31 |
#define LEFT 0 |
|
| 32 |
#define RIGHT 1 |
|
| 31 |
#define LEFT 1 |
|
| 32 |
#define RIGHT 2 |
|
| 33 |
#define NEITHER 0 |
|
| 33 | 34 |
|
| 34 | 35 |
using namespace std; |
| 35 | 36 |
|
| ... | ... | |
| 41 | 42 |
|
| 42 | 43 |
void ParkScout::run() {
|
| 43 | 44 |
|
| 44 |
/* update_readings();
|
|
| 45 |
start_side = get_side(readings);
|
|
| 45 |
update_readings(); |
|
| 46 |
start_side = get_side(); |
|
| 46 | 47 |
|
| 47 |
while ((start_side == LEFT && checkBOM(3, RIGHT_EMITTER_ID)) || |
|
| 48 |
(start_side == RIGHT && checkBOM(6, RIGHT_EMITTER_ID))) {
|
|
| 49 |
motors->set_sides(100, 100, MOTOR_PERCENT); |
|
| 50 |
bomR = bom->query(); |
|
| 48 |
while (ok() && ((start_side == LEFT && checkBOM(3, RIGHT_EMITTER_ID)) || |
|
| 49 |
(start_side == RIGHT && checkBOM(6, LEFT_EMITTER_ID)))) {
|
|
| 50 |
motors->set_sides(25, 25, MOTOR_PERCENT); |
|
| 51 | 51 |
update_readings(); |
| 52 |
} |
|
| 52 |
|
|
| 53 |
} |
|
| 53 | 54 |
|
| 54 | 55 |
motors->set_sides(0, 0, MOTOR_PERCENT); |
| 55 | 56 |
|
| 56 | 57 |
|
| 57 |
while(ok) {
|
|
| 58 |
reverse_park(); |
|
| 59 |
} */ |
|
| 58 |
while(ok()) {
|
|
| 59 |
update_readings(); |
|
| 60 |
reverse_park(); |
|
| 61 |
} |
|
| 62 |
|
|
| 60 | 63 |
|
| 61 | 64 |
|
| 62 | 65 |
} |
| 63 |
/* |
|
| 64 |
void reverse_park() {
|
|
| 65 |
if(checkBOM(5, LEFT_EMITTER_ID) ||
|
|
| 66 |
|
|
| 67 |
bool ParkScout::reverse_park() {
|
|
| 68 |
if(checkBOM(5, LEFT_EMITTER_ID) || |
|
| 66 | 69 |
checkBOM(4, RIGHT_EMITTER_ID)) { //move forward
|
| 67 | 70 |
motors->set_sides(10, 10, MOTOR_PERCENT); |
| 68 | 71 |
} |
| 69 | 72 |
else if(checkBOM(4, LEFT_EMITTER_ID) && |
| 70 | 73 |
checkBOM(5, RIGHT_EMITTER_ID)) {
|
| 71 | 74 |
motors->set_sides(-20, -20, MOTOR_PERCENT); |
| 75 |
return true; |
|
| 72 | 76 |
} |
| 73 | 77 |
else {
|
| 74 | 78 |
if (start_side == RIGHT) { //reverseright
|
| 75 |
motors->set_sides(100, 20, MOTOR_PERCENT);
|
|
| 79 |
motors->set_sides(-40, -10, MOTOR_PERCENT);
|
|
| 76 | 80 |
} |
| 77 | 81 |
else { //reverseleft
|
| 78 |
motors->set_sides(20, 100, MOTOR_PERCENT);
|
|
| 82 |
motors->set_sides(-10, -40, MOTOR_PERCENT);
|
|
| 79 | 83 |
|
| 80 | 84 |
} |
| 81 | 85 |
} |
| 82 |
|
|
| 86 |
return false; |
|
| 83 | 87 |
} |
| 84 | 88 |
|
| 85 |
int get_side() {
|
|
| 89 |
int ParkScout::get_side() {
|
|
| 86 | 90 |
|
| 87 |
return RIGHT; |
|
| 91 |
if ((readings[1] > 0 || readings[2] > 0|| readings[3] > 0) && |
|
| 92 |
(readings[6] == 0 && readings[7] == 0 && readings[8] == 0)) {
|
|
| 93 |
return LEFT; |
|
| 94 |
} |
|
| 95 |
else if ((readings[6] > 0 || readings[7] > 0|| readings[8] > 0) && |
|
| 96 |
(readings[1] == 0 && readings[2] == 0 && readings[3] == 0)) {
|
|
| 97 |
return RIGHT; |
|
| 98 |
} |
|
| 99 |
else {
|
|
| 100 |
return NEITHER; |
|
| 101 |
} |
|
| 88 | 102 |
} |
| 89 | 103 |
|
| 90 |
bool checkBOM(int bNum, int ID) {
|
|
| 104 |
bool ParkScout::checkBOM(int bNum, unsigned int ID) {
|
|
| 91 | 105 |
if(readings[bNum] == true) {
|
| 92 |
ROS_INFO("Emitter ID: %d", senders[bNum]);
|
|
| 106 |
//ROS_INFO("Emitter ID: %d", senders[bNum]);
|
|
| 93 | 107 |
return senders[bNum] == ID; |
| 94 | 108 |
} |
| 95 | 109 |
return false; |
| 96 | 110 |
|
| 97 | 111 |
} |
| 98 | 112 |
|
| 99 |
void update_readings() {
|
|
| 113 |
void ParkScout::update_readings() {
|
|
| 100 | 114 |
BomReadings bomR = bom->query(); |
| 101 | 115 |
readings = bomR.readings; |
| 102 | 116 |
senders = bomR.senders; |
| 103 | 117 |
|
| 104 |
}*/ |
|
| 118 |
} |
|
| scout/libscout/src/test_behaviors/ParkScout.h | ||
|---|---|---|
| 47 | 47 |
private: |
| 48 | 48 |
std::string name; |
| 49 | 49 |
pos* scout_pos; |
| 50 |
/*int start_side;
|
|
| 50 |
int start_side; |
|
| 51 | 51 |
std::vector<uint32_t> readings; |
| 52 | 52 |
std::vector<uint32_t> senders; |
| 53 | 53 |
|
| 54 |
void reverse_park();
|
|
| 54 |
bool reverse_park();
|
|
| 55 | 55 |
int get_side(); |
| 56 |
bool checkBOM(int bNum, int ID); |
|
| 57 |
void update_readings(); */
|
|
| 56 |
bool checkBOM(int bNum, unsigned int ID);
|
|
| 57 |
void update_readings(); |
|
| 58 | 58 |
|
| 59 | 59 |
}; |
| 60 | 60 |
#endif |
Also available in: Unified diff