9일차

2025.05.28

 

초음파센서 동작

#include <stdio.h>
#include <wiringPi.h>

#define TRIG 27
#define ECHO 28

void setup() {
    wiringPiSetup();
    pinMode(TRIG, OUTPUT);
    pinMode(ECHO, INPUT);
    digitalWrite(TRIG, LOW);
    delay(30);  // 센서 안정화 대기
}

double getDistance() {
    // Trigger pulse
    digitalWrite(TRIG, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG, LOW);

    // Echo 신호가 HIGH가 될 때까지 대기
    while (digitalRead(ECHO) == LOW);

    int startTime = micros();

    // Echo 신호가 LOW가 될 때까지 대기
    while (digitalRead(ECHO) == HIGH);

    int endTime = micros();

    // 거리 계산: 왕복 시간 x 음속(0.017 cm/us)
    double distance = (endTime - startTime) * 0.017;
    return distance;
}

int main() {
    setup();

    while (1) {
        double distance = getDistance();
        printf("Distance: %.2f cm\n", distance);
        delay(500);  // 0.5초 간격
    }

    return 0;
}

 

서보모터와 초음파센서 연동

#include <stdio.h>
#include <wiringPi.h>
#include <softPwm.h>

#define TRIG 27
#define ECHO 28
#define SERVO_PIN 29
#define PWM_RANGE 200

// 각도를 소프트 PWM duty로 변환 (0도:5 ~ 180도:25)
int angleToDuty(int angle) {
    return (5 * (180 - angle) + 25 * angle) / 180;
}

// 거리 측정 함수
double getDistance() {
    digitalWrite(TRIG, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG, LOW);

    while (digitalRead(ECHO) == LOW);
    int startTime = micros();

    while (digitalRead(ECHO) == HIGH);
    int endTime = micros();

    return (endTime - startTime) * 0.017;  // cm
}

// 초기화
int setup() {
    if (wiringPiSetup() == -1) return 1;

    pinMode(TRIG, OUTPUT);
    pinMode(ECHO, INPUT);
    digitalWrite(TRIG, LOW);
    delay(30);

    if (softPwmCreate(SERVO_PIN, 0, PWM_RANGE) != 0) {
        fprintf(stderr, "softPwmCreate failed\n");
        return 1;
    }

    return 0;
}

int main() {
    if (setup() != 0) return 1;

    while (1) {
        double distance = getDistance();
        printf("Distance: %.2f cm\n", distance);

        if (distance < 10.0) {
            // 역회전: 180° → 0°
            for (int ang = 180; ang >= 0; ang -= 5) {
                softPwmWrite(SERVO_PIN, angleToDuty(ang));
                delay(20);
            }
        } else {
            // 정회전: 0° → 180°
            for (int ang = 0; ang <= 180; ang += 5) {
                softPwmWrite(SERVO_PIN, angleToDuty(ang));
                delay(20);
            }
        }

        delay(500);  // 다음 측정까지 대기
    }

    return 0;
}

 

스텝모터와 초음파센서 연동

#include <stdio.h>
#include <wiringPi.h>

// 초음파 센서 핀 (wiringPi 번호)
#define TRIG    27
#define ECHO    28

// 스텝모터 핀 (wiringPi 번호)
#define ORANGE  21
#define YELLOW  22
#define PINK    23
#define BLUE    24
#define RED     25  // 모터 전원용

// 초음파 거리 측정 함수 (cm)
double get_distance() {
    digitalWrite(TRIG, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG, LOW);

    // Echo 신호 시작 대기
    while (digitalRead(ECHO) == LOW);
    long start_time = micros();
    // Echo 신호 끝 대기
    while (digitalRead(ECHO) == HIGH);
    long end_time = micros();

    // 거리 계산: 왕복 시간(us) × 음속(cm/us) / 2
    return (end_time - start_time) * 0.017;
}

// Full-step 모드 한 스텝 구동
void step_full(int step) {
    switch(step) {
        case 0:
            digitalWrite(ORANGE, HIGH);
            digitalWrite(YELLOW, HIGH);
            digitalWrite(PINK,   LOW);
            digitalWrite(BLUE,   LOW);
            break;
        case 1:
            digitalWrite(ORANGE, LOW);
            digitalWrite(YELLOW, HIGH);
            digitalWrite(PINK,   HIGH);
            digitalWrite(BLUE,   LOW);
            break;
        case 2:
            digitalWrite(ORANGE, LOW);
            digitalWrite(YELLOW, LOW);
            digitalWrite(PINK,   HIGH);
            digitalWrite(BLUE,   HIGH);
            break;
        case 3:
            digitalWrite(ORANGE, HIGH);
            digitalWrite(YELLOW, LOW);
            digitalWrite(PINK,   LOW);
            digitalWrite(BLUE,   HIGH);
            break;
        default:
            break;
    }
}

int main() {
    // wiringPi 초기화
    if (wiringPiSetup() == -1) {
        fprintf(stderr, "wiringPi setup failed\n");
        return 1;
    }

    // 핀 모드 설정
    pinMode(TRIG, OUTPUT);
    pinMode(ECHO, INPUT);
    pinMode(ORANGE, OUTPUT);
    pinMode(YELLOW, OUTPUT);
    pinMode(PINK, OUTPUT);
    pinMode(BLUE, OUTPUT);
    pinMode(RED, OUTPUT);

    // 모터 전원 공급
    digitalWrite(RED, HIGH);

    while (1) {
        double distance = get_distance();
        printf("Distance: %.2f cm\n", distance);

        if (distance < 10.0) {
            // 역회전: 역방향 인덱스 보정 적용
            for (int i = 0; i < 512; i++) {
                int idx = (3 - (i % 4) + 4) % 4;
                step_full(idx);
                delay(5);
            }
        } else {
            // 정회전
            for (int i = 0; i < 512; i++) {
                step_full(i % 4);
                delay(5);
            }
        }

        delay(1000);  // 1초 대기 후 다음 측정
    }

    return 0;
}