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.

345 lines
6.0 KiB

5 years ago
/*
* Athors: Oulis Evnagelos, Oulis Nikolaos, Katsibras Drosos
* Ultrasonic Sensor HC-SR04 and Arduino
* Dual Stepper Motor Shiled 1.1
5 years ago
* Subject: Autonomous Car Parking
5 years ago
*/
/*
* Include all libraries.
5 years ago
*
5 years ago
*/
5 years ago
/*
* Include the Arduino Servo Library
*/
#include <Servo.h>
/*
* Include the Ping Ulptrasonic Library
*/
#include <NewPing.h>
5 years ago
/*
* -------------------------------------------------------
*/
/*
* All Definitions.
*/
5 years ago
#define ULTRASONIC_TIMEOUT 100000
5 years ago
#define SERVO_SPEED 1
5 years ago
#define MAX_DISTANCE 200
5 years ago
/*
5 years ago
* Definitions for Motor Driver Pins.
5 years ago
*/
5 years ago
#define IN1 2
#define IN2 4
#define IN3 7
#define IN4 8
#define RIGHTSPEED 5
#define LEFTSPEED 3
5 years ago
5 years ago
#define MAX_WHEEL_SPEED 255
5 years ago
/*
5 years ago
* Definitions for Ultrasonic Pins
5 years ago
*/
5 years ago
#define TRIGPIN 12
#define ECHOPIN 11
5 years ago
/*
5 years ago
* Definitions for Servo Motor Pins.
5 years ago
*/
5 years ago
#define SERVOPIN 6
5 years ago
5 years ago
/*Definition for turn time*/
5 years ago
#define TURN_TIME 2000
#define FORWARD_STEP 500
5 years ago
5 years ago
/*
5 years ago
* -------------------------------------------------------
5 years ago
* Define Servo Data Struct
*/
Servo myServo;
5 years ago
5 years ago
/*
* Define Ultrasonic Struct
*/
5 years ago
NewPing sonar1 (TRIGPIN, ECHOPIN, MAX_DISTANCE);
5 years ago
5 years ago
/*
* Define all useful variables
*/
5 years ago
long duration;
5 years ago
float distance, distanceCm;
int pos, dir;
5 years ago
/*
5 years ago
* Turn Servo using custom Speed
*/
5 years ago
/*Function that takes as arguments from->first position and deg->next position */
5 years ago
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);
}
}
5 years ago
}
5 years ago
5 years ago
/*
* Setup Wiring Components
*/
5 years ago
void setup() {
5 years ago
//pinMode(TRIGPIN, OUTPUT); // Sets the TRIGPIN as an Output
//pinMode(ECHOPIN, INPUT); // Sets the ECHOPIN as an Input
5 years ago
//pinMode(response, OUTPUT); // Sets to led aoutput.
5 years ago
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
5 years ago
5 years ago
pinMode(RIGHTSPEED, OUTPUT);
pinMode(LEFTSPEED, OUTPUT);
5 years ago
5 years ago
//Initialize Servo Motor
5 years ago
myServo.attach(SERVOPIN);
5 years ago
pos = 1;
dir = 1;
5 years ago
//Initialize car engine in sleep mode
motor_stop();
5 years ago
5 years ago
Serial.begin(9600); // Starts the serial communication
}
5 years ago
void serialFlush(){
while(Serial.available() > 0) {
char t = Serial.read();
}
}
5 years ago
/*
5 years ago
* Stepper Function for Servo.
* 1st Step: straight
* 2nd Step: left
* 3rd Step: straight
* 4th Step: right
5 years ago
*/
void next_step()
{
if(pos == 1)
5 years ago
{ //Servo move to right
turnServo(90, 30);
5 years ago
pos = 2;
}
else if(pos ==2)
5 years ago
{ //Servo move to center
turnServo(30, 90);
5 years ago
pos = 3;
}
else if(pos == 3)
5 years ago
{ //Servo move to left
turnServo(90, 150);
5 years ago
pos = 4;
}
else if(pos == 4)
5 years ago
{ //Servo move to center
turnServo(150, 90);
5 years ago
pos = 1;
}
}
/*
* Distance meter via Ultrasonic
*/
5 years ago
float getDistance(){
5 years ago
do{
int uS1 = sonar1.ping();
distanceCm = uS1 / US_ROUNDTRIP_CM;
5 years ago
delayMicroseconds(2);
5 years ago
} while(distanceCm == 0);
5 years ago
return distanceCm;
}
/*
5 years ago
* Left turn walk
5 years ago
*/
5 years ago
void turnLeft(const int wheelSpeed){
analogWrite(LEFTSPEED, wheelSpeed);
analogWrite(RIGHTSPEED, 0);
5 years ago
}
5 years ago
/*
* Right turn walk
*/
5 years ago
void turnRight(const int wheelSpeed){
analogWrite(LEFTSPEED, 0);
analogWrite(RIGHTSPEED, wheelSpeed);
5 years ago
}
5 years ago
/*
* Straight walk
*/
5 years ago
void absolute(const int wheelSpeed){
analogWrite(LEFTSPEED, wheelSpeed);
analogWrite(RIGHTSPEED, wheelSpeed);
5 years ago
}
5 years ago
/*
* Backward walk
*/
5 years ago
void backward(){
// Set Motor A backward
5 years ago
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
5 years ago
// Set Motor B backward
5 years ago
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
5 years ago
}
5 years ago
/*
* Forward walk
*/
5 years ago
void forward(){
// Set Motor A forward
5 years ago
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
5 years ago
// Set Motor B forward
5 years ago
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
5 years ago
}
5 years ago
/*
* Stop motors
*/
5 years ago
void motor_stop(){
5 years ago
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
5 years ago
}
5 years ago
void turn(const int wheelSpeed){
5 years ago
if (pos == 1){
5 years ago
absolute(wheelSpeed);
5 years ago
} else if (pos == 2){
5 years ago
turnLeft(wheelSpeed);
5 years ago
} else if (pos == 3){
5 years ago
absolute(wheelSpeed);
5 years ago
} else if (pos == 4){
5 years ago
turnRight(wheelSpeed);
5 years ago
}
}
void moveCar(){
if (pos == 3){
if (dir == 1){
5 years ago
turnRight(MAX_WHEEL_SPEED);
5 years ago
backward();
dir = 2;
} else{
5 years ago
turnLeft(MAX_WHEEL_SPEED);
5 years ago
backward();
dir = 1;
}
delay(1500);
forward();
} else{
forward();
5 years ago
}
}
5 years ago
void park(int parkingNo) {
5 years ago
int row = (parkingNo / 2);
5 years ago
int side = (parkingNo % 2); //If side is 1 then the parking is to right
5 years ago
// else if is 0 the parking is to left.
5 years ago
absolute(255);
forward();
delay(row*FORWARD_STEP);
motor_stop();
5 years ago
//Check the side value to turn.
if (side == 1) {
turnRight(255);
5 years ago
}
5 years ago
else {
turnLeft(255);
}
5 years ago
forward();
delay(TURN_TIME);
5 years ago
absolute(255);
while(getDistance()>10){
forward();
5 years ago
delay(100);
5 years ago
}
motor_stop();
5 years ago
}
5 years ago
int Serial_Input()
{
int number;
5 years ago
5 years ago
while(!Serial.available()) ;
5 years ago
serialFlush();
5 years ago
if (Serial.available() > 0) {
number = Serial.parseInt();
while (number <= 0) {
// convert the incoming byte to a char and add it to the string:
number = Serial.parseInt();
5 years ago
//Serial.print("Value:");
//Serial.println(number);
// clear the string for new input:
5 years ago
serialFlush();
5 years ago
}
// if you get a newline, print the string, then the string's value:
5 years ago
}
5 years ago
return number;
}
5 years ago
5 years ago
void loop() {
// Calculating the distance
5 years ago
//distance= getDistance();
5 years ago
// Prints the distance on the Serial Monitor
5 years ago
//Serial.print("Distance: ");
5 years ago
// Serial.println(distance);
5 years ago
// Read serial input:
5 years ago
park(Serial_Input());
5 years ago
//delay(800);
5 years ago
// 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();
// // Move car
// moveCar();
// }
// delay(100);
5 years ago
}