ส่งงานครั้งที่15 Project หุ่นยนต์หลบสิ่งกีดขวาง

Project หุ่นยนต์หลบสิ่งกีดขวาง

อุปกรณ์

1. Ultrasonic sensors (HC-SR04).———————- (3)
2. Motor driver (L293D).———————————–(1)
3. Arduino UNO.———————————————(1)
4. DC Motor (200 RPM)———————————– (4)
5. 7.4V Rechargeablebattery.————————— (2)
6. Jumper wires.
7. Chassis plate

ตัวอย่างโค้ด

/*  OBSTACLE AVOIDANCE ROBOT */
int echoPinRight = 6;         // PIN 6 TO RECEIVE ECHO FROM RIGHT SENSOR
int trigerPinRight = 7;       // PIN 7 TO TRIGER THE RIGHT SENSOR
int echoPinMiddle = 8;        // PIN 8 TO RECEIVE ECHO FROM MIDDLE SENSOR
int trigerPinMiddle = 9;      // PIN 9 TO TRIGER THE MIDDLE SENSOR
int echoPinLeft = 13;         // PIN 13 TO RECEIVE ECHO FROM LEFT SENSOR
int trigerPinLeft = 10;       // PIN 10 TO TRIGER THE LEFT SENSOR
int motorA1 = 2;              // PIN 2 FOR CONTROLLING MOTOR A1
int motorA2 = 3;              // PIN 3 FOR CONTROLLING MOTOR A2
int motorB1 = 4;              // PIN 4 FOR CONTROLLING MOTOR B1
int motorB2 = 5;              // PIN 4 FOR CONTROLLING MOTOR B1
int motorSpeed = 11;          // ARDUINO PWM PIN 11 TO CONTROL MOTORS SPEED
double echoValueright, echoValuemiddle, echoValueleft;  
// INITIALIZING VARIABLE TO RECEIVE ECHO FROM THE SENSORS
float cmRight, cmMiddle, cmLeft;                           
 // INITIALIZING VARIABLES TO STORE DISTANCE MEASURED OF SENSORS IN CENTIMETERS
void setup()
{
  Serial.begin(9600);                                     // STARTING SERIAL COMMUNICATION TO PRINT VALUES RECEIVED BY SENSOR ON SERIAL MONITER
  pinMode(echoPinLeft, INPUT);                           // SETTING ARDUINO PIN AS INPUT
  pinMode(trigerPinLeft, OUTPUT);                       // SETTING ARDUINO PIN AS OUTPUT
  pinMode(echoPinMiddle, INPUT);                        // SETTING ARDUINO PIN AS INPUT
  pinMode(trigerPinMiddle, OUTPUT);                    // SETTING ARDUINO PIN AS OUTPU
  pinMode(echoPinRight, INPUT);                        // SETTING ARDUINO PIN AS INPUT
  pinMode(trigerPinRight, OUTPUT);                     // SETTING ARDUINO PIN AS OUTPU
  pinMode(motorSpeed, OUTPUT);          // SETTING ARDUINO PIN AS OUTPUT TO CONTROL MOTOR SPEED
}
void loop()
{
  float cmR, cmM, cmL;                                      // INITIALIZING VARIABLES TO STORE DISTANCE OBTAINED FROM SENSORS IN CM
  int maxRange = 30;                                        // SETTING MAX RANGE 
  analogWrite(motorSpeed, 90);                              // SETTING MOTORS SPEED YOU CAN SET SPEED BETWEEN 0-255
  digitalWrite(motorA1, HIGH);
  digitalWrite(motorA2, LOW);
  digitalWrite(motorB1, HIGH);
  digitalWrite(motorB2, LOW);

   
  cmR = sensorRight();
  cmM = sensorMiddle();
  cmL = sensorLeft();
  Serial.println();
  Serial.print(cmL);
  Serial.print("        ");
  Serial.print(cmM);
  Serial.print("        ");
  Serial.print(cmR);
  if (cmR < maxRange)
  {
    digitalWrite(motorA1, LOW);
    digitalWrite(motorA2, HIGH);
    digitalWrite(motorB1, HIGH);
    digitalWrite(motorB2, LOW);
  }
  if (cmL < maxRange)
  {
    digitalWrite(motorA1, HIGH);
    digitalWrite(motorA2, LOW);
    digitalWrite(motorB1, LOW);
    digitalWrite(motorB2, HIGH);
  }

  if (cmM < maxRange) { if (cmL > cmR)
    {
      digitalWrite(motorA1, LOW);
      digitalWrite(motorA2, HIGH);
      digitalWrite(motorB1, HIGH);
      digitalWrite(motorB2, LOW);
    }
    else if (cmR > cmL)
    {
      digitalWrite(motorA1, HIGH);
      digitalWrite(motorA2, LOW);
      digitalWrite(motorB1, LOW);
      digitalWrite(motorB2, HIGH);
    }
    else if (cmR < maxRange && cmL < maxRange)
    {
      digitalWrite(motorA1, LOW);
      digitalWrite(motorA2, HIGH);
      digitalWrite(motorB1, LOW);
      digitalWrite(motorB2, HIGH);
    }
  }

  delay(500);
}
float sensorRight()               // FUUNCTION TO OPERATE RIGHT SENSOR
{
  digitalWrite(trigerPinRight, LOW);
  delayMicroseconds(2);
  digitalWrite(trigerPinRight, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigerPinRight, LOW);
  echoValueright = pulseIn(echoPinRight, HIGH);
  cmRight = echoValueright / 59;
  //inchesRight=cmRight/2.54;
  //Serial.print(cmRight);
  return (cmRight);
}
float snssorMiddle()              // FUNCTION TO OPERATE MIDDLE SENSOR
{
  digitalWrite(trigerPinMiddle, LOW);
  delayMicroseconds(2);
  digitalWrite(trigerPinMiddle, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigerPinMiddle, LOW);
  echoValuemiddle = pulseIn(echoPinMiddle, HIGH);
  cmMiddle = echoValuemiddle / 59;

  //inchesMiddle=cmMiddle/2.54;
  //Serial.print(cmMiddle);
  return (cmMiddle);
}
float sensorLeft()              // FUNCTION TO OPERATE LETT SENSOR
{
  digitalWrite(trigerPinLeft, LOW);
  delayMicroseconds(2);
  digitalWrite(trigerPinLeft, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigerPinLeft, LOW);
  echoValueleft = pulseIn(echoPinLeft, HIGH);
  cmLeft = echoValueleft / 59;
  //inchesLeft=cmLeft/2.54;
  //Serial.print(cmLeft);
  return (cmLeft);
}

อ้างอิง