Advertisement
Koelion

Untitled

Apr 22nd, 2018
69
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 4.48 KB | None | 0 0
  1. #include <Servo.h>
  2.  
  3.  
  4. const int trigPin = 9;
  5. const int echoPin = 10;
  6.  
  7. unsigned long previousMillis;
  8. unsigned long interval = 500;
  9.  
  10. int MoveForward = 0;
  11. int MoveBackward = 1;
  12. int MoveLeft = 3;
  13. int MoveRight = 4;
  14. int distanceTraveled = 0;
  15. bool lastState;
  16. Servo myservo;
  17.  
  18. void setup() {
  19. Serial.begin(9600);
  20. pinMode(A3, INPUT);
  21. pinMode(A4, INPUT);
  22. pinMode(4, INPUT);
  23. pinMode(5, INPUT);
  24. pinMode(7, OUTPUT);
  25. pinMode(8, OUTPUT);
  26. pinMode(12, OUTPUT);
  27. pinMode(13, OUTPUT);
  28. pinMode(10, OUTPUT);
  29. pinMode(11, OUTPUT);
  30. previousMillis = millis();
  31. myservo.attach(A1);
  32. pinMode(A0, INPUT);
  33. pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  34. pinMode(echoPin, INPUT); // Sets the echoPin as an Input
  35. }
  36.  
  37. bool targetReatched = false;
  38. void loop() {
  39. int v1 = digitalRead(4);
  40. Serial.print(v1);
  41.  
  42. int v2 = digitalRead(5);
  43. Serial.print(v2);
  44.  
  45. String str = Serial.readString();
  46. ParseInput(str);
  47. }
  48.  
  49. float EchoDistance()
  50. {
  51. digitalWrite(trigPin, LOW);
  52. delayMicroseconds(2);
  53. // Sets the trigPin on HIGH state for 10 micro seconds
  54. digitalWrite(trigPin, HIGH);
  55. delayMicroseconds(10);
  56. digitalWrite(trigPin, LOW);
  57. // Reads the echoPin, returns the sound wave travel time in microseconds
  58. float duration = pulseIn(echoPin, HIGH);
  59. // Calculating the distance
  60. float distance = duration*0.034 / 2;
  61. return distance;
  62. }
  63.  
  64. void ParseInput(String str)
  65. {
  66. if (str.length() <= 0)
  67. return;
  68.  
  69. char func = str[0];
  70. int arg = str.substring(2).toInt();
  71.  
  72. if (str[0] == 's')
  73. {
  74. Stop();
  75. }
  76. else if (str[0] == 'm')
  77. {
  78. GoStraightDistance(arg);
  79. }
  80. else if (str[0] == 'r')
  81. {
  82. TurnDistance(arg);
  83. }
  84. else if (str[0] == 'b')
  85. {
  86. Serial.println(EchoDistance());
  87. }
  88. else if (str[0] == 'p')
  89. {
  90. RotateServo(arg);
  91. }
  92. else if (str[0] == 'c')
  93. {
  94. Serial.println(ReadDistanceFromInfraRed());
  95. }
  96. else if (str[0] == 'w')
  97. {
  98. RotateServo(arg);
  99. Serial.println(ReadDistanceFromInfraRed());
  100. }
  101. else if (str[0] == 's')
  102. {
  103. LoopRotateAndCheckDistance();
  104. }
  105. }
  106.  
  107. float ReadDistanceFromInfraRed()
  108. {
  109. float v = digitalRead(A0);
  110. float u = v / 1023.0f * 3.30f;
  111. if (u < 0.50f)
  112. {
  113. u = 0.50f;
  114. }
  115.  
  116. return 70.0f / u - 6.50f;
  117. }
  118.  
  119. void LoopRotateAndCheckDistance()
  120. {
  121. int currentRot = 0;
  122. for (currentRot = 0; currentRot <= 180; currentRot += 1) {
  123.  
  124. myservo.write(currentRot);
  125. Serial.println("rot: %i, dist: %f", currentRot, ReadDistanceFromInfraRed());
  126. delay(15);
  127. }
  128. for (currentRot = 180; currentRot >= 0; currentRot -= 1) {
  129. myservo.write(currentRot);
  130. Serial.println("rot: %i, dist: %f", currentRot, ReadDistanceFromInfraRed());
  131. delay(15);
  132. }
  133. }
  134.  
  135.  
  136. void RotateServo(int angle)
  137. {
  138. myservo.write(angle);
  139. }
  140.  
  141. void GoStraightDistance(int distanceToTravel)
  142. {
  143. distanceTraveled = 0;
  144. int dir = MoveForward;
  145. if (distanceToTravel < 0)
  146. {
  147. dir = MoveBackward;
  148. distanceToTravel = distanceToTravel * -1;
  149. Serial.print(distanceToTravel);
  150. }
  151.  
  152. while (distanceTraveled < distanceToTravel)
  153. {
  154. if (dir == MoveForward)
  155. {
  156. Front();
  157. }
  158. else if (dir == MoveBackward)
  159. {
  160. Back();
  161. }
  162. CalcDistanceTraveled();
  163. }
  164. Stop();
  165. }
  166.  
  167. void TurnDistance(int distanceToTravel)
  168. {
  169. distanceTraveled = 0;
  170. int dir = MoveLeft;
  171. if (distanceToTravel < 0)
  172. {
  173. dir = MoveRight;
  174. distanceToTravel = distanceToTravel * -1;
  175. }
  176. while (distanceTraveled < distanceToTravel)
  177. {
  178. if (dir == MoveLeft)
  179. {
  180. LeftFront();
  181. }
  182. else if (dir == MoveRight)
  183. {
  184. RightFront();
  185. }
  186. CalcDistanceTraveled();
  187. }
  188. Stop();
  189. }
  190.  
  191.  
  192. void CalcDistanceTraveled() {
  193. int state = digitalRead(A3);
  194. if (state != lastState)
  195. {
  196. distanceTraveled++;
  197. Serial.print("distanceTraveled:");
  198. Serial.print(distanceTraveled);
  199. Serial.println();
  200. }
  201. lastState = state;
  202. }
  203.  
  204.  
  205. void RightFront()
  206. {
  207. digitalWrite(7, HIGH);
  208. digitalWrite(8, LOW);
  209. digitalWrite(10, HIGH);
  210.  
  211. digitalWrite(12, LOW);
  212. digitalWrite(13, HIGH);
  213. digitalWrite(11, HIGH);
  214. }
  215.  
  216. void LeftFront()
  217. {
  218. digitalWrite(7, LOW);
  219. digitalWrite(8, HIGH);
  220. digitalWrite(10, HIGH);
  221.  
  222. digitalWrite(12, HIGH);
  223. digitalWrite(13, LOW);
  224. digitalWrite(11, HIGH);
  225. }
  226.  
  227.  
  228. void Front()
  229. {
  230. digitalWrite(7, HIGH);
  231. digitalWrite(8, LOW);
  232. digitalWrite(10, HIGH);
  233.  
  234. digitalWrite(12, HIGH);
  235. digitalWrite(13, LOW);
  236. digitalWrite(11, HIGH);
  237. }
  238.  
  239. void Back()
  240. {
  241. digitalWrite(7, LOW);
  242. digitalWrite(8, HIGH);
  243. digitalWrite(10, HIGH);
  244.  
  245. digitalWrite(12, LOW);
  246. digitalWrite(13, HIGH);
  247. digitalWrite(11, HIGH);
  248. }
  249.  
  250.  
  251. void Stop()
  252. {
  253. digitalWrite(10, 0);
  254. digitalWrite(11, 0);
  255. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement