daily pastebin goal
63%
SHARE
TWEET

Untitled

a guest Jan 11th, 2019 58 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. int U,P,I,D;
  2. #define KI 1
  3. #define KD 1
  4. #define KP 1
  5. task p()
  6. {while(true){
  7.   P=(45-Sensor(IN_3));
  8. }            }
  9. task i()
  10. {while(true){
  11.   Wait(10);
  12.   if(abs(45-Sensor(IN_3))<2)
  13.    {I=((45-Sensor(IN_3))+I);}
  14.   else
  15.    {I=0;}
  16. }            }
  17. task d()
  18. {while(true){
  19.   D=((45-Sensor(IN_3))-D);
  20.   Wait(8);
  21. }            }
  22. task calc()
  23. {while(true){
  24.   if (KP*P+KI*I+KD*D<30)
  25.    U=KP*P+KI*I+KD*D;
  26.   else
  27.    U=30;
  28. }            }
  29.  
  30. task turn()
  31. { while(true){
  32.   OnFwd(OUT_A, 40 +U);
  33.   OnFwd(OUT_C, 40 -U);
  34. }             }
  35.  
  36. task main()
  37. {SetSensorLight(IN_3);
  38. Precedes(p,i,d,calc,turn);
  39. I=0;
  40. D=0;}
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top