超音波センサHC-SR04の使い方｜ArduinoとRaspberry Piで解説

/*
  Created by Toshihiko Arai.
  https://101010.fun/arduino-hc-sr04.html
*/

#define TrigPin 1 // D1
#define EchoPin 2 // D2
 
double speedSound = 331.5 + 0.61 * 20; // 20は現在の気温
double distance = 0;


void setup() {
  Serial.begin(9600);
  pinMode(TrigPin, OUTPUT);
  pinMode(EchoPin, INPUT);
}


void loop() {
  trigger();

  double t = pulseIn(EchoPin, HIGH); // μS

  if (t > 0) {
    t = t / 2; //往復距離なので半分の時間
    distance = t * speedSound * 100 / 1000000; // 距離（cm）を計算
    Serial.println(distance);
  }
  delay(500);
}

void trigger() {
  digitalWrite(TrigPin, LOW); 
  delayMicroseconds(2);
  digitalWrite(TrigPin, HIGH );
  delayMicroseconds( 10 );
  digitalWrite(TrigPin, LOW );
}

/*
  Created by Toshihiko Arai.
  https://101010.fun/arduino-hc-sr04.html
*/

#include <U8g2lib.h>

U8G2_SSD1306_128X32_UNIVISION_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);

#define TrigPin 1 // D1
#define EchoPin 2 // D2
 
double speedSound = 331.5 + 0.61 * 20; // 20は現在の気温


void setup() {
  Serial.begin(9600);
  pinMode(TrigPin, OUTPUT);
  pinMode(EchoPin, INPUT);

  u8g2.begin();
}


void loop() {
  trigger();

  double distance = 0;
  double t = pulseIn(EchoPin, HIGH); // μS

  if (t > 0) {
    t = t / 2; //往復距離なので半分の時間
    distance = t * speedSound * 100 / 1000000; // 距離（cm）を計算
    displayOLCD(distance);
  }
  delay(500);
}

void trigger() {
  digitalWrite(TrigPin, LOW); 
  delayMicroseconds(2);
  digitalWrite(TrigPin, HIGH );
  delayMicroseconds( 10 );
  digitalWrite(TrigPin, LOW );
}


void displayOLCD(double distance) {
    char buf[10];
    snprintf(buf, 10, "%.1fcm", distance);

    u8g2.clearBuffer();
    u8g2.setFont(u8g2_font_crox3hb_tf);
    u8g2.drawStr(0, 16, "Distance");
    u8g2.drawStr(0, 32, buf);
    u8g2.sendBuffer();
}

import RPi.GPIO as GPIO
import time
import os
import signal

GPIO.setmode(GPIO.BCM)
TRIG = 20
ECHO = 21
C = 343  # 気温20度の時の音速(m/s)

GPIO.setup(TRIG, GPIO.OUT)
GPIO.setup(ECHO, GPIO.IN)
GPIO.output(TRIG, 0)
time.sleep(0.3)


def readDistance():
    GPIO.output(TRIG, 1)
    time.sleep(0.00001)  # 10μs
    GPIO.output(TRIG, 0)

    while GPIO.input(ECHO) == 0:
        signaloff = time.time()
    while GPIO.input(ECHO) == 1:
        signalon = time.time()

    t = signalon - signaloff
    distance = t * C * 100 / 2
    return distance


def cleanup():
    print('cleanup')
    GPIO.cleanup()


try:
    while True:
        dist = readDistance()
        print(dist)
        time.sleep(0.1)


except KeyboardInterrupt:
    # SIGINTを監視していれば不要
    print('KeyboardInterrupt')
except:
    print('other')
finally:
    # 終了処理
    cleanup()

import RPi.GPIO as GPIO
import time
import os
import signal

GPIO.setmode(GPIO.BCM)
SIG = 26
GPIO.setup(SIG, GPIO.OUT)

servo = GPIO.PWM(SIG, 50)
time.sleep(0.3)

servo.start(0)
servo.ChangeDutyCycle(6.3)  # 0°
time.sleep(1.0)

for i in range(5):
    servo.ChangeDutyCycle(2.2)  # 0°
    time.sleep(1.0)

    servo.ChangeDutyCycle(10.8)  # 180°
    time.sleep(1.0)

servo.ChangeDutyCycle(6.3)  # 90°
time.sleep(1.0)

servo.stop()
GPIO.cleanup()

import RPi.GPIO as GPIO
import time
import os
import signal

GPIO.setmode(GPIO.BCM)  # 役割ピン番号で命名
TRIG = 20
ECHO = 21
SIG = 26
SERVO_PWM_MIN = 2.2  # 0° 時計の針で9時を0°とする
SERVO_PWM_MAX = 10.8  # 180°
SERVO_PWM_1DEGREE = (SERVO_PWM_MAX - SERVO_PWM_MIN) / 180
MONITORING_DIST = 20  # 最短距離

C = 343  # 気温20度での音速(m/s)

GPIO.setup(TRIG, GPIO.OUT)
GPIO.setup(ECHO, GPIO.IN)
GPIO.setup(SIG, GPIO.OUT)

GPIO.output(TRIG, 0)
servo = GPIO.PWM(SIG, 50)
time.sleep(0.3)

servo.start(0)


def readDistance():
    GPIO.output(TRIG, 1)
    time.sleep(0.00001)  # 10μs
    GPIO.output(TRIG, 0)

    while GPIO.input(ECHO) == 0:
        signaloff = time.time()  # 秒
    while GPIO.input(ECHO) == 1:
        signalon = time.time()  # 秒

    t = signalon - signaloff  # 秒
    distance = t * C * 100 / 2
    return distance


def rotate(angle):  # 0°から180°
    pwm = angle * SERVO_PWM_1DEGREE + SERVO_PWM_MIN
    if pwm > SERVO_PWM_MAX:
        pwm = SERVO_PWM_MAX
    elif pwm < SERVO_PWM_MIN:
        pwm = SERVO_PWM_MIN

    servo.ChangeDutyCycle(pwm)
    return


def cleanup():
    print('cleanup')
    rotate(90)
    time.sleep(1)
    servo.stop()
    GPIO.cleanup()


initAngle = 0
currentAngle = 0
clockwise = True
isCatched = False
isMissing = False  # 物体を見失う
isWondering = False  # 不安フラグ
missingCount = 0


try:
    rotate(initAngle)
    time.sleep(1)
    while True:

        dist = readDistance()
        # print('角度: {0}, 距離: {1}'.format(currentAngle, dist))

        isExistObject = dist < MONITORING_DIST

        if isExistObject:
            isMissing = False
            missingCount = 0
            isWondering = False
            if isCatched == False:  # 新規発見！
                isCatched = True
                clockwise = False if clockwise == True else True
        else:
            if isCatched:
                isCatched = False
                clockwise = False if clockwise == True else True
                # print("物体を見逃しますた！")
                isMissing = True

            if isMissing:
                missingCount += 1

        if missingCount > 100:  # 物体が存在しなかったのでリセットする
            isCatched = False
            isWondering = False
            isMissing = False
        elif missingCount > 10:  # 不安フラグを立てる
            isWondering = True

        if isCatched:
            gain = 0.7
        elif isWondering:
            gain = 4
        else:
            gain = 1

        if clockwise:
            gain *= 1
        else:
            gain *= -1

        currentAngle += 1 * gain

        if currentAngle > 180:
            clockwise = False
        elif currentAngle < 0:
            clockwise = True

        rotate(currentAngle)
        time.sleep(0.005)


except KeyboardInterrupt:
    print('KeyboardInterrupt')
except:
    print('other')
finally:
    cleanup()