超聲波模塊與主板接線示意圖及Arduino測試代碼

ID:558099 · 發(fā)表于 2019-7-2 18:05

#include <Servo.h>
int pinLB=14;    // Define a 14 Pin
int pinLF=15;    // Define a 15 Pin

int pinRB=16; // Define a 16 Pin
int pinRF=17; // Define a 17 Pin

int MotorLPWM=5;  //Define a 5 Pin
int MotorRPWM=6;  //Define a 6 Pin

int inputPin = 9;  // Define the ultrasound signal receiving a Pin
int outputPin =8;  //Define the ultrasound signal emission Pin

int Fspeedd = 0;    // go
int Rspeedd = 0;    // The right to
int Lspeedd = 0;    // Turn left to
int directionn = 0; // After the former = 8 = 2 left = 4 right = 6
Servo myservo;       // Set up the myservo
int delay_time = 250; // After the servo motor to the stability of the time

int Fgo = 8;       // go
int Rgo = 6;       // The right to
int Lgo = 4;       // Turn left to
int Bgo = 2;       // astern

void setup()
{
  Serial.begin(9600);    // Initialize
  pinMode(pinLB,OUTPUT); // Define 14 pin for the output (PWM)
  pinMode(pinLF,OUTPUT); // Define 15 pin for the output (PWM)
  pinMode(pinRB,OUTPUT); // Define 16 pin for the output (PWM)
  pinMode(pinRF,OUTPUT); // Define 17 pin for the output (PWM)

  pinMode(MotorLPWM,  OUTPUT);  // Define 5 pin for PWM output
  pinMode(MotorRPWM,  OUTPUT);  // Define 6 pin for PWM output

  pinMode(inputPin, INPUT); // Define the ultrasound enter pin
  pinMode(outputPin, OUTPUT);  // Define the ultrasound output pin

  myservo.attach(11); // Define the servo motor output 10 pin(PWM)
}
void advance(int a)    // go
{
   digitalWrite(pinRB,LOW);  // 16 feet for high level
   digitalWrite(pinRF,HIGH); //17 feet for low level
   analogWrite(MotorRPWM,180);//Set the output speed(PWM)
   digitalWrite(pinLB,LOW);  // 14 feet for high level
   digitalWrite(pinLF,HIGH); //15 feet for high level
   analogWrite(MotorLPWM,180);//Set the output speed(PWM)
   delay(a * 1);
}

void right(int b)       //right
{
   digitalWrite(pinRB,HIGH);
   digitalWrite(pinRF,LOW);
   analogWrite(MotorRPWM,250);
   digitalWrite(pinLB,LOW);
   digitalWrite(pinLF,LOW);
   delay(b * 100);
}
void left(int c)       //left
{
   digitalWrite(pinRB,LOW);
   digitalWrite(pinRF,LOW);
   digitalWrite(pinLB,LOW);
   digitalWrite(pinLF,HIGH);
   analogWrite(MotorLPWM,250);
   delay(c * 100);
}
void turnR(int d)       //right
{
   digitalWrite(pinRB,LOW);
   digitalWrite(pinRF,HIGH);
   analogWrite(MotorRPWM,250);
   digitalWrite(pinLB,HIGH);
   digitalWrite(pinLF,LOW);
   analogWrite(MotorLPWM,250);
   delay(d * 50);
}
void turnL(int e)       //left
{
   digitalWrite(pinRB,HIGH);
   digitalWrite(pinRF,LOW);
   analogWrite(MotorRPWM,220);
   digitalWrite(pinLB,LOW);
   digitalWrite(pinLF,HIGH);
   analogWrite(MotorLPWM,220);
   delay(e * 50);
}
void stopp(int f)       //stop
{
   digitalWrite(pinRB,LOW);
   digitalWrite(pinRF,LOW);
   digitalWrite(pinLB,LOW);
   digitalWrite(pinLF,LOW);
   delay(f * 100);
}
void back(int g)       //back
{

   digitalWrite(pinRB,HIGH);
   digitalWrite(pinRF,LOW);
   analogWrite(MotorRPWM,0);
   digitalWrite(pinLB,HIGH);
   digitalWrite(pinLF,LOW);
   analogWrite(MotorLPWM,230);
   delay(g * 500);
}

