update

autonomousCar/driver/driver.ino

@@ -0,0 +1,220 @@
+/*
+* 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 Stepper Library
+#include <Stepper.h>
+
+/*
+ * -------------------------------------------------------
+*/
+
+/*
+ * All Definitions.
+*/
+// Number of steps per internal motor revolution 
+#define STEPS_PER_REV 32
+
+#define GEAR_RED 64
+/*
+ * -------------------------------------------------------
+*/
+
+// defines pins numbers
+const int trigPin = 12;
+const int echoPin = 9;
+//const int response = 13;
+/*
+ * 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 Stepper Motor Pins
+*/
+const int in1_1 = 6;
+const int in1_2 = 10;
+const int in1_3 = 11;
+const int in1_4 = 13;
+
+const int stepper_motor_speed = 700;
+
+// Number of steps per geared output rotation
+const float STEPS_PER_OUT_REV = STEPS_PER_REV * GEAR_RED;
+const int StepsRequired  =  STEPS_PER_OUT_REV / 4;
+
+// define Stepper
+Stepper steppermotor(STEPS_PER_REV, in1_1, in1_2, in1_3, in1_4);
+
+// defines variables
+long duration;
+int distance, distanceCm, pos;
+/*
+ * 
+*/
+void clearUltrasonic(){
+  // Clears the trigPin
+  analogWrite(trigPin, LOW);
+  delayMicroseconds(2);
+}
+
+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);
+
+  Serial.begin(9600); // Starts the serial communication
+  clearUltrasonic();
+}
+
+/*
+ * Stepper Function
+ */
+void next_step()
+{
+  if(pos == 1)
+  {
+    steppermotor.setSpeed(stepper_motor_speed);
+    steppermotor.step(StepsRequired);
+    pos = 2;
+  }
+  else if(pos ==2)
+  {
+    steppermotor.setSpeed(stepper_motor_speed);
+    steppermotor.step(-1*StepsRequired);
+    pos = 3;
+  }
+  else if(pos == 3)
+  {
+    steppermotor.setSpeed(stepper_motor_speed);
+    steppermotor.step(-1*StepsRequired);
+    pos = 4;
+  }
+  else if(pos == 4)
+  {
+    steppermotor.setSpeed(stepper_motor_speed);
+    steppermotor.step(StepsRequired);
+    pos = 1;
+  }
+}
+
+/*
+ * Distance meter via Ultrasonic
+ */
+
+int getDistance(int echoPin, int trigPin){
+  clearUltrasonic();
+  // Sets the trigPin on HIGH state for 10 micro seconds
+  digitalWrite(trigPin, HIGH);
+  delayMicroseconds(10);
+  digitalWrite(trigPin, LOW);
+
+  // Reads the echbackwardoPin, returns the sound wave travel time in microseconds
+  duration = pulseIn(echoPin, HIGH);
+  distanceCm = duration * 0.0340 / 2;
+
+  return distanceCm;
+}
+
+/*
+ * Stepper Function
+ */
+void step(/*boolean dir,*/int steps){
+//  digitalWrite(dirPin1,dir);
+//  digitalWrite(dirPin2,dir);
+  delay(50);
+  for(int i=0;i<steps;i++){
+    forward();
+    delayMicroseconds(100);
+    motor_stop();
+    delayMicroseconds(100);
+  }
+}
+
+
+void turnLeft(){
+  analogWrite(leftSpeed, 255);
+  analogWrite(rightSpeed, 0);
+}
+
+void turnRight(){
+  analogWrite(leftSpeed, 0);
+  analogWrite(rightSpeed, 255);
+}
+
+void absolute(){
+  analogWrite(leftSpeed, 255);
+  analogWrite(rightSpeed, 255);
+}
+
+void backward(){
+  // Set Motor A backward
+  digitalWrite(in1, HIGH);
+  digitalWrite(in2, LOW);
+  // Set Motor B backward
+  digitalWrite(in3, HIGH);
+  digitalWrite(in4, LOW);
+}
+
+void forward(){
+  // Set Motor A forward
+  digitalWrite(in1, LOW);
+  digitalWrite(in2, HIGH);
+  // Set Motor B forward
+  digitalWrite(in3, LOW);
+  digitalWrite(in4, HIGH);
+}
+
+void motor_stop(){
+  digitalWrite(in1, LOW);
+  digitalWrite(in2, LOW);
+  digitalWrite(in3, LOW);
+  digitalWrite(in4, LOW);
+}
+
+void turn(){
+  if (pos == 1){
+    turnLeft();
+  } else if (pos == 2){
+    absolute();
+  } else if (pos == 3){
+    turnRight();
+  } else if (pos == 4){
+    absolute();
+  }
+}
+
+void loop() {
+  // Calculating the distance
+  distance= getDistance(echoPin, trigPin);
+  
+  // Prints the distance on the Serial Monitor
+  //Serial.print("Distance: ");
+  //Serial.println(distance);
+  if (distance < 9){
+    motor_stop();
+    turn();
+    forward();
+  }
+  delay(500);
+}