Advertisement
Guest User

Untitled

a guest
Aug 15th, 2019
149
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <Encoder.h>
  2.  
  3. Encoder Left(3, 2);
  4. Encoder Right(0, 1);
  5.  
  6. float R = 385; //mm
  7. float r = 12.5;
  8. float L = 8;
  9.  
  10. double LeftDist = 0;
  11. long LOldPos = -999;
  12. float LNewPos;
  13. int Correction;
  14.  
  15. double RightDist = 0;
  16. long ROldPos = -999;
  17. float RNewPos;
  18.  
  19. int fi = 0;
  20. unsigned long t;
  21.  
  22. int Xt = 0;
  23. int Xtb = 0;
  24. int Yt = 0;
  25. int Ytb = 0;
  26. int fit = 0;
  27. int fitb = 0;
  28. int Vicc;
  29.  
  30. void setup() {
  31. Serial.begin(9600);
  32. t = millis();
  33. }
  34.  
  35. void loop() {
  36. Encoders();
  37. if ((millis() - t) >= 100) {
  38. Xt = Xtb;
  39. Yt = Ytb;
  40. fit = fitb;
  41.  
  42. Vicc = (L * (LeftDist / (RightDist / LeftDist))) + 0.5;
  43. Xtb = Xt + (Vicc * (sin(fit + fi) - sin (fit)));
  44. Ytb = Yt + (Vicc * (cos(fit) - cos(fit + fi)));
  45. fitb = fit + fi;
  46.  
  47. t = millis();
  48. }
  49. Serial.print(Vicc);
  50. Serial.print(" ");
  51. Serial.print(Xtb);
  52. Serial.print(" ");
  53. Serial.println(Ytb);
  54. }
  55.  
  56. //0.9mm/1 deg
  57. void Encoders() {
  58. LNewPos = Left.read();
  59. if (LNewPos != LOldPos) {
  60. LOldPos = LNewPos;
  61. LeftDist = ((LNewPos / 840) * (2 * r * 3.14)) / 10; //MM / Rev
  62. }
  63. //-----------------------------------------------------
  64. RNewPos = Right.read();
  65. if (RNewPos != ROldPos) {
  66. ROldPos = RNewPos;
  67. RightDist = ((RNewPos / 840) * (2 * r * 3.14)) / 10; //MM / Rev
  68. }
  69. fi = (abs(RightDist - LeftDist) * 0.9) - Correction;
  70. if (fi >= 360) Correction += 360;
  71. else if (fi <= -360) Correction -= 360;
  72. }
Advertisement
Advertisement
Advertisement
RAW Paste Data Copied
Advertisement