Update v1.1

autonomousCar/autonomousCar.ino

@@ -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 Ultrasonic Pins
+#define IN3 7
- */
+#define IN4 8
-const int trigPin = 12;
+#define RIGHTSPEED 5
-const int echoPin = 11;
+#define LEFTSPEED 3
+
+#define MAX_WHEEL_SPEED 255
 /*
- * For Motor Driver Pis.
+ * Definitions for Ultrasonic Pins
 */
-const int in1 = 2;
+#define TRIGPIN 12
-const int in2 = 4;
+#define ECHOPIN 11
-const int in3 = 7;
-const int in4 = 8;
-const int rightSpeed = 5;
-const int leftSpeed = 3;
-
 /*
- * 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(TRIGPIN, OUTPUT); // Sets the TRIGPIN as an Output
-  //pinMode(echoPin, INPUT); // Sets the echoPin as an Input
+  //pinMode(ECHOPIN, INPUT); // Sets the ECHOPIN as an Input
   
   //pinMode(response, OUTPUT); // Sets to led aoutput.
   
-  pinMode(in1, OUTPUT); //Motor Driver 1st Pin
+  pinMode(IN1, OUTPUT); //Motor Driver 1st Pin
-  pinMode(in2, OUTPUT); //Motor Driver 2nd Pin
+  pinMode(IN2, OUTPUT); //Motor Driver 2nd Pin
-  pinMode(in3, OUTPUT); //Motor Driver 3rd Pin
+  pinMode(IN3, OUTPUT); //Motor Driver 3rd Pin
-  pinMode(in4, OUTPUT); //Motor Driver 4th Pin
+  pinMode(IN4, OUTPUT); //Motor Driver 4th Pin
   
-  pinMode(rightSpeed, OUTPUT);
+  pinMode(RIGHTSPEED, OUTPUT);
-  pinMode(leftSpeed, 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(){
+void turnLeft(const int wheelSpeed){
-  analogWrite(leftSpeed, 255);
+  analogWrite(LEFTSPEED, wheelSpeed);
-  analogWrite(rightSpeed, 0);
+  analogWrite(RIGHTSPEED, 0);
 }
 
 /*
  * Right turn walk
  */
-void turnRight(){
+void turnRight(const int wheelSpeed){
-  analogWrite(leftSpeed, 0);
+  analogWrite(LEFTSPEED, 0);
-  analogWrite(rightSpeed, 255);
+  analogWrite(RIGHTSPEED, wheelSpeed);
 }
 
 /*
  * Straight walk
  */
-void absolute(){
+void absolute(const int wheelSpeed){
-  analogWrite(leftSpeed, 255);
+  analogWrite(LEFTSPEED, wheelSpeed);
-  analogWrite(rightSpeed, 255);
+  analogWrite(RIGHTSPEED, wheelSpeed);
 }
 
 /*
@@ -195,11 +190,11 @@ void absolute(){
  */
 void backward(){
   // Set Motor A backward
-  digitalWrite(in1, HIGH);
+  digitalWrite(IN1, HIGH);
-  digitalWrite(in2, LOW);
+  digitalWrite(IN2, LOW);
   // Set Motor B backward
-  digitalWrite(in3, HIGH);
+  digitalWrite(IN3, HIGH);
-  digitalWrite(in4, LOW);
+  digitalWrite(IN4, LOW);
 }
 
 /*
@@ -207,44 +202,44 @@ void backward(){
  */
 void forward(){
   // Set Motor A forward
-  digitalWrite(in1, LOW);
+  digitalWrite(IN1, LOW);
-  digitalWrite(in2, HIGH);
+  digitalWrite(IN2, HIGH);
   // Set Motor B forward
-  digitalWrite(in3, LOW);
+  digitalWrite(IN3, LOW);
-  digitalWrite(in4, HIGH);
+  digitalWrite(IN4, HIGH);
 }
 
 /*
  * Stop motors
  */
 void motor_stop(){
-  digitalWrite(in1, LOW);
+  digitalWrite(IN1, LOW);
-  digitalWrite(in2, LOW);
+  digitalWrite(IN2, LOW);
-  digitalWrite(in3, LOW);
+  digitalWrite(IN3, LOW);
-  digitalWrite(in4, 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.print("Distance: ");
-  Serial.println(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);
 }