Statistics
| Branch: | Revision:

scoutos / scout / libscout / src / test_behaviors / ParkScout.cpp @ 68845e8e

History | View | Annotate | Download (3.16 KB)

1
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 */
25

    
26
#include "ParkScout.h"
27
#define LEFT_EMITTER_ID 1
28
#define RIGHT_EMITTER_ID 0
29

    
30

    
31
#define LEFT 1
32
#define RIGHT 2
33
#define NEITHER 0
34

    
35
using namespace std;
36

    
37
ParkScout::ParkScout(string scoutname, Sensors* sensors) 
38
    : Behavior(scoutname, "Park Scout", sensors) {
39
    name = scoutname;
40
    scout_pos = new pos;
41
}
42

    
43
void ParkScout::run() {
44
    
45
    update_readings();
46
    start_side = get_side();
47

    
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
       update_readings();
52
        
53
     }
54

    
55
    motors->set_sides(0, 0, MOTOR_PERCENT);
56
           
57
    
58
    while(ok()) {  
59
        update_readings(); 
60
        reverse_park(); 
61
    }
62

    
63

    
64

    
65
}
66

    
67
bool ParkScout::reverse_park() {
68
    if(checkBOM(5, LEFT_EMITTER_ID) ||
69
       checkBOM(4, RIGHT_EMITTER_ID)) { //move forward
70
        motors->set_sides(10, 10, MOTOR_PERCENT);
71
    }
72
    else if(checkBOM(4, LEFT_EMITTER_ID) && 
73
            checkBOM(5, RIGHT_EMITTER_ID)) {
74
        motors->set_sides(-20, -20, MOTOR_PERCENT);
75
        return true;
76
    }
77
    else {   
78
        if (start_side == RIGHT) { //reverseright
79
            motors->set_sides(-40, -10, MOTOR_PERCENT);
80
        }
81
        else { //reverseleft
82
            motors->set_sides(-10, -40, MOTOR_PERCENT);
83
        
84
        }
85
    }
86
    return false;    
87
}
88

    
89
int ParkScout::get_side() {
90
    
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
    }
102
}
103

    
104
bool ParkScout::checkBOM(int bNum, unsigned int ID) {    
105
    if(readings[bNum] == true) {
106
        //ROS_INFO("Emitter ID: %d", senders[bNum]);
107
        return senders[bNum] == ID;
108
    }    
109
    return false;
110
    
111
}
112

    
113
void ParkScout::update_readings() {
114
    BomReadings bomR = bom->query();
115
    readings = bomR.readings;
116
    senders = bomR.senders;
117

    
118
}