Advertisement
Guest User

Untitled

a guest
Jan 16th, 2017
93
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 4.93 KB | None | 0 0
  1. int motors [] = {5,6,10,11};
  2. int inputPins [] = {A1,A2,A5}; //0 - center, 1 - right, 2 - left --------- Left,
  3. int inputVal [] = {0,0,0};
  4. int centerWhite = 800;
  5. int leftWhite = 765;
  6. int rightWhite = 635;
  7. int whiteCenter = 860;
  8. int whites[] = {900, 750, 765};
  9. int defWhite = 750;
  10. int readSpeed = 15;
  11. int dutyCycle = 65;
  12. int turnCount = 0;
  13. int interTurnTimer = 1050;
  14. int allWhiteForward = 900;
  15. boolean spinOnce = 0;
  16. unsigned long timer;
  17. /*
  18. Center: white - 980, black - 740, threshold - 860
  19. Right: white - 830, black - 440, threshold - 635
  20. Left: white - 880, black - 650, threshold - 765
  21. */
  22. //left - A3, center - A1, right - A2
  23. void setup (){
  24. Serial.begin (9600);
  25. for (int i = 0; i < 3; i++)
  26. {
  27. pinMode (motors [i], OUTPUT);
  28. }
  29. }
  30. void loop (){
  31. Serial.println ((inputVal [0])+100); //center
  32. Serial.println ((inputVal [1])+100); //right
  33. Serial.println ((inputVal [2])); //left
  34. Serial.println();
  35.  
  36. if(inputVal[0]+100 <= whites[0])
  37. {
  38. turnCount = 0;
  39. spinOnce = 0;
  40. if(inputVal[1]+100 <= defWhite && inputVal[0]+100 <= defWhite)
  41. {
  42. //go right if at intersection
  43. Serial.println("Intersection");
  44. timer = millis ();
  45. move (1);
  46. delay (35h0);
  47. while (true)
  48. {
  49. move(5);
  50. Serial.println("inter turn");
  51. Serial.println ((inputVal [0])+100); //center
  52. Serial.println ((inputVal [1])+100); //right
  53. Serial.println ((inputVal [2])); //left
  54. Serial.println();
  55. delay (readSpeed);
  56. for (int i = 0; i < 3; i++)
  57. {
  58. inputVal [i] = analogRead (inputPins [i]);
  59. }
  60. if (millis () - timer > interTurnTimer && inputVal [0] <= whites [0]){
  61. break;
  62. }
  63. }
  64. }
  65. else
  66. {
  67. spinOnce = 0;
  68. //go forward
  69. Serial.println("Forward");
  70. move (1);
  71. for (int i = 0; i < 3; i++){
  72. inputVal [i] = analogRead (inputPins [i]);
  73. }
  74. }
  75. }
  76. else if(inputVal [1]+100 <= whites[1]){
  77. turnCount = 0;
  78. spinOnce = 0;
  79. //go right
  80. Serial.println("Right");
  81. move (3);
  82. for (int i = 0; i < 3; i++){
  83. inputVal [i] = analogRead (inputPins [i]);
  84. }
  85. }else if(inputVal [2] <= whites[2]){
  86. turnCount = 0;
  87. spinOnce = 0;
  88. //go left
  89. Serial.println("Left");
  90. move (4);
  91. for (int i = 0; i < 3; i++){
  92. inputVal [i] = analogRead (inputPins [i]);
  93. }
  94. }else if(inputVal[0]+100 >= whites[0] && inputVal [1]+100 >= whites[1] && inputVal[2] >= whites[2]){
  95. //go left if all white
  96. turnCount++;
  97. if (turnCount >= 3)
  98. {
  99. if (spinOnce = 0){
  100. Serial.println ("Move for white");
  101. timer = millis ();
  102. move (1);
  103. delay (300);
  104. spinOnce = 1;
  105. }
  106. while (true)
  107. {
  108. Serial.println("All white. Left");
  109. Serial.println ((inputVal [0])+100); //center
  110. Serial.println ((inputVal [1])+100); //right
  111. Serial.println ((inputVal [2])); //left
  112. Serial.println ();
  113. move (6);
  114. for (int i = 0; i < 3; i++)
  115. {
  116. inputVal [i] = analogRead (inputPins [i]);
  117. }
  118. if (inputVal [0]+100 <= whites [0] || inputVal[1]+100 <= whites[1] || inputVal[2] <= whites[2]){
  119.  
  120. break;
  121. }
  122. }
  123. }
  124.  
  125. } else{
  126. Serial.println("no scenario found");
  127. move (1);
  128. for (int i = 0; i < 3; i++){
  129. inputVal [i] = analogRead (inputPins [i]);
  130. }
  131. }
  132. delay(readSpeed);
  133.  
  134. }
  135. //Sets movement direction.
  136. //1 is forwards | 2 is backwards | 3 is turn right | 4 is turn left
  137. void move (int direction){
  138.  
  139. if(direction == 1){
  140. analogWrite (motors [0], dutyCycle);
  141. analogWrite (motors [1], LOW);
  142.  
  143. analogWrite (motors [2], dutyCycle);
  144. analogWrite (motors [3], LOW);
  145. }
  146. else if(direction == 2){
  147. analogWrite (motors [0], LOW);
  148. analogWrite (motors [1], dutyCycle);
  149.  
  150. analogWrite (motors [2], LOW);
  151. analogWrite (motors [3], dutyCycle);
  152. }
  153. else if(direction == 3){
  154. analogWrite (motors [0], dutyCycle - 15);
  155. analogWrite (motors [1], LOW);
  156.  
  157. analogWrite (motors [2], LOW);
  158. analogWrite (motors [3], LOW);
  159. }
  160. else if(direction == 4){
  161. analogWrite (motors [0], LOW);
  162. analogWrite (motors [1], LOW);
  163.  
  164. analogWrite (motors [2], dutyCycle-10);
  165. analogWrite (motors [3], LOW);
  166. }else if(direction == 5){
  167. analogWrite (motors [0], dutyCycle-5); //+15
  168. analogWrite (motors [1], LOW);
  169.  
  170. analogWrite (motors [2], LOW);
  171. analogWrite (motors [3], dutyCycle-5); //-25
  172. }else if(direction == 6){
  173. analogWrite (motors [0], LOW);
  174. analogWrite (motors [1], dutyCycle-20);
  175.  
  176. analogWrite (motors [2], dutyCycle-20);
  177. analogWrite (motors [3], LOW);
  178. }
  179. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement