|
|
@ -2,7 +2,7 @@ |
|
|
|
* Athors: Oulis Evnagelos, Oulis Nikolaos, Katsibras Drosos |
|
|
|
* Ultrasonic Sensor HC-SR04 and Arduino |
|
|
|
* Dual Stepper Motor Shiled 1.1 |
|
|
|
* |
|
|
|
* Subject: Autonomous Car Parking |
|
|
|
*/ |
|
|
|
|
|
|
|
/*
|
|
|
@ -28,33 +28,31 @@ |
|
|
|
* All Definitions. |
|
|
|
*/ |
|
|
|
#define ULTRASONIC_TIMEOUT 100000 |
|
|
|
#define SERVO_SPEED 6 |
|
|
|
#define SERVO_SPEED 1 |
|
|
|
#define MAX_DISTANCE 200 |
|
|
|
/*
|
|
|
|
* ------------------------------------------------------- |
|
|
|
* Definitions for Motor Driver Pins. |
|
|
|
*/ |
|
|
|
#define IN1 2 |
|
|
|
#define IN2 4 |
|
|
|
#define IN3 7 |
|
|
|
#define IN4 8 |
|
|
|
#define RIGHTSPEED 5 |
|
|
|
#define LEFTSPEED 3 |
|
|
|
|
|
|
|
#define MAX_WHEEL_SPEED 255 |
|
|
|
/*
|
|
|
|
* Define Ultrasonic Pins |
|
|
|
*/ |
|
|
|
const int trigPin = 12; |
|
|
|
const int echoPin = 11; |
|
|
|
/*
|
|
|
|
* For Motor Driver Pis. |
|
|
|
* Definitions for Ultrasonic Pins |
|
|
|
*/ |
|
|
|
const int in1 = 2; |
|
|
|
const int in2 = 4; |
|
|
|
const int in3 = 7; |
|
|
|
const int in4 = 8; |
|
|
|
const int rightSpeed = 5; |
|
|
|
const int leftSpeed = 3; |
|
|
|
|
|
|
|
#define TRIGPIN 12 |
|
|
|
#define ECHOPIN 11 |
|
|
|
/*
|
|
|
|
* For Servo Motor Pins |
|
|
|
* Definitions for Servo Motor Pins. |
|
|
|
*/ |
|
|
|
const int servoPin = 6; |
|
|
|
#define SERVOPIN 6 |
|
|
|
|
|
|
|
/*
|
|
|
|
* ------------------------------------------------------- |
|
|
|
* Define Servo Data Struct |
|
|
|
*/ |
|
|
|
Servo myServo; |
|
|
@ -62,7 +60,7 @@ Servo myServo; |
|
|
|
/*
|
|
|
|
* Define Ultrasonic Struct |
|
|
|
*/ |
|
|
|
NewPing sonar1 (trigPin, echoPin, MAX_DISTANCE); |
|
|
|
NewPing sonar1 (TRIGPIN, ECHOPIN, MAX_DISTANCE); |
|
|
|
|
|
|
|
/*
|
|
|
|
* Define all useful variables |
|
|
@ -93,33 +91,29 @@ void turnServo(int from, int deg){ |
|
|
|
* Setup Wiring Components |
|
|
|
*/ |
|
|
|
void setup() { |
|
|
|
//pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
|
|
|
|
//pinMode(echoPin, INPUT); // Sets the echoPin as an Input
|
|
|
|
//pinMode(TRIGPIN, OUTPUT); // Sets the TRIGPIN as an Output
|
|
|
|
//pinMode(ECHOPIN, INPUT); // Sets the ECHOPIN as an Input
|
|
|
|
|
|
|
|
//pinMode(response, OUTPUT); // Sets to led aoutput.
|
|
|
|
|
|
|
|
pinMode(in1, OUTPUT); //Motor Driver 1st Pin
|
|
|
|
pinMode(in2, OUTPUT); //Motor Driver 2nd Pin
|
|
|
|
pinMode(in3, OUTPUT); //Motor Driver 3rd Pin
|
|
|
|
pinMode(in4, OUTPUT); //Motor Driver 4th Pin
|
|
|
|
pinMode(IN1, OUTPUT); //Motor Driver 1st Pin
|
|
|
|
pinMode(IN2, OUTPUT); //Motor Driver 2nd Pin
|
|
|
|
pinMode(IN3, OUTPUT); //Motor Driver 3rd Pin
|
|
|
|
pinMode(IN4, OUTPUT); //Motor Driver 4th Pin
|
|
|
|
|
|
|
|
pinMode(rightSpeed, OUTPUT); |
|
|
|
pinMode(leftSpeed, OUTPUT); |
|
|
|
pinMode(RIGHTSPEED, OUTPUT); |
|
|
|
pinMode(LEFTSPEED, OUTPUT); |
|
|
|
|
|
|
|
//Initialize Servo Motor
|
|
|
|
myServo.attach(servoPin); |
|
|
|
myServo.attach(SERVOPIN); |
|
|
|
pos = 1; |
|
|
|
dir = 1; |
|
|
|
|
|
|
|
//Initialize car direction and make walk
|
|
|
|
absolute(); |
|
|
|
absolute(MAX_WHEEL_SPEED); |
|
|
|
forward(); |
|
|
|
|
|
|
|
Serial.begin(9600); // Starts the serial communication
|
|
|
|
|
|
|
|
// Clears the trigPin
|
|
|
|
analogWrite(trigPin, LOW); |
|
|
|
delayMicroseconds(2); |
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
@ -157,10 +151,11 @@ void next_step() |
|
|
|
* Distance meter via Ultrasonic |
|
|
|
*/ |
|
|
|
|
|
|
|
float getDistance(int echoPin, int trigPin){ |
|
|
|
float getDistance(){ |
|
|
|
do{ |
|
|
|
int uS1 = sonar1.ping(); |
|
|
|
distanceCm = uS1 / US_ROUNDTRIP_CM; |
|
|
|
delayMicroseconds(2); |
|
|
|
} while(distanceCm == 0); |
|
|
|
|
|
|
|
return distanceCm; |
|
|
@ -169,25 +164,25 @@ float getDistance(int echoPin, int trigPin){ |
|
|
|
/*
|
|
|
|
* Left turn walk |
|
|
|
*/ |
|
|
|
void turnLeft(){ |
|
|
|
analogWrite(leftSpeed, 255); |
|
|
|
analogWrite(rightSpeed, 0); |
|
|
|
void turnLeft(const int wheelSpeed){ |
|
|
|
analogWrite(LEFTSPEED, wheelSpeed); |
|
|
|
analogWrite(RIGHTSPEED, 0); |
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
|
* Right turn walk |
|
|
|
*/ |
|
|
|
void turnRight(){ |
|
|
|
analogWrite(leftSpeed, 0); |
|
|
|
analogWrite(rightSpeed, 255); |
|
|
|
void turnRight(const int wheelSpeed){ |
|
|
|
analogWrite(LEFTSPEED, 0); |
|
|
|
analogWrite(RIGHTSPEED, wheelSpeed); |
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
|
* Straight walk |
|
|
|
*/ |
|
|
|
void absolute(){ |
|
|
|
analogWrite(leftSpeed, 255); |
|
|
|
analogWrite(rightSpeed, 255); |
|
|
|
void absolute(const int wheelSpeed){ |
|
|
|
analogWrite(LEFTSPEED, wheelSpeed); |
|
|
|
analogWrite(RIGHTSPEED, wheelSpeed); |
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
@ -195,11 +190,11 @@ void absolute(){ |
|
|
|
*/ |
|
|
|
void backward(){ |
|
|
|
// Set Motor A backward
|
|
|
|
digitalWrite(in1, HIGH); |
|
|
|
digitalWrite(in2, LOW); |
|
|
|
digitalWrite(IN1, HIGH); |
|
|
|
digitalWrite(IN2, LOW); |
|
|
|
// Set Motor B backward
|
|
|
|
digitalWrite(in3, HIGH); |
|
|
|
digitalWrite(in4, LOW); |
|
|
|
digitalWrite(IN3, HIGH); |
|
|
|
digitalWrite(IN4, LOW); |
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
@ -207,44 +202,44 @@ void backward(){ |
|
|
|
*/ |
|
|
|
void forward(){ |
|
|
|
// Set Motor A forward
|
|
|
|
digitalWrite(in1, LOW); |
|
|
|
digitalWrite(in2, HIGH); |
|
|
|
digitalWrite(IN1, LOW); |
|
|
|
digitalWrite(IN2, HIGH); |
|
|
|
// Set Motor B forward
|
|
|
|
digitalWrite(in3, LOW); |
|
|
|
digitalWrite(in4, HIGH); |
|
|
|
digitalWrite(IN3, LOW); |
|
|
|
digitalWrite(IN4, HIGH); |
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
|
* Stop motors |
|
|
|
*/ |
|
|
|
void motor_stop(){ |
|
|
|
digitalWrite(in1, LOW); |
|
|
|
digitalWrite(in2, LOW); |
|
|
|
digitalWrite(in3, LOW); |
|
|
|
digitalWrite(in4, LOW); |
|
|
|
digitalWrite(IN1, LOW); |
|
|
|
digitalWrite(IN2, LOW); |
|
|
|
digitalWrite(IN3, LOW); |
|
|
|
digitalWrite(IN4, LOW); |
|
|
|
} |
|
|
|
|
|
|
|
void turn(){ |
|
|
|
void turn(const int wheelSpeed){ |
|
|
|
if (pos == 1){ |
|
|
|
absolute(); |
|
|
|
absolute(wheelSpeed); |
|
|
|
} else if (pos == 2){ |
|
|
|
turnLeft(); |
|
|
|
turnLeft(wheelSpeed); |
|
|
|
} else if (pos == 3){ |
|
|
|
absolute(); |
|
|
|
absolute(wheelSpeed); |
|
|
|
} else if (pos == 4){ |
|
|
|
turnRight(); |
|
|
|
turnRight(wheelSpeed); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
void moveCar(){ |
|
|
|
if (pos == 3){ |
|
|
|
if (dir == 1){ |
|
|
|
turnRight(); |
|
|
|
turnRight(MAX_WHEEL_SPEED); |
|
|
|
backward(); |
|
|
|
|
|
|
|
dir = 2; |
|
|
|
} else{ |
|
|
|
turnLeft(); |
|
|
|
turnLeft(MAX_WHEEL_SPEED); |
|
|
|
backward(); |
|
|
|
|
|
|
|
dir = 1; |
|
|
@ -258,20 +253,27 @@ void moveCar(){ |
|
|
|
|
|
|
|
void loop() { |
|
|
|
// Calculating the distance
|
|
|
|
distance= getDistance(echoPin, trigPin); |
|
|
|
distance= getDistance(); |
|
|
|
|
|
|
|
// Prints the distance on the Serial Monitor
|
|
|
|
Serial.print("Distance: "); |
|
|
|
Serial.println(distance); |
|
|
|
//Serial.print("Distance: ");
|
|
|
|
//Serial.println(distance);
|
|
|
|
|
|
|
|
if (distance <15) { |
|
|
|
// Change direction to motors
|
|
|
|
turn(225); |
|
|
|
} else if (distance <13){ |
|
|
|
turn(200); |
|
|
|
} else { |
|
|
|
// Change direction to motors
|
|
|
|
turn(MAX_WHEEL_SPEED); |
|
|
|
} |
|
|
|
if (distance < 6){ |
|
|
|
motor_stop(); |
|
|
|
// Set Ultrasonic to next step
|
|
|
|
next_step(); |
|
|
|
// Change direction to motors
|
|
|
|
turn(); |
|
|
|
// Move car
|
|
|
|
moveCar(); |
|
|
|
delay(1000); |
|
|
|
} |
|
|
|
delay(100); |
|
|
|
} |
|
|
|