Revision 68845e8e scout/libscout/src/test_behaviors/ParkScout.cpp

View differences:

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
}

Also available in: Unified diff