Guest User

Minibot

a guest
Jan 8th, 2014
70
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <AFMotor.h>
  2. #include <Servo.h>
  3. #include <NewPing.h>
  4.  
  5. #define TRIG_PIN A4 // Pin A4 on the Motor Drive Shield soldered to the ultrasonic sensor
  6. #define ECHO_PIN A5 // Pin A5 on the Motor Drive Shield soldered to the ultrasonic sensor
  7. #define MAX_DISTANCE 200 // sets maximum useable sensor measuring distance to 200cm
  8. #define MAX_SPEED 180 // sets speed of DC traction motors to 180/256 or about 70% of full speed - to get power drain down.
  9. #define MAX_SPEED_OFFSET 20 // this sets offset to allow for differences between the two DC traction motors
  10. #define COLL_DIST 10 // sets distance at which robot stops and reverses to 10cm
  11. #define TURN_DIST COLL_DIST+10 // sets distance at which robot veers away from object (not reverse) to 20cm (10+10)
  12. NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); // sets up sensor library to use the correct pins to measure distance.
  13.  
  14. AF_DCMotor motor1(1, MOTOR12_1KHZ); // create motor #1 using M1 output on Motor Drive Shield, set to 1kHz PWM frequency
  15. AF_DCMotor motor2(4, MOTOR12_1KHZ); // create motor #2, using M2 output, set to 1kHz PWM frequency
  16. Servo myservo; // create servo object to control a servo
  17.  
  18. int pos = 0; // this sets up variables for use in the sketch (code)
  19. int maxDist = 0;
  20. int maxAngle = 0;
  21. int maxRight = 0;
  22. int maxLeft = 0;
  23. int maxFront = 0;
  24. int course = 0;
  25. int curDist = 0;
  26. String motorSet = "";
  27. int speedSet = 0;
  28.  
  29. //-------------------------------------------- SETUP LOOP ----------------------------------------------------------------------------
  30. void setup() {
  31. myservo.attach(9); // attaches the servo on pin 9 (SERVO_2 on the Motor Drive Shield to the servo object
  32. myservo.write(90); // tells the servo to position at 90-degrees ie. facing forward.
  33. delay(2000); // delay for two seconds
  34. checkPath(); // run the CheckPath routine to find the best path to begin travel
  35. motorSet = "FORWARD"; // set the director indicator variable to FORWARD
  36. myservo.write(90); // make sure servo is still facing forward
  37. moveForward(); // run function to make robot move forward
  38. }
  39. //------------------------------------------------------------------------------------------------------------------------------------
  40.  
  41. //---------------------------------------------MAIN LOOP ------------------------------------------------------------------------------
  42. void loop() {
  43. checkForward(); // check that if the robot is supposed to be moving forward, that the drive motors are set to move forward - this is needed to overcome some issues with only using 4 AA NiMH batteries
  44. checkPath(); // set ultrasonic sensor to scan for any possible obstacles
  45. }
  46. //-------------------------------------------------------------------------------------------------------------------------------------
  47. void checkPath() {
  48. int curLeft = 0;
  49. int curFront = 0;
  50. int curRight = 0;
  51. int curDist = 0;
  52. myservo.write(144); // set servo to face left 54-degrees from forward
  53. delay(120); // wait 120milliseconds for servo to reach position
  54. for(pos = 144; pos >= 36; pos-=18) // loop to sweep the servo (& sensor) from 144-degrees left to 36-degrees right at 18-degree intervals.
  55. {
  56. myservo.write(pos); // tell servo to go to position in variable 'pos'
  57. delay(90); // wait 90ms for servo to get to position
  58. checkForward(); // check the robot is still moving forward
  59. curDist = readPing(); // get the current distance to any object in front of sensor
  60. if (curDist < COLL_DIST) { // if the current distance to object is less than the collision distance
  61. checkCourse(); // run the checkCourse function
  62. break; // jump out of this loop
  63. }
  64. if (curDist < TURN_DIST) { // if current distance is less than the turn distance
  65. changePath(); // run the changePath function
  66. }
  67. if (curDist > curDist) {maxAngle = pos;}
  68. if (pos > 90 && curDist > curLeft) { curLeft = curDist;}
  69. if (pos == 90 && curDist > curFront) {curFront = curDist;}
  70. if (pos < 90 && curDist > curRight) {curRight = curDist;}
  71. }
  72. maxLeft = curLeft;
  73. maxRight = curRight;
  74. maxFront = curFront;
  75. }
  76. //-------------------------------------------------------------------------------------------------------------------------------------
  77. void setCourse() { // set direction for travel based on a very basic distance map, simply which direction has the greatest distance to and object - turning right or left?
  78. if (maxAngle < 90) {turnRight();}
  79. if (maxAngle > 90) {turnLeft();}
  80. maxLeft = 0;
  81. maxRight = 0;
  82. maxFront = 0;
  83. }
  84. //-------------------------------------------------------------------------------------------------------------------------------------
  85. void checkCourse() { // we're about to hit something so move backwards, stop, find where the empty path is.
  86. moveBackward();
  87. delay(500);
  88. moveStop();
  89. setCourse();
  90. }
  91. //-------------------------------------------------------------------------------------------------------------------------------------
  92. void changePath() {
  93. if (pos < 90) {veerLeft();} // if current pos of sensor is less than 90-degrees, it means the object is on the right hand side so veer left
  94. if (pos > 90) {veerRight();} // if current pos of sensor is greater than 90-degrees, it means the object is on the left hand side so veer right
  95. }
  96. //-------------------------------------------------------------------------------------------------------------------------------------
  97.  
  98. int readPing() { // read the ultrasonic sensor distance
  99. delay(70);
  100. unsigned int uS = sonar.ping();
  101. int cm = uS/US_ROUNDTRIP_CM;
  102. return cm;
  103. }
  104. //-------------------------------------------------------------------------------------------------------------------------------------
  105. void checkForward() { if (motorSet=="FORWARD") {motor1.run(FORWARD); motor2.run(FORWARD); } } // make sure motors are going forward
  106. //-------------------------------------------------------------------------------------------------------------------------------------
  107. void checkBackward() { if (motorSet=="BACKWARD") {motor1.run(BACKWARD); motor2.run(BACKWARD); } } // make sure motors are going backward
  108. //-------------------------------------------------------------------------------------------------------------------------------------
  109.  
  110. // In some cases, the Motor Drive Shield may just stop if the supply voltage is too low (due to using only four NiMH AA cells).
  111. // The above functions simply remind the Shield that if it's supposed to go forward, then make sure it is going forward and vice versa.
  112.  
  113. //-------------------------------------------------------------------------------------------------------------------------------------
  114. void moveStop() {motor1.run(RELEASE); motor2.run(RELEASE);} // stop the motors.
  115. //-------------------------------------------------------------------------------------------------------------------------------------
  116. void moveForward() {
  117. motorSet = "FORWARD";
  118. motor1.run(FORWARD); // turn it on going forward
  119. motor2.run(FORWARD); // turn it on going forward
  120. for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
  121. {
  122. motor1.setSpeed(speedSet);
  123. motor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
  124. delay(5);
  125. }
  126. }
  127. //-------------------------------------------------------------------------------------------------------------------------------------
  128. void moveBackward() {
  129. motorSet = "BACKWARD";
  130. motor1.run(BACKWARD); // turn it on going forward
  131. motor2.run(BACKWARD); // turn it on going forward
  132. for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
  133. {
  134. motor1.setSpeed(speedSet);
  135. motor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
  136. delay(5);
  137. }
  138. }
  139. //-------------------------------------------------------------------------------------------------------------------------------------
  140. void turnRight() {
  141. motorSet = "RIGHT";
  142. motor1.run(FORWARD); // turn motor 1 forward
  143. motor2.run(BACKWARD); // turn motor 2 backward
  144. delay(400); // run motors this way for 400ms
  145. motorSet = "FORWARD";
  146. motor1.run(FORWARD); // set both motors back to forward
  147. motor2.run(FORWARD);
  148. }
  149. //-------------------------------------------------------------------------------------------------------------------------------------
  150. void turnLeft() {
  151. motorSet = "LEFT";
  152. motor1.run(BACKWARD); // turn motor 1 backward
  153. motor2.run(FORWARD); // turn motor 2 forward
  154. delay(400); // run motors this way for 400ms
  155. motorSet = "FORWARD";
  156. motor1.run(FORWARD); // turn it on going forward
  157. motor2.run(FORWARD); // turn it on going forward
  158. }
  159. //-------------------------------------------------------------------------------------------------------------------------------------
  160. void veerRight() {motor2.run(BACKWARD); delay(400); motor2.run(FORWARD);} // veering right? set right motor backwards for 400ms
  161. //-------------------------------------------------------------------------------------------------------------------------------------
  162. void veerLeft() {motor1.run(BACKWARD); delay(400); motor1.run(FORWARD);} // veering left? set left motor backwards for 400ms
  163. //-------------------------------------------------------------------------------------------------------------------------------------
RAW Paste Data