Zanuark

Lab 5 code

Apr 14th, 2020
48
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 1.96 KB | None | 0 0
  1. #include <RedBot.h> //includes the redbot library
  2. RedBotMotors motors;// Instantiate the motor control object.
  3. RedBotEncoder encoder = RedBotEncoder(A3, 10); // Initialize encoders
  4.  
  5. //1 wheel turn goes 20.42 centimeters
  6. //one meter is 4.897 turns
  7. //one wheel turn is 48 motor turns
  8. //one meter is 235 motor turns
  9. //one meter is 940 encoder ticks
  10. //one 90* turn is 66 motor turns.
  11.  
  12.  
  13. int lCount=0;
  14. int rCount=0;
  15. int n = 0;
  16. bool amIStraight = true;
  17. void setup(){
  18. Serial.begin(9600);
  19. Serial.println("left right");
  20. Serial.println("================");
  21. encoder.clearEnc(BOTH);
  22. }
  23.  
  24.  
  25. void loop() {
  26.  
  27. lCount = encoder.getTicks(LEFT); // read the left motor encoder
  28. rCount = encoder.getTicks(RIGHT);
  29. Serial.print(lCount);
  30. Serial.print(" ");
  31. Serial.print("\t");
  32. Serial.println(rCount);
  33.  
  34. while(amIStraight==true){
  35. lCount = encoder.getTicks(LEFT); // read the left motor encoder
  36. rCount = encoder.getTicks(RIGHT);
  37. Serial.print(lCount);
  38. Serial.print(" ");
  39. Serial.print("\t");
  40. Serial.println(rCount);
  41. motors.leftMotor(-60);
  42. motors.rightMotor(56);
  43. if(lCount > rCount + 10){
  44. motors.leftMotor(-57);
  45. motors.rightMotor(56);
  46. }
  47. if(lCount + 10 < rCount) {
  48. motors.rightMotor(54);
  49. motors.leftMotor(-60);
  50. }
  51. if((lCount + 10 > rCount) && (lCount < rCount + 10)){
  52. motors.leftMotor(-60);
  53. motors.rightMotor(57);
  54. }
  55. if(lCount > 940 && rCount > 940){
  56. amIStraight = false;
  57. motors.stop();
  58. encoder.clearEnc(BOTH);
  59. }
  60. }
  61. //This is where I come to cry. GO ON HOUR 5 BABY!
  62. delay(500);
  63. while(amIStraight == false){
  64. lCount = encoder.getTicks(LEFT); // read the left motor encoder
  65. rCount = encoder.getTicks(RIGHT);
  66. Serial.print(lCount);
  67. Serial.print(" ");
  68. Serial.print("\t");
  69. Serial.println(rCount);
  70. motors.leftMotor(-60);
  71. motors.rightMotor(0);
  72.  
  73. if(lCount > 204){
  74. motors.stop();
  75. amIStraight = true;
  76. encoder.clearEnc(BOTH);
  77. }
  78.  
  79. delay(500);
  80. }}
Advertisement
Add Comment
Please, Sign In to add comment