プログラムソースコード（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未満になると停止