Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include<Servo.h>
- const int pingPin = 7;
- const int dangerThresh = 12;
- const int PAN_MOTOR_PIN = 5;
- const int DIR_DRIVE = 12; //DIRA
- const int DIR_TURN = 13; //DIRB
- const int BRAKE_DRIVE = 9; //BRAKEA
- const int BRAKE_TURN = 8; //BRAKEB
- const int PWM_DRIVE = 3; //PWMA
- const int PWM_TURN = 11; //PWMB
- int leftDistance, rightDistance;
- Servo panMotor;
- long duration;
- void setup() {
- pinMode(DIR_DRIVE, OUTPUT);
- pinMode(DIR_TURN, OUTPUT);
- pinMode(BRAKE_DRIVE, OUTPUT);
- pinMode(BRAKE_TURN, OUTPUT);
- pinMode(PWM_DRIVE, OUTPUT);
- pinMode(PWM_TURN, OUPTUT);
- digitalWrite(BRAKE_DRIVE, LOW);
- digitalWrite(BRAKE_TURN, HIGH);
- digitalWrite(DIR_DRIVE, HIGH);
- analogWrite(PWM_DRIVE, 175);
- //panMotor.attach(11); // are you sure you want to use this pin for the servo?
- // why not 5 or 6? 11 is meant for the PWMB
- panMotor.attach(PAN_MOTOR_PIN);
- panMotor.write(90);
- }
- void loop() {
- int distanceFwd = ping();
- if(distanceFwd > dangerThresh) {
- digitalWrite(DIR_DRIVE, HIGH);
- digitalWrite(BRAKE_DRIVE, LOW);
- analogWrite(PWM_DRIVE, 175);
- } else {
- panMotor.write(0);
- delay(500);
- rightDistance = ping();
- delay(500);
- panMotor.write(160);
- delay(700);
- leftDistance = ping();
- delay(500);
- panMotor.write(90);
- delay(100);
- compareDistance();
- }
- }
- void compareDistance() {
- if(leftDistance > rightDistance) {
- digitalWrite(BRAKE_TURN, LOW);
- digitalWrite(DIR_TURN, HIGH);
- analogWrite(PWM_TURN, 255);
- delay(500);
- } else if(rightDistance > leftDistance) {
- digitalWrite(BRAKE_TURN, LOW);
- digitalWrite(DIR_TURN, LOW);
- analogWrite(PWM_TURN, 255);
- delay(500);
- } else {
- digitalWrite(DIR_DRIVE, HIGH);
- digitalWrite(BRAKE_DRIVE, LOW);
- analogWrite(PWM_DRIVE, 175);
- }
- }
- long ping() {
- pinMode(pingPin, OUTPUT);
- digitalWrite(pingPin, LOW);
- delayMicroseconds(2);
- digitalWrite(pingPin, HIGH);
- delayMicroseconds(5);
- digitalWrite(pingPin, LOW);
- pinMode(pingPin, INPUT);
- duration = pulseIn(pingPin, HIGH);
- return duration / 29 / 2;
- }
Advertisement
Add Comment
Please, Sign In to add comment