Project

General

Profile

Revision ac55b507

IDac55b507db3210e91c96e5f56325e3c58043bf97
Parent 1d198f8b
Child 46daeb86

Added by Yuyang Guo over 10 years ago

basic drive towards BOM working

View differences:

scout/libscout/src/test_behaviors/ApproachEmitter.cpp
34 34 
}
35 35 

  		




  36
  36
  
    
void ApproachEmitter::run() {


  		




  
  37
  
    
    vector<uint32_t> r;


  		




  
  38
  
    
    int r_weighted, l_weighted;


  		




  
  39
  
    
    int speed = 20;


  		




  37
  40
  
    
    while (ok()) {


  		




  38
  
  
    
        vector<uint32_t> readings;


  		




  39
  
  
    
        readings = bom->query();


  		




  40
  
  
    
        ROS_INFO("front BOM readings %d", readings[0]);


  		




  
  41
  
    
        r = bom->query();


  		




  
  42
  
    
        if (accumulate(r.begin(), r.end(), 0)) {


  		




  
  43
  
    
            l_weighted = r[0] *5 + r[1]*10 + r[2]*15 + r[3]*20 + r[4]*30;


  		




  
  44
  
    
            r_weighted = r[9] *5 + r[8]*10 + r[7]*15 + r[6]*20 - r[5]*30;


  		




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


  		




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


  		




  
  47
  
    
        }


  		




  41
  48
  
    
    }


  		




  42
  49
  
    
}


  		












Also available in:  Unified diff