115 lines
2.3 KiB
C++
Executable File
115 lines
2.3 KiB
C++
Executable File
#include <CarDriver.h>
|
|
|
|
boolean goesForward = false;
|
|
|
|
NewPing sonar(trig_pin, echo_pin, maximum_distance); //sensor function
|
|
Servo servo_motor;
|
|
|
|
void setupCar() {
|
|
pinMode(RightMotorForward, OUTPUT);
|
|
pinMode(LeftMotorForward, OUTPUT);
|
|
pinMode(LeftMotorBackward, OUTPUT);
|
|
pinMode(RightMotorBackward, OUTPUT);
|
|
|
|
servo_motor.attach(ServoPin); //our servo pin
|
|
servo_motor.write(115);
|
|
delay(2000);
|
|
getPingDistance();
|
|
delay(100);
|
|
getPingDistance();
|
|
delay(100);
|
|
getPingDistance();
|
|
delay(100);
|
|
getPingDistance();
|
|
delay(100);
|
|
}
|
|
|
|
int lookRight() {
|
|
servo_motor.write(50);
|
|
delay(500);
|
|
int distance = getPingDistance();
|
|
delay(100);
|
|
servo_motor.write(115);
|
|
return distance;
|
|
}
|
|
|
|
int lookLeft() {
|
|
servo_motor.write(170);
|
|
delay(500);
|
|
int distance = getPingDistance();
|
|
delay(100);
|
|
servo_motor.write(115);
|
|
delay(100);
|
|
return distance;
|
|
}
|
|
|
|
int getPingDistance() {
|
|
delay(70);
|
|
int cm = sonar.ping_cm();
|
|
if(cm == 0) {
|
|
return maximum_distance;
|
|
}
|
|
return cm;
|
|
}
|
|
|
|
void moveStop() {
|
|
digitalWrite(RightMotorForward, LOW);
|
|
digitalWrite(LeftMotorForward, LOW);
|
|
digitalWrite(RightMotorBackward, LOW);
|
|
digitalWrite(LeftMotorBackward, LOW);
|
|
}
|
|
|
|
void moveForward() {
|
|
if(!goesForward) {
|
|
goesForward = true;
|
|
|
|
digitalWrite(LeftMotorForward, HIGH);
|
|
digitalWrite(RightMotorForward, HIGH);
|
|
|
|
digitalWrite(LeftMotorBackward, LOW);
|
|
digitalWrite(RightMotorBackward, LOW);
|
|
}
|
|
}
|
|
|
|
void moveBackward() {
|
|
goesForward = false;
|
|
|
|
digitalWrite(LeftMotorBackward, HIGH);
|
|
digitalWrite(RightMotorBackward, HIGH);
|
|
|
|
digitalWrite(LeftMotorForward, LOW);
|
|
digitalWrite(RightMotorForward, LOW);
|
|
|
|
}
|
|
|
|
void turnRight() {
|
|
digitalWrite(LeftMotorForward, HIGH);
|
|
digitalWrite(RightMotorBackward, HIGH);
|
|
|
|
digitalWrite(LeftMotorBackward, LOW);
|
|
digitalWrite(RightMotorForward, LOW);
|
|
|
|
delay(500);
|
|
|
|
digitalWrite(LeftMotorForward, HIGH);
|
|
digitalWrite(RightMotorForward, HIGH);
|
|
|
|
digitalWrite(LeftMotorBackward, LOW);
|
|
digitalWrite(RightMotorBackward, LOW);
|
|
}
|
|
|
|
void turnLeft() {
|
|
digitalWrite(LeftMotorBackward, HIGH);
|
|
digitalWrite(RightMotorForward, HIGH);
|
|
|
|
digitalWrite(LeftMotorForward, LOW);
|
|
digitalWrite(RightMotorBackward, LOW);
|
|
|
|
delay(500);
|
|
|
|
digitalWrite(LeftMotorForward, HIGH);
|
|
digitalWrite(RightMotorForward, HIGH);
|
|
|
|
digitalWrite(LeftMotorBackward, LOW);
|
|
digitalWrite(RightMotorBackward, LOW);
|
|
} |