node-robot-648a.c

/***************robot.c**********
node-serialportから制御できるロボットを作るため
UART入力でモーターを制御する。
追加機能でクラッカーを引く＆超音波センサで距離を読み取る。
*****************************************/

#include <16F648A.h>
#fuses INTRC_IO,NOWDT,PUT,NOPROTECT,NOMCLR
#use delay(CLOCK = 4000000)		//4MHz
#use rs232(baud=9600,parity=N,xmit=PIN_B2,rcv=PIN_B1)	//ハードウェアUART
#use fast_io(a)
#use fast_io(b)

/***************PINについて******************
A0,A1 モーター1
A2,A3 モーター2
A6,A7 モーター3
B1,B2 RX,TX
B3/CCP LED コモン　カソード
B4 LED 赤アノード
B5 LED 青アノード
B6 LED 緑アノード
B0 超音波センサ入力
B7 超音波センサトリガ
*****************************************/

#define MOTOR1A PIN_A0
#define MOTOR1B PIN_A1
#define MOTOR2A PIN_A3
#define MOTOR2B PIN_A2
#define MOTOR3A PIN_A6
#define MOTOR3B PIN_A7
#define MSTP_US 700

#define LED_R PIN_B6
#define LED_G PIN_B4
#define LED_B PIN_B5
#define LED_COM PIN_B3

#define SS_ECHO PIN_B0
#define SS_TRIG PIN_B7
#define SS_ATSND PIN_A4

#define STOP_DISTANCE 100		//100*0.187=18.7cm
#define LONG_MAX 65535

/************スレッドについて******************
タイマ割り込みで約0.5秒に一回距離を測定する。
timer2はccpで使う。
*****************************************/


//グローバル変数の定義
int led_bright;
int command;

void motor1_foward()
{
	output_low(MOTOR1A);
	output_low(MOTOR1B);
	delay_us(MSTP_US);
	output_high(MOTOR1A);
}

void motor1_back()
{
	output_low(MOTOR1A);
	output_low(MOTOR1B);
	delay_us(MSTP_US);
	output_high(MOTOR1B);
}

void motor1_stop()
{
	output_low(MOTOR1A);
	output_low(MOTOR1B);
	delay_us(MSTP_US);
}


void motor2_foward()
{
	output_low(MOTOR2A);
	output_low(MOTOR2B);
	delay_us(MSTP_US);
	output_high(MOTOR2A);
}

void motor2_back()
{
	output_low(MOTOR2A);
	output_low(MOTOR2B);
	delay_us(MSTP_US);
	output_high(MOTOR2B);
}

void motor2_stop()
{
	output_low(MOTOR2A);
	output_low(MOTOR2B);
	delay_us(MSTP_US);
}
void motor3_foward()
{
	output_low(MOTOR3A);
	output_low(MOTOR3B);
	delay_us(MSTP_US);
	output_high(MOTOR3A);
}

void motor3_back()
{
	output_low(MOTOR3A);
	output_low(MOTOR3B);
	delay_us(MSTP_US);
	output_high(MOTOR3B);
}

void motor3_stop()
{
	output_low(MOTOR3A);
	output_low(MOTOR3B);
	delay_us(MSTP_US);
}


long distance, distance_old;

/*タイマ割り込みで約0.52秒に1回距離を測定する*/
#INT_TIMER1
void timer1_isr(){
	
	set_timer1(LONG_MAX);
	
	output_high(SS_TRIG);
	delay_us(10);
	output_low(SS_TRIG);
	
	distance = 0;
	
	while(!input(SS_ECHO) && distance != LONG_MAX){	//echoがHIになるのを待つ
		distance++;									//センサが繋がっていない時はタイムアップするようにする。
	}	
	
	distance = 0;
	
	while(input(SS_ECHO) && distance != LONG_MAX){		//echoが帰ってきている間カウントアップ。
		distance++;									//なぜかechoがHのまま固まってしまうことがある。
	}
	
	//超音波センサがバグっていたら前回の値で上書きする。
	if (distance == 0 || distance == LONG_MAX){
		distance = distance_old;
	}else{
		distance_old = distance;
	}
	/*
	//結果の出力。割り算とかはパソコンにしてもう。
	//ちなみに.lstファイルによると、11命令で1加算、4MHzでは1命令=1usなので
	//音速を340m/sとすると、340*11*distance/10^6*100/2(cm) = 0.187 * distance (cm)となる。
	*/
	if(!input(SS_ATSND)){
		printf("%Lu\r\n",distance);
	}
	
	//障害物が近くにあったら停止
	if(distance < STOP_DISTANCE){	
		motor1_stop();
		motor2_stop();
	}
}


