Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <Servo.h>
- const int pingPin = 7;
- const int minsafedistance = 5;
- int rightDistance, leftDistance;
- int leftMotor2 = 3;
- int leftMotor7 = 4;
- int rightMotor10 = 5;
- int rightMotor15 = 6;
- long duration, distance;
- Servo servo;
- // SETUP
- void setup()
- {
- servo.attach(8);
- pinMode(leftMotor2, OUTPUT);
- pinMode(leftMotor7, OUTPUT);
- pinMode(rightMotor10, OUTPUT);
- pinMode(rightMotor15, OUTPUT);
- }
- // LOOP
- void loop()
- {
- lookForward();
- int fwdDistance = ping();
- if(fwdDistance>minsafedistance)
- {
- GoForward();
- }
- else
- {
- // this section is incomplete
- lookRight();
- rightDistance = ping();
- delay(100);
- lookLeft();
- leftDistance = ping();
- delay(100);
- }
- }
- // FUNCTIONS
- 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);
- distance = (duration/2) /29.3866996;
- return distance;
- }
- //---------------------------------------------------------
- void GoForward()
- {
- digitalWrite(leftMotor7, LOW);
- digitalWrite(leftMotor2, HIGH);
- digitalWrite(rightMotor15, LOW);
- digitalWrite(rightMotor10, HIGH);
- }
- //---------------------------------------------------------
- void GoBackward()
- {
- digitalWrite(leftMotor2, LOW);
- digitalWrite(leftMotor7, HIGH);
- digitalWrite(rightMotor10, LOW);
- digitalWrite(rightMotor15, HIGH);
- }
- //---------------------------------------------------------
- void GoLeft()
- {
- digitalWrite(leftMotor2, LOW);
- digitalWrite(leftMotor7, HIGH);
- digitalWrite(rightMotor15, LOW);
- digitalWrite(rightMotor10, HIGH);
- }
- //---------------------------------------------------------
- void GoRight()
- {
- digitalWrite(leftMotor7, LOW);
- digitalWrite(leftMotor2, HIGH);
- digitalWrite(rightMotor10, LOW);
- digitalWrite(rightMotor15, HIGH);
- }
- //---------------------------------------------------------
- void Stop()
- {
- digitalWrite(leftMotor7, LOW);
- digitalWrite(leftMotor2, LOW);
- digitalWrite(rightMotor10, LOW);
- digitalWrite(rightMotor15, LOW);
- }
- //---------------------------------------------------------
- void lookRight()
- {
- servo.write(180);
- delay(2000);
- }
- //---------------------------------------------------------
- void lookLeft()
- {
- servo.write(0);
- delay(2000);
- }
- //---------------------------------------------------------
- void lookForward()
- {
- servo.write(90);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement