Raspberry Pi から ESC を制御してラジコンを走らせてみた（２）

Raspberry Pi から ESC を制御してラジコンを走らせてみた（１） の続き

今回は、超音波距離センサ(HC-SR04)を組み込んで「自動ブレーキ」ぽいものを作った

Raspberry Pi から モータドライバと距離センサを使ってミニ四駆を自立走行させてみたが。。　のバニラ基板を流用するため、PWMはGPIO18ではなくGPIO12を使用した

プログラムソースコード（C言語）

前方の障害物までの距離が100cmを超えると走り出し、75cm未満になると停止するプログラム

main.c

//gcc -Wall -O2 main.c -o main -lwiringPi
#include <stdio.h>
#include <stdlib.h>
#include <wiringPi.h>
#include <unistd.h>
#include <sys/time.h>

//HC-SR04
#define HC_SR04_TRIG_PIN        8
#define HC_SR04_ECHO_PIN        7

//TA7291P                               //未使用
#define GPIO_14_OUT             14      //未使用
#define GPIO_15_OUT             15      //未使用
#define GPIO_18_PWM_OUT         18      //未使用

//HC-SR04
void HC_SR04_init(void);
long HC_SR04_get_distance(void);
int HC_SR04_pulseIn(int pin, int level, int timeout);

//PWM
void PWM_init(int nGPIO_port,int nClock,int nRange);
#define GPIO_12_PWM_OUT         12
#define PWM_STOP                92
#define PWM_FORWARD             88
#define PWM_REVERSE             96

/*****************************************************************
*
*****************************************************************/
int main (int argc, char **argv)
{
    long distance;

    if(wiringPiSetupGpio() == -1){
        printf("wiringPiSetupGpio() error\n");
        return -1;
    }

    //未使用GPIO 14,15,18 初期化
    pinMode(GPIO_14_OUT, OUTPUT);
    pinMode(GPIO_15_OUT, OUTPUT);
    pinMode(GPIO_18_PWM_OUT, PWM_OUTPUT);
    digitalWrite(GPIO_14_OUT, LOW);
    digitalWrite(GPIO_15_OUT, LOW);
    pwmWrite(GPIO_18_PWM_OUT, LOW);


    PWM_init(GPIO_12_PWM_OUT,320,1000);     //PWM初期化
    pwmWrite(GPIO_12_PWM_OUT, PWM_STOP);

    HC_SR04_init(); //HC-SR04 初期化

    for(;;){
        delayMicroseconds(200000); //wait
        distance = HC_SR04_get_distance();  //HC-SR04 距離取得

        if(distance > 100){
            pwmWrite(GPIO_12_PWM_OUT, PWM_FORWARD);
        }

        if(distance < 75){
            pwmWrite(GPIO_12_PWM_OUT, PWM_STOP);
        }

        printf("distance=%ld cm\n",distance);
    }

    pwmWrite(GPIO_12_PWM_OUT, PWM_STOP);

    return 0;
}
/*****************************************************************
*   pwm初期化
*****************************************************************/
void PWM_init(int nGPIO_port,int nClock,int nRange)
{
    pinMode(nGPIO_port, PWM_OUTPUT);
    pwmSetMode(PWM_MODE_MS);
    pwmSetClock(nClock);
    pwmSetRange(nRange);
}
/*****************************************************************
*   HC-SR04 距離取得
*****************************************************************/
long HC_SR04_get_distance(void)
{
    long duration, distance;

    digitalWrite(HC_SR04_TRIG_PIN, LOW);
    delayMicroseconds(2);

    digitalWrite(HC_SR04_TRIG_PIN, HIGH);
    delayMicroseconds(10);

    digitalWrite(HC_SR04_TRIG_PIN, LOW);
    duration = HC_SR04_pulseIn(HC_SR04_ECHO_PIN, HIGH, 1000000);
//  printf("duration=%d\n",duration);
    distance = (duration/2) / 29.1;
//  printf("distance=%d cm\n",distance);

    return distance;
}
/*****************************************************************
*   HC-SR04 初期化
*****************************************************************/
void HC_SR04_init(void)
{
    pinMode(HC_SR04_TRIG_PIN,OUTPUT);
    pinMode(HC_SR04_ECHO_PIN,INPUT);
}
/*****************************************************************
*   HC-SR04 パルス波時間を取得する
*****************************************************************/
int HC_SR04_pulseIn(int pin, int level, int timeout)
{
    struct timeval tn, t0, t1;
    long micros;

    gettimeofday(&t0, NULL);
    micros = 0;
    while (digitalRead(pin) != level){
        gettimeofday(&tn, NULL);
        if (tn.tv_sec > t0.tv_sec){
            micros = 1000000L;
        }
        else{
            micros = 0;
        }
        micros += (tn.tv_usec - t0.tv_usec);
        if (micros > timeout) return 0;
    }

    gettimeofday(&t1, NULL);
    while (digitalRead(pin) == level){
        gettimeofday(&tn, NULL);
        if (tn.tv_sec > t0.tv_sec){
            micros = 1000000L;
        }
        else{
            micros = 0;
        }
        micros = micros + (tn.tv_usec - t0.tv_usec);
        if (micros > timeout) return 0;
    }

    if (tn.tv_sec > t1.tv_sec) micros = 1000000L; else micros = 0;
    micros = micros + (tn.tv_usec - t1.tv_usec);

    return micros;
}

外観

動作テスト動画(YouTube)

前方の障害物までの距離が100cmを超えると走り出し、75cm未満になると停止