|
// 26k0312rx2m.c
// 赤外線受信機2モータ制御 PIC12F629用 2006.3.12
// 入力??GP3:赤外線受光素子の出力
// 出力??GP0:右モータ、GP1:なし、GP2:左モータ、GP4:なし、GP5:なし
#include<12f629.h>
#fuses INTRC_IO,NOWDT,NOMCLR,NOPUT,NOBROWNOUT,NOPROTECT
#use delay(clock=4000000)
#define ON 1
#define OFF 0
#define HI 1
#define LOW 0
#byte GP=5
#bit IRSIG=GP.3
#bit rmotor=GP.0
#bit lmotor=GP.2
int rcycl_cnt,lcycl_cnt,rduty_set,lduty_set;
#int_rtcc
void tmr0_warikomi()
{
set_timer0(231);
if(rcycl_cnt < rduty_set)
rmotor=ON;
else
rmotor=OFF;
if(lcycl_cnt < lduty_set)
lmotor=ON;
else
lmotor=OFF;
rcycl_cnt++;
rcycl_cnt %=20;
lcycl_cnt++;
lcycl_cnt %=20;
}
//
//メイン関数
//
main(){
int i,gcount,el_center,el_init,th_init,el_count;
signed int rmix,lmix,th_low,th_count;
//デジタル入力切り替え
setup_comparator(NC_NC);
GP=0;
set_tris_a(0x08);//GP3入力、他は出力
rduty_set=rcycl_cnt=0;
lduty_set=lcycl_cnt=0;
setup_timer_0(RTCC_INTERNAL | RTCC_DIV_2);
set_timer0(231);
enable_interrupts(INT_RTCC);
enable_interrupts(GLOBAL);
el_init=OFF;
th_init=OFF;
th_low=0;
rmotor=OFF;
lmotor=OFF;
rmix=0;
lmix=0;
delay_ms(1000);
while(1){//ギャップ検出ルーチン
gcount=0;
while(gcount < 30){
if(IRSIG==HI){
delay_us(100);
gcount++;
}
else{
gcount=0;
}
}
While(IRSIG==HI){
}
// 1ch(エルロン):未使用
el_count=0;
while(IRSIG==LOW){
delay_us(50);
el_count++;
}
while(IRSIG==HI){
delay_us(50);
el_count++;
}
if(el_init==OFF){
el_center=el_count;
el_init=ON;
}
if(el_count > el_center+3){
lmix=(el_count-el_center)/2;
// rmix=0;
rmix=-lmix;
}
else if(el_count < el_center-3){
rmix=(el_center-el_count)/2;
// lmix=0;
lmix=-rmix;
}
else{
rmix=0;
lmix=0;
}
// 2ch(エレベータ):未使用
while(IRSIG==LOW){
}
while(IRSIG==HI){
}
// 3ch(スロットル)
th_count=0;
while(IRSIG==LOW){
delay_us(50);
th_count++;
}
while(IRSIG==HI){
delay_us(50);
th_count++;
}
if(th_init==OFF){
th_low=th_count;
th_init=ON;
}
th_count=th_count-th_low;
if(th_count <= 2){
th_count=0;
}
else if(th_count >= 16){
th_count = 20;
}
// rduty_set=th_count;
// lduty_set=th_count;
rduty_set=th_count+rmix;
lduty_set=th_count+lmix;
if(rduty_set < 0)
rduty_set=0;
if(lduty_set < 0)
lduty_set=0;
// 4ch(ラダー)
while(IRSIG==LOW){
}
while(IRSIG==HI){
}
}
}
|
|