/*メイン関数*/
void main()
{	
	set_tris_a(0b00110000);		//a4,a5は入力（
	set_tris_b(0b00000011);		//b0,b1は入力

	//1サイクル4クロックなので実質1MHz、8分周しているので125kHz
	//割り込みは8us毎に発生、timerを0xffに設定すると524.28ms毎に割り込み発生
	setup_timer_1(T1_INTERNAL | T1_DIV_BY_8);	//T1_DIVは8まで。
	set_timer1(LONG_MAX);
	output_low(SS_TRIG);
	distance_old = LONG_MAX;
	
	//LEDの明るさ調整用CCP/PWM設定
	setup_oscillator(OSC_4MHZ);
	setup_ccp1(CCP_PWM); 
	setup_timer_2(T2_DIV_BY_1,255,1);
	set_pwm1_duty(32);
	led_bright = 0;
	
	output_high(LED_R);
	output_high(LED_G);
	output_high(LED_B);
	
	motor1_stop();
	motor2_stop();
	motor3_stop();

	enable_interrupts(INT_TIMER1);		//タイマ１割込み許可
	enable_interrupts(GLOBAL);	
	
	while(true)
	{
		if(kbhit()){
			command = getc();
			
			switch(command){
			case 'a' :				//左回転
				motor1_foward();
				motor2_stop();
				break;
			
			case 's' :				//後退
				motor1_back();
				motor2_back();
				break;
				
			case 'd' :				//右回転
				motor1_stop();
				motor2_foward();
				break;
			
			case 'w' :				//前進
				motor1_foward();
				motor2_foward();
				break;
				
			case 'q' :				//停止
				motor1_stop();
				motor2_stop();
				motor3_stop();
				break;
				
			case 'z' :				//左急回転
				motor1_foward();
				motor2_back();
				break;
				
			case 'c' :				//右急回転
				motor1_back();
				motor2_foward();
				break;
				
			case 'e' :				//クラッカー引く
				motor3_foward();
				break;
			
			case 'x' :				//クラッカー戻す
				motor3_back();
				break;
				
			case '9' :				//LED明るさ
				led_bright += 127;
			case '8' :				//LED明るさ
				led_bright += 64;
			case '7' :				//LED明るさ
				led_bright += 32;
			case '6' :				//LED明るさ
				led_bright += 16;
			case '5' :				//LED明るさ
				led_bright += 8;
			case '4' :				//LED明るさ
				led_bright += 4;
			case '3' :				//LED明るさ
				led_bright += 2;
			case '2' :				//LED明るさ
				led_bright += 1;
			case '1' :				//LED明るさ
				led_bright += 1;
			case '0' :				//LED消灯
				set_pwm1_duty(led_bright);
				led_bright = 0;
				break;
				
				
			case 'r' :				//LED赤
				output_high(LED_R);
				output_low(LED_G);
				output_low(LED_B);
				break;
			
			case 't' :				//LED緑
				output_low(LED_R);
				output_high(LED_G);
				output_low(LED_B);
				break;
				
			case 'y' :				//LED青
				output_low(LED_R);
				output_low(LED_G);
				output_high(LED_B);
				break;
				
			case 'u' :				//LEDシアン
				output_low(LED_R);
				output_high(LED_G);
				output_high(LED_B);
				break;
			
			case 'i' :				//LEDマゼンダ
				output_high(LED_R);
				output_low(LED_G);
				output_high(LED_B);
				break;
				
			case 'o' :				//LED黄
				output_high(LED_R);
				output_high(LED_G);
				output_low(LED_B);
				break;
			
			case 'p' :				//LED白
				output_high(LED_R);
				output_high(LED_G);
				output_high(LED_B);
				break;
				
			case '?' :				//距離を送信
				printf("%Lu\r\n",distance);
				break;
			default :
				break;
			}
		}
	}

}