Mikkel_Serrantes

10955M PID

Apr 16th, 2021
727
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. void SetMotorBreakingType(brakeType type = brakeType::coast) {
  2.   backLeft.setStopping(type);
  3.   backRight.setStopping(type);
  4.   frontRight.setStopping(type);
  5.   frontLeft.setStopping(type);
  6.  
  7. }
  8.  
  9. const int accel_step = 9;
  10. const int deccel_step = 100; // no decel slew
  11. static int leftSpeed = 0;
  12. static int rightSpeed = 0;
  13.  
  14. void LeftMotorFBS(int leftTarget){
  15.   int step;
  16.  
  17.   if(abs(leftSpeed) < abs(leftTarget))
  18.     step = accel_step;
  19.   else
  20.     step = deccel_step;
  21.  
  22.   if(leftTarget > leftSpeed + step)
  23.     leftSpeed += step;
  24.   else if(leftTarget < leftSpeed - step)
  25.     leftSpeed -= step;
  26.   else
  27.     leftSpeed = leftTarget;
  28.  
  29.   LeftMotorFB(leftSpeed);
  30. }
  31. //slew control
  32. void RightMotorFBS(int rightTarget){
  33.   int step;
  34.  
  35.   if(abs(rightSpeed) < abs(rightTarget))
  36.     step = accel_step;
  37.   else
  38.     step = deccel_step;
  39.  
  40.   if(rightTarget > rightSpeed + step)
  41.     rightSpeed += step;
  42.   else if(rightTarget < rightSpeed - step)
  43.     rightSpeed -= step;
  44.   else
  45.     rightSpeed = rightTarget;
  46.  
  47.   RightMotorFB(rightSpeed);
  48. }
  49.  
  50. double distkP = 0.05; ///0.05
  51. double distkI = 0.01; ///0.01
  52. double distkD = 0.1; //0.1
  53.  
  54. double diffkP = 0.05;
  55. double diffkI = 0.0;
  56. double diffkD = 0.0;
  57.  
  58. double driveTarget;
  59. double distSpeed;
  60. double diffSpeed;
  61.  
  62. double distError;
  63. double diffError;
  64.  
  65. double prevDistError;
  66. double prevDiffError;
  67.  
  68. double diffIntegral;
  69. double distIntegral;
  70.  
  71. double distDerivative;
  72. double diffDerivative;
  73.  
  74. bool enableDrivePID = true;
  75.  
  76. void resetEncoders(){
  77.   frontLeft.setPosition(0,degrees);
  78.   frontRight.setPosition(0,degrees);
  79.   backLeft.setPosition(0,degrees);
  80.   backRight.setPosition(0,degrees);
  81.  
  82. }
  83.  
  84. int sleepTimeout = 10;
  85.  
  86. void driveStraight(int distance, int timeOut = 1250) // cm
  87. {
  88.  
  89.   resetEncoders();
  90.  
  91.   while (distError != -1)
  92.   {
  93.     Brain.Screen.printAt( 10, 50, "distError %6.2f", distError);
  94.     Brain.Screen.printAt( 10, 80, "diffError %6.2f", diffError);
  95.     double leftEncoder = (frontLeft.position(degrees) + backLeft.position(degrees)/2);
  96.      
  97.     leftEncoder = (leftEncoder/ 360)* 53.28;
  98.      
  99.     double rightEncoder = (frontRight.position(degrees) + backRight.position(degrees)/2);
  100.     rightEncoder = (rightEncoder/ 360)* 53.28;
  101.    
  102.     distError = distance - ((leftEncoder + rightEncoder)/2); //Calculate distance error
  103.     diffError = leftEncoder - rightEncoder; //Calculate difference error
  104.    
  105.     // Find the integral ONLY if within controllable range AND if the distance error is not equal to zero
  106.     if( std::abs(distError) < 2 && distError != 0)
  107.     {
  108.       distIntegral = distIntegral + distError;
  109.     }
  110.     else
  111.     {
  112.       distIntegral = 0; //Otherwise, reset the integral
  113.     }
  114.    
  115.     // Find the integral ONLY if within controllable range AND if the difference error is not equal to zero
  116.     if( std::abs(diffError) < 60 && diffError != 0)
  117.     {
  118.       diffIntegral = diffIntegral + diffError;
  119.     }
  120.     else
  121.     {
  122.       diffIntegral = 0; //Otherwise, reset the integral
  123.     }
  124.  
  125.     distDerivative = distError - prevDistError; //Calculate distance derivative
  126.     diffDerivative = diffError - prevDiffError; //Calculate difference derivative
  127.  
  128.     prevDistError = distError; //Update previous distance error
  129.     prevDiffError = diffError; //Update previous difference error
  130.  
  131.     distSpeed = distError * distkP + distIntegral * distkI + distDerivative * distkD; //Calculate distance speed
  132.     LeftMotorFBS(distSpeed);
  133.     RightMotorFBS(distSpeed);
  134.  
  135.     diffSpeed = (diffError * diffkP) + (diffIntegral * diffkI) + (diffDerivative* diffkD); //Calculate difference (turn) speed
  136.  
  137.  
  138.  
  139.     frontLeft.spin(fwd, distSpeed  + diffSpeed, voltageUnits::volt); //Set motor values
  140.     frontRight.spin(fwd, distSpeed + diffSpeed, voltageUnits::volt); //Set motor values
  141.     backLeft.spin(fwd, distSpeed - diffSpeed, voltageUnits::volt); //Set motor values
  142.     backRight.spin(fwd, distSpeed + diffSpeed, voltageUnits::volt);
  143.  
  144.     wait(sleepTimeout, msec);
  145.     timeOut = timeOut - sleepTimeout;
  146.   } while (distError >= distance && timeOut >= 0);
  147.  
  148. SetMotorBreakingType(); ////All four motors stop
  149. enableDrivePID = false;
  150. }
  151.  
  152.  
  153.  
  154. void autonomous(void) {
  155.  
  156.  
  157. enableDrivePID = true;
  158. driveStraight(-100);
  159.  
  160. enableDrivePID = true;
  161. driveStraight(100);
  162.  
  163. }
  164.  
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.

×