Mikkel_Serrantes

Untitled

Apr 17th, 2021
676
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  
  2. double distkP = 0.05; ///0.05
  3. double distkI = 0.01; ///0.01
  4. double distkD = 0.1; //0.1
  5.  
  6. double diffkP = 0.05;
  7. double diffkI = 0.0;
  8. double diffkD = 0.0;
  9.  
  10. double driveTarget;
  11. double distSpeed;
  12. double diffSpeed;
  13.  
  14. double distError;
  15. double diffError;
  16.  
  17. double prevDistError;
  18. double prevDiffError;
  19.  
  20. double diffIntegral;
  21. double distIntegral;
  22.  
  23. double distDerivative;
  24. double diffDerivative;
  25.  
  26. bool enableDrivePID = true;
  27.  
  28. void resetEncoders(){
  29.   frontLeft.setPosition(0,degrees);
  30.   frontRight.setPosition(0,degrees);
  31.   backLeft.setPosition(0,degrees);
  32.   backRight.setPosition(0,degrees);
  33.  
  34. }
  35.  
  36. bool isDrive = true;
  37.  
  38. int sleepTimeout = 10;
  39.  
  40. void driveStraight(int distance, int timeOut = 1250) // cm
  41. {
  42.  
  43.   resetEncoders();
  44.  
  45.   while (distError != 1)
  46.   {
  47.     Brain.Screen.printAt( 10, 50, "distError %6.2f", distError);
  48.     Brain.Screen.printAt( 10, 80, "diffError %6.2f", diffError);
  49.     double leftEncoder = (frontLeft.position(degrees) + backLeft.position(degrees)/2);
  50.      
  51.     leftEncoder = (leftEncoder/ 360)* 53.28;
  52.      
  53.     double rightEncoder = (frontRight.position(degrees) + backRight.position(degrees)/2);
  54.     rightEncoder = (rightEncoder/ 360)* 53.28;
  55.    
  56.     distError = distance - ((leftEncoder + rightEncoder)/2); //Calculate distance error
  57.     diffError = leftEncoder - rightEncoder; //Calculate difference error
  58.    
  59.     // Find the integral ONLY if within controllable range AND if the distance error is not equal to zero
  60.     if( std::abs(distError) < 2 && distError != 0)
  61.     {
  62.       distIntegral = 0;
  63.     }
  64.     else
  65.     {
  66.       distIntegral = 0; //Otherwise, reset the integral
  67.     }
  68.    
  69.     // Find the integral ONLY if within controllable range AND if the difference error is not equal to zero
  70.     if( std::abs(diffError) < 60 && diffError != 0)
  71.     {
  72.       diffIntegral = diffIntegral + diffError;
  73.     }
  74.     else
  75.     {
  76.       diffIntegral = 0; //Otherwise, reset the integral
  77.     }
  78.  
  79.     distDerivative = distError - prevDistError; //Calculate distance derivative
  80.     diffDerivative = diffError - prevDiffError; //Calculate difference derivative
  81.  
  82.     prevDistError = distError; //Update previous distance error
  83.     prevDiffError = diffError; //Update previous difference error
  84.  
  85.     distSpeed = distError * distkP + distIntegral * distkI + distDerivative * distkD; //Calculate distance speed
  86.     LeftMotorFBS(distSpeed);
  87.     RightMotorFBS(distSpeed);
  88.  
  89.     diffSpeed = (diffError * diffkP) + (diffIntegral * diffkI) + (diffDerivative* diffkD); //Calculate difference (turn) speed
  90.  
  91.     frontLeft.spin(fwd, distSpeed  + diffSpeed, voltageUnits::volt); //Set motor values
  92.     frontRight.spin(fwd, distSpeed + diffSpeed, voltageUnits::volt); //Set motor values
  93.     backLeft.spin(fwd, distSpeed - diffSpeed, voltageUnits::volt); //Set motor values
  94.     backRight.spin(fwd, distSpeed + diffSpeed, voltageUnits::volt);
  95.  
  96.     wait(sleepTimeout, msec);
  97.     timeOut = timeOut - sleepTimeout;
  98.   } while (distError > -1 && timeOut >= 0) {
  99.  
  100. SetMotorBreakingType(); ////All four motors stop
  101. enableDrivePID = false;
  102.  
  103.   }
  104. }
  105.  
RAW Paste Data

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×