Revision b8085493

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
  
    

  		












  

    
      devices/DiWheelDrive/linefollow2.hpp
    
  





  19
  19
  
    
  ;


  		




  20
  20
  
    
  int accSum = 0;


  		




  21
  21
  
    
  float oldError = 0;


  		




  
  22
  
    
  int biggestDiff = 0;


  		




  22
  23
  
    

  		




  23
  24
  
    
};


  		




  24
  25
  
    

  		












  

    
      devices/DiWheelDrive/userthread.cpp
    
  





  462
  462
  
    
	//   lineFollownew


  		




  463
  463
  
    
	//else


  		




  464
  464
  
    
            // lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl, &global);


  		




  465
  
  
    
            setRpmSpeed(rpmFuzzyCtrl);


  		




  
  465
  
    
            // setRpmSpeed(rpmFuzzyCtrl);


  		




  466
  466
  
    
        }


  		




  467
  467
  
    

  		




  468
  468
  
    
		// this->sleep(US2ST(5));


  		












Also available in:  Unified diff