Project

General

Profile

Revision 8ae3771a

ID8ae3771af6e41759df00f7b9892697aa48526696
Parent 797244ed
Child 46ed9b9b

Added by Yuyang Guo over 10 years ago

change type of bom reading to include sender

View differences:

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

  		




  41
  41
  
    
void ApproachEmitter::run() {


  		




  42
  
  
    
    vector<uint32_t> r;


  		




  
  42
  
    
    BomReadings bomR;


  		




  43
  43
  
    
    int r_weighted, l_weighted;


  		




  44
  44
  
    
    int speed = 20;


  		




  
  45
  
    
    vector<uint32_t> r;


  		




  
  46
  
    
    vector<uint32_t> s;


  		




  45
  47
  
    
    while (ok()) {


  		




  46
  
  
    
        r = bom->query();


  		




  
  48
  
    
        bomR = bom->query();


  		




  
  49
  
    
        r = bomR.readings;


  		




  
  50
  
    
        s = bomR.senders;


  		




  47
  51
  
    
        if (accumulate(r.begin(), r.end(), 0)) {


  		




  48
  
  
    
            l_weighted = r[0] *BOM_W1 + r[1]*BOM_W2 + r[2]*BOM_W3 + r[3]*BOM_W4 + r[4]*BOM_W5;


  		




  49
  
  
    
            r_weighted = r[9] *BOM_W1 + r[8]*BOM_W2 + r[7]*BOM_W3 + r[6]*BOM_W4 - r[5]*BOM_W5;


  		




  50
  
  
    
            motors->set_sides(min(r_weighted+speed, 100),


  		




  51
  
  
    
                              min(l_weighted+speed, 100),  MOTOR_PERCENT);


  		




  
  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);


  		




  52
  56
  
    
        }


  		




  53
  57
  
    
    }


  		




  54
  58
  
    
}


  		












Also available in:  Unified diff