Advertisement
Koelion

Untitled

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