void detection()       //Measuring three angles(0.90.179)
{
   int delay_time = 200; // After the servo motor to the stability of the time
   ask_pin_F();          // Read in front of the distance

   if(Fspeedd < 10)       // If the front distance less than 10 cm
   {
   stopp(1);             // Remove the output data
   back(2);             // The back two milliseconds
   }

   if(Fspeedd < 25)       // If the front distance less than 25 cm
   {
      stopp(1);             // Remove the output data
      ask_pin_L();          // Read the left distance
      delay(delay_time);    // Waiting for the servo motor is stable
      ask_pin_R();          // Read the right distance
      delay(delay_time);    //  Waiting for the servo motor is stable

      if(Lspeedd > Rspeedd) //If the distance is greater than the right distance on the left
      {
      directionn = Lgo;    //Left
      }

      if(Lspeedd <= Rspeedd) //If the distance is less than or equal to the distance on the right
      {
      directionn = Rgo;    //right
      }

      if (Lspeedd < 15 && Rspeedd < 15) /*If the left front distance and distance and the right distance is less than 15 cm */
      {

      directionn = Bgo;    //Walk backwards
      }
   }
   else                   //If the front is not less than 25 cm (greater than)
   {
      directionn = Fgo;       //Walk forward
   }

}
void ask_pin_F() // Measure the distance ahead
{
   myservo.write(90);
   digitalWrite(outputPin, LOW); // For low voltage 2 us ultrasonic launch
   delayMicroseconds(2);
   digitalWrite(outputPin, HIGH);  // Let ultrasonic launch 10 us high voltage, there is at least 10 us
   delayMicroseconds(10);
   digitalWrite(outputPin, LOW); // Maintaining low voltage ultrasonic launch
   float Fdistance = pulseIn(inputPin, HIGH);  // Read the time difference
   Fdistance= Fdistance/5.8/10;    // A time to distance distance (unit: cm）
   Serial.print("F distance:");    //The output distance (unit: cm)
   Serial.println(Fdistance);       //According to the distance
   Fspeedd = Fdistance;
}
void ask_pin_L() // Measure the distance on the left
{
   myservo.write(5);
   delay(delay_time);
   digitalWrite(outputPin, LOW); // For low voltage 2 us ultrasonic launch
   delayMicroseconds(2);
   digitalWrite(outputPin, HIGH);  // Let ultrasonic launch 10 us high voltage, there is at least 10 us
   delayMicroseconds(10);
   digitalWrite(outputPin, LOW); // Maintaining low voltage ultrasonic launch
   float Ldistance = pulseIn(inputPin, HIGH);  // Read the time difference
   Ldistance= Ldistance/5.8/10;    // Will be time to distance distance (unit: cm)
   Serial.print("L distance:");    //The output distance (unit: cm)
   Serial.println(Ldistance);       //According to the distance
   Lspeedd = Ldistance;             // Will reading Lspeedd distance
}
void ask_pin_R() // Measure the distance on the right
{
   myservo.write(177);
   delay(delay_time);
   digitalWrite(outputPin, LOW); // For low voltage 2 us ultrasonic launch
   delayMicroseconds(2);
   digitalWrite(outputPin, HIGH);  // Let ultrasonic launch 10 us high voltage, there is at least 10 us
   delayMicroseconds(10);
   digitalWrite(outputPin, LOW); // Maintaining low voltage ultrasonic launch
   float Rdistance = pulseIn(inputPin, HIGH);  // Read the time difference
   Rdistance= Rdistance/5.8/10;    //Will be time to distance distance (unit: cm)
   Serial.print("R distance:");    //The output distance (unit: cm)
   Serial.println(Rdistance);       //According to the distance
   Rspeedd = Rdistance;
}

void loop()
{
myservo.write(90);  /*Make the servo motor ready position Prepare the next measurement */
detection();       //Measuring Angle And determine which direction to go to

if(directionn == 2)  //If directionn (direction) = 2 (back)
{
   back(8);                   //  back
   turnL(2);                //Move slightly to the left (to prevent stuck in dead end lane)
   Serial.print(" Reverse "); //According to the direction (reverse)
}
if(directionn == 6)          //If directionn (direction) = 6 (right)
{
   back(1);
   turnR(6);                // right
   Serial.print(" Right "); //According to the direction (Right)
}
if(directionn == 4)       //If directionn (direction) = 4 (left)
{
   back(1);
   turnL(6);                // left
   Serial.print(" Left ");    //According to the direction (Left)
}
if(directionn == 8)       //If directionn (direction) = 8 (forward)
{
advance(1);                // go
Serial.print(" Advance "); //According to the direction (Advance)
Serial.print(" ");
}
}

ID:558099 · 發(fā)表于 2019-7-2 18:09

需要的可以拿去做測試

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

超聲波模塊與主板接線示意圖及Arduino測試代碼