Advertisement
zhalpern

Final Final Code

Nov 26th, 2018
106
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 2.53 KB | None | 0 0
  1. #include <Servo.h>
  2. #include <WemosInit.h> //Custom library - ultrasonicPing()
  3.  
  4. #define motor1pin 12 //GPIO pin setting for motor1
  5. #define motor2pin 2 //GPIO pin setting for motor2
  6.  
  7. Servo motor1; //Create servo motor1 object to control a servo, right wheel
  8. Servo motor2; //Create servo motor2 object to control a servo
  9.  
  10. double distance;
  11.  
  12. void setup() //put your setup code here, to run once:
  13. {
  14. Serial.begin(115200); //to use the Serial Monitor
  15. pinMode(D2, OUTPUT); //Trigger - Sends out ultranonic wave
  16. pinMode(D3, INPUT); //Echo - Receives return signal
  17.  
  18. motor1.attach(motor1pin); //motor1 is attached using the motor1pin
  19. motor2.attach(motor2pin); //motor2 is attached using the motor2pin
  20.  
  21. motor1.write(0); //motor1 rotates with a full speed
  22. motor2.write(0); //motor2 rotates with a full speed
  23. }
  24.  
  25. void loop()
  26. {
  27. delay(1000);
  28.  
  29. //Starting Run
  30. motor1.write(0);
  31. motor2.write(0);
  32. delay(1200);
  33.  
  34. //First Turn
  35. motor1.write(0);
  36. motor2.write(90);
  37. delay(200);
  38.  
  39. //Straightaway before Target 1
  40. motor1.write(0);
  41. motor2.write(0);
  42. delay(1100);
  43.  
  44. //Backup after target 1
  45. motor1.write(180);
  46. motor2.write(180);
  47. delay(700);
  48.  
  49. //Turn back after target 1
  50. motor1.write(0);
  51. motor2.write(90);
  52. delay(250);
  53.  
  54. //Second Straightaway
  55. motor1.write(0);
  56. motor2.write(0);
  57. delay(1925);
  58.  
  59. //Turn to target 2
  60. motor1.write(0);
  61. motor2.write(90);
  62. delay(350);
  63.  
  64. //Move towards target 2
  65. motor1.write(0);
  66. motor2.write(0);
  67. delay(850);
  68.  
  69. //Back up to target 3
  70. motor1.write(180);
  71. motor2.write(180);
  72. delay(1975);
  73.  
  74. //Move forward after target 3
  75. motor1.write(0);
  76. motor2.write(0);
  77. delay(100);
  78.  
  79. //Turn after target 3
  80. motor1.write(0);
  81. motor2.write(90);
  82. delay(525);
  83.  
  84. //Movement towards target 4
  85. motor1.write(0);
  86. motor2.write(0);
  87. delay(1650);
  88.  
  89. //Turn perpendicular to target 4
  90. motor1.write(90);
  91. motor2.write(0);
  92. delay(400);
  93.  
  94. //Hit target 4
  95. motor1.write(0);
  96. motor2.write(0);
  97. delay(925);
  98.  
  99. //Reverse from target 4
  100. motor1.write(180);
  101. motor2.write(180);
  102. delay(600);
  103.  
  104. //Turn towards start
  105. motor1.write(0);
  106. motor2.write(90);
  107. delay(650);
  108.  
  109. //Run towards start
  110. motor1.write(0);
  111. motor2.write(0);
  112. delay(500);
  113.  
  114. /* motor1.write(0);
  115. motor2.write(90);
  116. delay(750);
  117. motor1.write(0);
  118. motor2.write(0);
  119. delay(3500);
  120. motor1.write(90);
  121. motor2.write(0);
  122. delay(500);
  123. motor1.write(0);
  124. motor2.write(0);
  125. delay(700);
  126. */
  127.  
  128. motor1.write(90);
  129. motor2.write(90);
  130. delay(99999);
  131. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement