Revision bfffb0bd devices/DiWheelDrive/linefollow2.cpp

View differences:

devices/DiWheelDrive/linefollow2.cpp
24 24 
    int error = 0;
25 25 
    switch (this->strategy)
26 26 
    {
27 
    case LineFollowStrategy::EDGE_RIGHT:
27 
    case LineFollowStrategy::EDGE_RIGHT: case LineFollowStrategy::DOCKING:
28 28 
        error = -(FL -targetL + FR - targetR);
29 29 
        break;
30 30 
    case LineFollowStrategy::EDGE_LEFT:
......
57 57 
}
58 58 

  		




  59
  59
  
    
int LineFollow::followLine(int (&rpmSpeed)[2]){


  		




  60
  
  
    

  		




  
  60
  
    
    int correctionSpeed = 0;


  		




  61
  61
  
    
    switch (this->strategy)


  		




  62
  62
  
    
    {


  		




  63
  63
  
    
    case LineFollowStrategy::FUZZY:


  		




  64
  
  
    
        for (int i = 0; i < 4; i++) {


  		




  65
  
  
    
                vcnl4020AmbientLight[i] = global->vcnl4020[i].getAmbientLight();


  		




  66
  
  
    
                vcnl4020Proximity[i] = global->vcnl4020[i].getProximityScaledWoOffset();


  		




  67
  
  
    
            }


  		




  
  64
  
    
      for (int i = 0; i < 4; i++) {


  		




  
  65
  
    
          vcnl4020AmbientLight[i] = global->vcnl4020[i].getAmbientLight();


  		




  
  66
  
    
          vcnl4020Proximity[i] = global->vcnl4020[i].getProximityScaledWoOffset();


  		




  
  67
  
    
      }


  		




  
  68
  
    
      lineFollowing(vcnl4020Proximity, rpmSpeed);


  		




  
  69
  
    
      break;


  		




  
  70
  
    

  		




  
  71
  
    
    case LineFollowStrategy::DOCKING:


  		




  
  72
  
    
      correctionSpeed = -getPidCorrectionSpeed();


  		




  
  73
  
    
      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -global->forwardSpeed;


  		




  
  74
  
    

  		




  
  75
  
    
      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -global->forwardSpeed;


  		




  
  76
  
    
      


  		




  
  77
  
    
      break;


  		




  68
  78
  
    

  		




  69
  
  
    
        lineFollowing(vcnl4020Proximity, rpmSpeed);


  		




  70
  
  
    
        break;


  		




  71
  
  
    
    


  		




  72
  79
  
    
    default:


  		




  73
  
  
    
        int correctionSpeed = getPidCorrectionSpeed();


  		




  74
  
  
    
        // chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed,  global->threshWhite);


  		




  
  80
  
    
      correctionSpeed = getPidCorrectionSpeed();


  		




  
  81
  
    
      // chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed,  global->threshWhite);


  		




  75
  82
  
    

  		




  76
  
  
    
        rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   global->forwardSpeed + correctionSpeed;


  		




  
  83
  
    
      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   global->forwardSpeed + correctionSpeed;


  		




  77
  84
  
    

  		




  78
  
  
    
        rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed - correctionSpeed;


  		




  79
  
  
    
        return whiteFlag;


  		




  80
  
  
    
        break;


  		




  
  85
  
    
      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed - correctionSpeed;


  		




  
  86
  
    
      break;


  		




  81
  87
  
    
    }


  		




  
  88
  
    
    return whiteFlag;


  		




  82
  89
  
    
}


  		




  83
  90
  
    

  		




  84
  91
  
    

  		












Also available in:  Unified diff