birdmun

updated code

Feb 26th, 2014
182
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 2.07 KB | None | 0 0
  1. #include<Servo.h>
  2. const int pingPin = 7;
  3. const int dangerThresh = 12;
  4. const int PAN_MOTOR_PIN = 5;
  5. const int DIR_DRIVE = 12; //DIRA
  6. const int DIR_TURN = 13; //DIRB
  7. const int BRAKE_DRIVE = 9; //BRAKEA
  8. const int BRAKE_TURN = 8; //BRAKEB
  9. const int PWM_DRIVE = 3; //PWMA
  10. const int PWM_TURN = 11; //PWMB
  11. int leftDistance, rightDistance;
  12. Servo panMotor;
  13. long duration;
  14.  
  15. void setup() {
  16.   pinMode(DIR_DRIVE, OUTPUT);
  17.   pinMode(DIR_TURN, OUTPUT);
  18.   pinMode(BRAKE_DRIVE, OUTPUT);
  19.   pinMode(BRAKE_TURN, OUTPUT);
  20.   pinMode(PWM_DRIVE, OUTPUT);
  21.   pinMode(PWM_TURN, OUPTUT);
  22.   digitalWrite(BRAKE_DRIVE, LOW);
  23.   digitalWrite(BRAKE_TURN, HIGH);
  24.   digitalWrite(DIR_DRIVE, HIGH);
  25.   analogWrite(PWM_DRIVE, 175);
  26.   //panMotor.attach(11); // are you sure you want to use this pin for the servo?
  27.   // why not 5 or 6? 11 is meant for the PWMB
  28.   panMotor.attach(PAN_MOTOR_PIN);
  29.   panMotor.write(90);
  30. }
  31.  
  32. void loop() {
  33.   int distanceFwd = ping();
  34.   if(distanceFwd > dangerThresh) {
  35.     digitalWrite(DIR_DRIVE, HIGH);
  36.     digitalWrite(BRAKE_DRIVE, LOW);
  37.     analogWrite(PWM_DRIVE, 175);
  38.   } else {
  39.     panMotor.write(0);
  40.     delay(500);
  41.     rightDistance = ping();
  42.     delay(500);
  43.     panMotor.write(160);
  44.     delay(700);
  45.     leftDistance = ping();
  46.     delay(500);
  47.     panMotor.write(90);
  48.     delay(100);
  49.     compareDistance();
  50.   }
  51. }
  52.  
  53. void compareDistance() {
  54.   if(leftDistance > rightDistance) {
  55.     digitalWrite(BRAKE_TURN, LOW);
  56.     digitalWrite(DIR_TURN, HIGH);
  57.     analogWrite(PWM_TURN, 255);
  58.     delay(500);
  59.   } else if(rightDistance > leftDistance) {
  60.     digitalWrite(BRAKE_TURN, LOW);
  61.     digitalWrite(DIR_TURN, LOW);
  62.     analogWrite(PWM_TURN, 255);
  63.     delay(500);
  64.   } else {
  65.     digitalWrite(DIR_DRIVE, HIGH);
  66.     digitalWrite(BRAKE_DRIVE, LOW);
  67.     analogWrite(PWM_DRIVE, 175);
  68.   }
  69. }
  70.  
  71. long ping() {
  72.   pinMode(pingPin, OUTPUT);
  73.   digitalWrite(pingPin, LOW);
  74.   delayMicroseconds(2);
  75.   digitalWrite(pingPin, HIGH);
  76.   delayMicroseconds(5);
  77.   digitalWrite(pingPin, LOW);
  78.   pinMode(pingPin, INPUT);
  79.   duration = pulseIn(pingPin, HIGH);
  80.   return duration / 29 / 2;
  81.  }
Advertisement
Add Comment
Please, Sign In to add comment