Zaloha - Davidovo automaticke auto.

zalohy/auto_david.ino

@@ -0,0 +1,165 @@
+
+#include <Servo.h>
+#include <Servo.h>
+Servo myservo;
+int Echo_Pin=A0;  // ultrasonic module   ECHO to A0
+int Trig_Pin=A1;  // ultrasonic module  TRIG to A1
+#define Lpwm_pin  5     //pin of controlling speed---- ENA of motor driver board
+#define Rpwm_pin  6    //pin of controlling speed---- ENB of motor driver board
+int pinLB=2;             //pin of controlling turning---- IN1 of motor driver board
+int pinLF=4;             //pin of controlling turning---- IN2 of motor driver board
+int pinRB=7;            //pin of controlling turning---- IN3 of motor driver board
+int pinRF=8;            //pin of controlling turning---- IN4 of motor driver board
+volatile int D_mix;
+volatile int D_mid;
+volatile int D_max;
+volatile int Front_Distance;
+volatile int Left_Distance;
+volatile int Right_Distance;
+volatile int Right_IR_Value;
+volatile int Left_IR_Value;
+
+float checkdistance() {
+  digitalWrite(Trig_Pin, LOW);
+  delayMicroseconds(2);
+  digitalWrite(Trig_Pin, HIGH);
+  delayMicroseconds(10);
+  digitalWrite(Trig_Pin, LOW);
+  float distance = pulseIn(Echo_Pin, HIGH) / 58.00;
+  delay(10);
+  return distance;
+}
+
+void Detect_Left_and_Right__distance() {
+  myservo.write(180);
+  delay(400);
+  Left_Distance = checkdistance();
+  delay(600);
+  Serial.print("Left_Distance:");
+  Serial.println(Left_Distance);
+  myservo.write(0);
+  delay(400);
+  Right_Distance = checkdistance();
+  delay(600);
+  Serial.print("Right_Distance:");
+  Serial.println(Right_Distance);
+  myservo.write(90);
+}
+
+void Ultrasonic_obstacle_avoidance()
+{
+  Front_Distance=checkdistance(); //obtain the value detected by ultrasonic sensor 
+  if((Front_Distance < 20)&&(Front_Distance > 0)) {
+      stopp();//stop
+      delay(100);
+      myservo.write(180);
+      delay(500);
+      Left_Distance=checkdistance();//measure the distance
+      delay(100);
+      myservo.write(0);
+      delay(500);
+      Right_Distance=checkdistance();//measure the distance
+      delay(100);
+      if(Left_Distance > Right_Distance) {
+        rotate_left(150);//turn left
+        myservo.write(90);
+        delay(300); 
+      }
+      else {
+        rotate_right(150);// turn right
+        myservo.write(90);
+        delay(300); 
+      }
+  }
+  else {
+    go_forward(100);//go forward
+  }
+}
+
+
+
+
+
+
+void Obstacle_Avoidance_Main() 
+{
+  Ultrasonic_obstacle_avoidance();
+}
+
+
+
+void setup(){
+  myservo.attach(A2);
+  Serial.begin(9600);
+  D_mix = 10;
+  D_mid = 20;
+  D_max = 100;
+  Front_Distance = 0;
+  Left_Distance = 0;
+  Right_Distance = 0;
+  myservo.write(90);
+  pinMode(Echo_Pin, INPUT);      
+  pinMode(Trig_Pin, OUTPUT);    
+  pinMode(pinLB,OUTPUT); // /pin 2
+  pinMode(pinLF,OUTPUT); // pin 4
+  pinMode(pinRB,OUTPUT); // pin 7
+  pinMode(pinRF,OUTPUT);  // pin 8
+  pinMode(Lpwm_pin,OUTPUT);  // pin 5 (PWM) 
+  pinMode(Rpwm_pin,OUTPUT);  // pin 6(PWM)   
+}
+
+void loop(){
+  Obstacle_Avoidance_Main();
+
+}
+
+
+
+void go_forward(unsigned char speed_val)    // speed_val：0~255
+    {digitalWrite(pinRB,HIGH); 
+     digitalWrite(pinRF,LOW);
+     digitalWrite(pinLB,HIGH);
+     digitalWrite(pinLF,LOW);
+     analogWrite(Lpwm_pin,speed_val+6);
+     analogWrite(Rpwm_pin,speed_val+5);
+     
+      
+    }
+
+void go_backward(unsigned char speed_val)    // speed_val：0~255
+    {
+     digitalWrite(pinRB,LOW);  
+     digitalWrite(pinRF,HIGH);
+     digitalWrite(pinLB,LOW);  
+     digitalWrite(pinLF,HIGH);
+     analogWrite(Lpwm_pin,speed_val);
+     analogWrite(Rpwm_pin,speed_val);
+    }
+    
+void rotate_left(unsigned char speed_val)        // speed_val：0~255
+    {digitalWrite(pinRB,HIGH);
+     digitalWrite(pinRF,LOW );  
+     digitalWrite(pinLB,LOW); 
+     digitalWrite(pinLF,HIGH);
+     analogWrite(Lpwm_pin,speed_val);
+     analogWrite(Rpwm_pin,speed_val);
+      
+     
+    }
+void rotate_right(unsigned char speed_val)    // speed_val：0~255
+    {
+      digitalWrite(pinRB,LOW);  
+     digitalWrite(pinRF,HIGH);
+     digitalWrite(pinLB,HIGH);
+     digitalWrite(pinLF,LOW);  
+     analogWrite(Lpwm_pin,speed_val);
+     analogWrite(Rpwm_pin,speed_val);
+     
+    }    
+void stopp()        //stop
+    {
+     digitalWrite(pinRB,HIGH);
+     digitalWrite(pinRF,HIGH);
+     digitalWrite(pinLB,HIGH);
+     digitalWrite(pinLF,HIGH);
+    }