Revision b8085493 devices/DiWheelDrive/linefollow2.cpp

View differences:

devices/DiWheelDrive/linefollow2.cpp
56 56 

  		




  57
  57
  
    
void LineFollow::stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){


  		




  58
  58
  
    
    int targetSensor = 0x38;


  		




  59
  
  
    
    int actualSensorL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] >> 8;


  		




  60
  
  
    
    int actualSensorR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] >> 8;


  		




  
  59
  
    
    int actualSensorL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] ;


  		




  
  60
  
    
    int actualSensorR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] ;


  		




  61
  61
  
    
    int targetSpeedL = global->rpmForward[constants::DiWheelDrive::LEFT_WHEEL];


  		




  62
  62
  
    
    int targetSpeedR = global->rpmForward[constants::DiWheelDrive::RIGHT_WHEEL];


  		




  63
  63
  
    

  		




  
  64
  
    

  		




  
  65
  
    
    int diff = actualSensorR - actualSensorL; 


  		




  64
  66
  
    
    int error = targetSensor - (actualSensorL + actualSensorR);


  		




  65
  67
  
    

  		




  66
  68
  
    
    accSum += error;


  		




  67
  69
  
    
    int dTerm = error - oldError;


  		




  68
  70
  
    

  		




  
  71
  
    
    if (diff > biggestDiff){


  		




  
  72
  
    
        biggestDiff = diff;


  		




  
  73
  
    
    }


  		




  69
  74
  
    
    int correctionSpeed = (int) (Kp * error + Ki * accSum + Kd * dTerm);   


  		




  70
  75
  
    
    chprintf((BaseSequentialStream*) &SD1, "Correction Speed: %d\n", correctionSpeed);


  		




  71
  76
  
    
    rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = targetSpeedL + correctionSpeed;


  		




  72
  77
  
    
    rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = targetSpeedR - correctionSpeed;


  		




  73
  78
  
    

  		




  
  79
  
    
    chprintf((BaseSequentialStream*) &SD1, "Diff: %d, Biggest: %d\n", correctionSpeed, biggestDiff);


  		




  
  80
  
    

  		




  74
  81
  
    
}


  		




  75
  82
  
    

  		




  76
  83
  
    

  		












Also available in:  Unified diff