Guest User

Untitled

a guest
Jul 7th, 2012
1,018
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. //-- Killer Robot v0.3
  2.  
  3. //--
  4.  
  5. //-- ULTRASONIC
  6. #include <NewPing.h>
  7.  
  8. #define  TRIGGER_PIN  47
  9. #define  ECHO_PIN_RIGHT  53
  10. #define  ECHO_PIN_CENTER  51
  11. #define  ECHO_PIN_LEFT 49
  12.  
  13. #define MAX_DISTANCE 300
  14.  
  15. NewPing sonarRight(TRIGGER_PIN, ECHO_PIN_RIGHT, MAX_DISTANCE);
  16. NewPing sonarCenter(TRIGGER_PIN, ECHO_PIN_CENTER, MAX_DISTANCE);
  17. NewPing sonarLeft(TRIGGER_PIN, ECHO_PIN_LEFT, MAX_DISTANCE);
  18.  
  19. int rangeRight = 0;
  20. int rangeCenter = 0;
  21. int rangeLeft = 0;
  22.  
  23. int rangeLedRight = 52;
  24. int rangeLedCenter = 50;
  25. int rangeLedLeft = 48;
  26.  
  27. int creepLed = 46;
  28.  
  29. int minDistanceSide = 35;
  30. int prefDistanceSide = 50;
  31. int minDistanceFront = 40;
  32. int prefDistanceFront = 100;
  33.  
  34. long previousMillis = 0;
  35. long sensorInterval = 50;
  36.  
  37. int sensorCycle = 0;
  38.  
  39. //-- MOTOR A --
  40. int ENA=5; //Connected to Arudino Pin 5(pwm)
  41. int IN1=2; //Connected to Arudino Pin 2
  42. int IN2=3; //Connected to Arudino Pin 3
  43.  
  44. //-- MOTOR B --
  45. int IN3=4; //Connected to Arudino Pin 4
  46. int IN4=7; //Connected to Arudino Pin 7
  47. int ENB=6; //Connected to Arudino Pin 6(pwm)
  48.  
  49. int creepSpeed = 140;
  50.  
  51. void setup()
  52. {
  53.   //Serial.begin(9600);
  54.  
  55.   pinMode(ENA,OUTPUT);
  56.   pinMode(ENB,OUTPUT);
  57.   pinMode(IN1,OUTPUT);
  58.   pinMode(IN2,OUTPUT);
  59.   pinMode(IN3,OUTPUT);
  60.   pinMode(IN4,OUTPUT);
  61.  
  62.   stopMotors();
  63.  
  64.   pinMode(rangeLedRight, OUTPUT);
  65.   pinMode(rangeLedCenter, OUTPUT);
  66.   pinMode(rangeLedLeft, OUTPUT);
  67.   pinMode(creepLed, OUTPUT);
  68.  
  69.   rangeSweep(); // Do a full distance sweep without moving
  70. }
  71.  
  72. void loop()
  73. {
  74.   unsigned long currentMillis = millis();
  75.  
  76.   if (currentMillis - previousMillis > sensorInterval)
  77.   {
  78.     previousMillis = currentMillis;
  79.     findRange();
  80.   }
  81.   if (minDistanceSide <= rangeRight && minDistanceSide <= rangeLeft && minDistanceFront <= rangeCenter)
  82.   {
  83.     driveForward();
  84.   }
  85.   else if (minDistanceSide >= rangeRight && prefDistanceSide <= rangeLeft)
  86.   {
  87.     turnLeft();
  88.   }
  89.   else if (prefDistanceSide <= rangeRight && minDistanceSide >= rangeLeft)
  90.   {
  91.     turnRight();
  92.   }
  93.   else
  94.   {
  95.     turnAround();
  96.   }
  97. }
  98.  
  99. void findRange()
  100. {
  101.   if (sensorCycle == 0) // can't poll all 3 sensors at the same time, interferance issues, thus spaced 50ms apart
  102.   {
  103.     rangeLeft = sonarLeft.ping_cm();
  104.     sensorCycle++;
  105.   }
  106.   else if (sensorCycle == 1)
  107.   {
  108.     rangeCenter = sonarCenter.ping_cm();
  109.     sensorCycle++;
  110.   }
  111.   else if (sensorCycle >= 2) // should be impossible for it to reach 3, but hey
  112.   {
  113.     rangeRight = sonarRight.ping_cm();
  114.     sensorCycle = 0;
  115.   }
  116.  
  117.   // "0" is no signal, means 3m+, lowest distance it can measure is 2cm
  118.   if (rangeLeft == 0)
  119.     rangeLeft = 300;
  120.   if (rangeRight == 0)
  121.     rangeRight = 300;
  122.   if (rangeCenter == 0)
  123.     rangeCenter = 300;  
  124.  
  125.   if (minDistanceSide >= rangeLeft)  
  126.     digitalWrite(rangeLedLeft, HIGH);
  127.   else
  128.     digitalWrite(rangeLedLeft, LOW);
  129.  
  130.   if (minDistanceFront >= rangeCenter)
  131.     digitalWrite(rangeLedCenter, HIGH);
  132.   else
  133.     digitalWrite(rangeLedCenter, LOW);
  134.  
  135.   if (minDistanceSide >= rangeRight)
  136.     digitalWrite(rangeLedRight, HIGH);
  137.   else
  138.     digitalWrite(rangeLedRight, LOW);
  139.  
  140.   //Serial.print("L");
  141.   //Serial.print(rangeLeft);
  142.   //Serial.print(" C");
  143.   //Serial.print(rangeCenter);
  144.   //Serial.print(" R");
  145.   //Serial.println(rangeRight);
  146. }
  147.  
  148. void rangeSweep() // Do a full distance sweep without moving
  149. {
  150.   delay(50);
  151.   findRange();
  152.   delay(50);
  153.   findRange();
  154.   delay(50);
  155.   findRange();
  156.   delay(50);
  157. }
  158.  
  159. void driveForward()
  160. {
  161.  
  162.   motorsForward();
  163.  
  164.   int speedPot = analogRead(0) / 4;
  165.  
  166.   if (200 <= speedPot)
  167.     speedPot = 255;
  168.   else if (50 >= speedPot)
  169.     speedPot = 0;
  170.  
  171.   // Deadzones to overcome math errors, motors don't move robot at too low engine speed anyway
  172.  
  173.   if (prefDistanceFront <= rangeCenter && prefDistanceSide <= rangeLeft && prefDistanceSide <= rangeRight && creepSpeed >= speedPot)
  174.   {
  175.     analogWrite(ENA, speedPot);
  176.     analogWrite(ENB, speedPot);
  177.     digitalWrite(creepLed, LOW);
  178.   }
  179.   else // Reduce speed if something is detected nearby
  180.   {
  181.     analogWrite(ENA, creepSpeed);
  182.     analogWrite(ENB, creepSpeed);
  183.     digitalWrite(creepLed, HIGH);
  184.   }
  185. }
  186.  
  187. void turnLeft()
  188. {
  189.   stopMotors();
  190.   delay(500);
  191.   motorsTurnLeft();
  192.   runMotors();
  193.   delay(500);
  194.   stopMotors();
  195.   delay(500);
  196.   // Bought a compass module, will replace delays with actual compass comparison when it arrives
  197.   rangeSweep();
  198. }
  199.  
  200. void turnRight()
  201. {
  202.   stopMotors();
  203.   delay(500);
  204.   motorsTurnRight();
  205.   runMotors();
  206.   delay(500);
  207.   stopMotors();
  208.   delay(500);
  209.   // Bought a compass module, will replace delays with actual compass comparison when it arrives
  210.   rangeSweep();
  211. }
  212.  
  213. void turnAround()
  214. {
  215.   stopMotors();
  216.   delay(500);
  217.  
  218.   if (rangeLeft >= rangeRight)
  219.     motorsTurnLeft();
  220.   else
  221.     motorsTurnRight();
  222.    
  223.   runMotors();
  224.   delay(1000);
  225.   stopMotors();
  226.   delay(500);
  227.   // Bought a compass module, will replace delays with actual compass comparison when it arrives
  228.   rangeSweep();
  229. }
  230.  
  231. void stopMotors()
  232. {
  233.   digitalWrite(ENA, LOW);
  234.   digitalWrite(ENB, LOW);
  235. }
  236.  
  237. void runMotors()
  238. {
  239.   digitalWrite(ENA, HIGH);
  240.   digitalWrite(ENB, HIGH);
  241. }
  242.  
  243. void motorsForward()
  244. {
  245.   digitalWrite(IN1,HIGH);
  246.   digitalWrite(IN2,LOW);
  247.   digitalWrite(IN3,LOW);
  248.   digitalWrite(IN4,HIGH);
  249. }
  250.  
  251. void motorsBackward()
  252. {
  253.   digitalWrite(IN1,LOW);
  254.   digitalWrite(IN2,HIGH);
  255.   digitalWrite(IN3,HIGH);
  256.   digitalWrite(IN4,LOW);
  257. }
  258.  
  259. void motorsTurnLeft()
  260. {
  261.   digitalWrite(IN1,HIGH);
  262.   digitalWrite(IN2,LOW);
  263.   digitalWrite(IN3,HIGH);
  264.   digitalWrite(IN4,LOW);
  265. }
  266.  
  267. void motorsTurnRight()
  268. {
  269.   digitalWrite(IN1,LOW);
  270.   digitalWrite(IN2,HIGH);
  271.   digitalWrite(IN3,LOW);
  272.   digitalWrite(IN4,HIGH);
  273. }
RAW Paste Data