Smart and Autonomous parking.
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 

277 lines
4.4 KiB

/*
* Athors: Oulis Evnagelos, Oulis Nikolaos, Katsibras Drosos
* Ultrasonic Sensor HC-SR04 and Arduino
* Dual Stepper Motor Shiled 1.1
*
*/
/*
* Include all libraries.
*
*/
/*
* Include the Arduino Servo Library
*/
#include <Servo.h>
/*
* Include the Ping Ulptrasonic Library
*/
#include <NewPing.h>
/*
* -------------------------------------------------------
*/
/*
* All Definitions.
*/
#define ULTRASONIC_TIMEOUT 100000
#define SERVO_SPEED 6
#define MAX_DISTANCE 200
/*
* -------------------------------------------------------
*/
/*
* Define Ultrasonic Pins
*/
const int trigPin = 12;
const int echoPin = 11;
/*
* For Motor Driver Pis.
*/
const int in1 = 2;
const int in2 = 4;
const int in3 = 7;
const int in4 = 8;
const int rightSpeed = 5;
const int leftSpeed = 3;
/*
* For Servo Motor Pins
*/
const int servoPin = 6;
/*
* Define Servo Data Struct
*/
Servo myServo;
/*
* Define Ultrasonic Struct
*/
NewPing sonar1 (trigPin, echoPin, MAX_DISTANCE);
/*
* Define all useful variables
*/
long duration;
float distance, distanceCm;
int pos, dir;
/*
* Turn Servo using custom Speed
*/
void turnServo(int from, int deg){
int d;
if (from > deg){
for(d=from; d>=deg; d--){
myServo.write(d);
delay(SERVO_SPEED);
}
} else{
for(d=from; d<=deg; d++){
myServo.write(d);
delay(SERVO_SPEED);
}
}
}
/*
* Setup Wiring Components
*/
void setup() {
//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(rightSpeed, OUTPUT);
pinMode(leftSpeed, OUTPUT);
//Initialize Servo Motor
myServo.attach(servoPin);
pos = 1;
dir = 1;
//Initialize car direction and make walk
absolute();
forward();
Serial.begin(9600); // Starts the serial communication
// Clears the trigPin
analogWrite(trigPin, LOW);
delayMicroseconds(2);
}
/*
* Stepper Function for Servo.
* 1st Step: straight
* 2nd Step: left
* 3rd Step: straight
* 4th Step: right
*/
void next_step()
{
if(pos == 1)
{ //Servo move to right
turnServo(90, 30);
pos = 2;
}
else if(pos ==2)
{ //Servo move to center
turnServo(30, 90);
pos = 3;
}
else if(pos == 3)
{ //Servo move to left
turnServo(90, 150);
pos = 4;
}
else if(pos == 4)
{ //Servo move to center
turnServo(150, 90);
pos = 1;
}
}
/*
* Distance meter via Ultrasonic
*/
float getDistance(int echoPin, int trigPin){
do{
int uS1 = sonar1.ping();
distanceCm = uS1 / US_ROUNDTRIP_CM;
} while(distanceCm == 0);
return distanceCm;
}
/*
* Left turn walk
*/
void turnLeft(){
analogWrite(leftSpeed, 255);
analogWrite(rightSpeed, 0);
}
/*
* Right turn walk
*/
void turnRight(){
analogWrite(leftSpeed, 0);
analogWrite(rightSpeed, 255);
}
/*
* Straight walk
*/
void absolute(){
analogWrite(leftSpeed, 255);
analogWrite(rightSpeed, 255);
}
/*
* Backward walk
*/
void backward(){
// Set Motor A backward
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
// Set Motor B backward
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
}
/*
* Forward walk
*/
void forward(){
// Set Motor A forward
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
// Set Motor B forward
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
}
/*
* Stop motors
*/
void motor_stop(){
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
}
void turn(){
if (pos == 1){
absolute();
} else if (pos == 2){
turnLeft();
} else if (pos == 3){
absolute();
} else if (pos == 4){
turnRight();
}
}
void moveCar(){
if (pos == 3){
if (dir == 1){
turnRight();
backward();
dir = 2;
} else{
turnLeft();
backward();
dir = 1;
}
delay(1500);
forward();
} else{
forward();
}
}
void loop() {
// Calculating the distance
distance= getDistance(echoPin, trigPin);
// Prints the distance on the Serial Monitor
Serial.print("Distance: ");
Serial.println(distance);
if (distance < 6){
motor_stop();
// Set Ultrasonic to next step
next_step();
// Change direction to motors
turn();
// Move car
moveCar();
delay(1000);
}
delay(100);
}