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未満になると停止