Advertisement
Guest User

robocar drive

a guest
Dec 1st, 2015
71
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 2.09 KB | None | 0 0
  1. int fwdPin1 = 4;
  2. int revPin1 = 1;
  3. int fwdPin2 = 2;
  4. int revPin2 = 3;
  5. int ledPin = 9;
  6. int sensorPin1 = 0;
  7. int sensorPin2 = 1;
  8. int sensorPin3 = 2;
  9. int lightLevel1, lightLevel2, lightLevel3;
  10. int white = 0, black = 0;
  11.  
  12. void setup() {
  13. // put your setup code here, to run once:
  14. pinMode(fwdPin1, OUTPUT);
  15. pinMode(revPin1, OUTPUT);
  16. pinMode(fwdPin2, OUTPUT);
  17. pinMode(revPin2, OUTPUT);
  18. pinMode(ledPin, OUTPUT);
  19. analogWrite(ledPin, 255);
  20. delay(5000);
  21. white = (analogRead(sensorPin1) + analogRead(sensorPin3)) / 2;
  22. black = analogRead(sensorPin2);
  23. }
  24.  
  25. void loop() {
  26. // put your main code here, to run repeatedly:
  27. int rightCount = 0, leftCount = 0;
  28. lightLevel1 = analogRead(sensorPin1); // left photoresistor
  29. lightLevel2 = analogRead(sensorPin2); // middle photoresistor
  30. lightLevel3 = analogRead(sensorPin3); // right photoresistor
  31. if (lightLevel2 <= black + 5) {
  32. forward(125);
  33. }
  34. else {
  35. boolean found = false;
  36. boolean goLeft = false;
  37.  
  38. while (found == false) {
  39.  
  40. if (lightLevel2 <= black + 30) {
  41. if (goLeft == true) {
  42. forward(250);
  43. }
  44. found = true;
  45. break;
  46. }
  47. if (rightCount < 20 && goLeft == false) {
  48. right(85);
  49. rightCount++;
  50. }
  51. if (rightCount == 20 && goLeft == false) {
  52. left(600);
  53. goLeft = true;
  54. }
  55. if (goLeft == true) {
  56. left(85);
  57. }
  58. }
  59. }
  60. }
  61.  
  62. void forward(int ms) {
  63. digitalWrite(fwdPin1, HIGH);
  64. digitalWrite(fwdPin2, HIGH);
  65. delay(ms); // time to run
  66. digitalWrite(fwdPin1, LOW);
  67. digitalWrite(fwdPin2, LOW);
  68. delay(200);
  69. }
  70.  
  71. void right(int ms) {
  72. digitalWrite(fwdPin1, HIGH);
  73. digitalWrite(revPin2, HIGH);
  74. delay(ms); // time to run
  75. digitalWrite(fwdPin1, LOW);
  76. digitalWrite(revPin2, LOW);
  77. lightLevel2 = analogRead(sensorPin2);
  78. delay(125);
  79.  
  80. }
  81.  
  82. void left(int ms) {
  83. digitalWrite(fwdPin2, HIGH);
  84. digitalWrite(revPin1, HIGH);
  85. delay(ms);
  86. digitalWrite(fwdPin2, LOW);
  87. digitalWrite(revPin1, LOW);
  88. lightLevel2 = analogRead(sensorPin2);
  89. delay(125);
  90.  
  91. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement