Revision b71f5bca scout/libscout/src/test_behaviors/ApproachEmitter.cpp

View differences:

scout/libscout/src/test_behaviors/ApproachEmitter.cpp
39 39
}
40 40

  
41 41
void ApproachEmitter::run() {
42
    ROS_INFO("BOM Main Running");
42 43
    BomReadings bomR;
43 44
    int r_weighted, l_weighted;
44 45
    int speed = 20;
......
48 49
        bomR = bom->query();
49 50
        r = bomR.readings;
50 51
        s = bomR.senders;
52
        ROS_INFO("BOM Main Running");
51 53
        if (accumulate(r.begin(), r.end(), 0)) {
52
           l_weighted = r[0] *BOM_W1 + r[1]*BOM_W2 + r[2]*BOM_W3 + r[3]*BOM_W4 + r[4]*BOM_W5;
53
           r_weighted = r[9] *BOM_W1 + r[8]*BOM_W2 + r[7]*BOM_W3 + r[6]*BOM_W4 - r[5]*BOM_W5;
54
           motors->set_sides(min(r_weighted+speed, 100),
55
                             min(l_weighted+speed, 102),  MOTOR_PERCENT);
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);
56 60
        }
57 61
    }
58 62
}

Also available in: Unified diff