Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- // Libraries
- #include <Servo.h>
- //Fixed pins
- // Servo
- const int servoPin = 3;
- // Ultrasonic Sensor
- const int trigPin = 12;
- const int echoPin = 11;
- // Motors
- const int enA = 10;
- const int in1 = 9;
- const int in2 = 8;
- const int in3 = 7;
- const int in4 = 6;
- const int enB = 5;
- //other variables
- Servo myServo;
- long duration;
- int distance;
- int zero = 0;
- int right = 50;
- int middle = 100;
- int left = 150;
- void setup() {
- // put your setup code here, to run once:
- pinMode(enA, OUTPUT);
- pinMode(enB, OUTPUT);
- pinMode(in1, OUTPUT);
- pinMode(in2, OUTPUT);
- pinMode(in3, OUTPUT);
- pinMode(in4, OUTPUT);
- myServo.attach(servoPin);
- pinMode(trigPin, OUTPUT);
- pinMode(echoPin, INPUT);
- Serial.begin(9600);
- }
- void loop() {
- // make it run only once (there's probably a better way ^^)
- if(zero == 0){
- testDriving();
- zero++;
- }
- }
- void testDriving()
- {
- digitalWrite(in1, HIGH);
- digitalWrite(in2, LOW);
- analogWrite(enA, 200);
- digitalWrite(in3, HIGH);
- digitalWrite(in4, LOW);
- analogWrite(enB, 200);
- delay(5000);
- Serial.println(getDistance(right));
- delay(2000);
- Serial.println(getDistance(middle));
- delay(2000);
- Serial.println(getDistance(left));
- delay(2000);
- }
- int getDistance(int rotation){
- myServo.write(rotation);
- delay(500);
- // Clears the trigPin
- digitalWrite(trigPin, LOW);
- delayMicroseconds(2);
- // Sets the trigPin on HIGH state for 10 micro seconds
- digitalWrite(trigPin, HIGH);
- delayMicroseconds(10);
- digitalWrite(trigPin, LOW);
- // Reads the echoPin, returns the sound wave travel time in microseconds
- duration = pulseIn(echoPin, HIGH);
- // Calculating the distance
- distance= duration*0.034/2;
- return distance;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement