/********************************************************/
/* main program */
/* */
/* Ver No, date written by */
/* 2.1 1996.02.13 yohey kinosita */
/********************************************************/
#include"hard.h"
void move(); /* for test */
void re_test(); /* for test */
void stop(); /* for test */
extern lre_r, lre_l;
int dip;
int duty_r, duty_l;
int re_r, re_l;
void inzt00()
{
sys0(0, 0x1010);
sys2(0, 10, 0, 0);
sys8(0, 3, 0); /* dip */
}
void nrmt00(a,b)
int a,b;
{
/* No normal task */
}
void timt00(a,b)
int a,b;
{
RE_TS_DATA re_ts;
int t0_1,t0_3,t1_1,t1_2,t2_1,t2_2,t2_3,t3_1,t4_1,t4_2,t4_3,t5_1;
int durty_r,durty_l,sss_l,sss_r,sss_c,enc_r,enc_l;
static int sss_l_old = 0;
static int sss_r_old = 0;
static int sss_c_old = 0;
static int flag1,flag2,flag3,flag4,f,touch,count;
long lr_r, lr_l;
dip = sys9(0, 3);
duty_r = sys9(1, 1);
duty_l = sys9(1, 2);
re_r = sys9(2, 1);
re_l = sys9(2, 2);
t0_1 = sys9(0,1); /* mode data */
t0_3 = sys9(0,3); /* dipsw_data*/
t2_1 = sys9(2,1); /* encorder_right[cm] */
t2_2 = sys9(2,2); /* encorder_left[cm] */
t2_3 = sys9(2,3); /* touch_data */
t3_1 = sys9(3,1); /* ir_data */
t4_1 = sys9(4,1); /* sss_left[cm] */
t4_2 = sys9(4,2); /* sss_center[cm] */
t4_3 = sys9(4,3); /* sss_right[cm] */
t5_1 = sys9(5,1); /* power ON or OFF*/
/* test */
if(dip >= 0x02){
if(((dip >= 0x02) && (dip <= 0x08)) || (dip >= 0x0d))
duty_r = duty_l = 0;
if(dip == 0x09)
move(); /* front - back - right - left */
if(dip == 0x0c)
re_test(); /* turn right - left */
sys8(1, 1, duty_r);
sys8(1, 2, duty_l);
}
if(t0_3 == 0) /* dip = 0 */
{
durty_r = 70;
durty_l = 70;
lr_r = lre_r;
lr_l = lre_l;
if(lre_r<0)lre_r = -lre_r;
if(lre_l<0)lre_l = -lre_l;
flag1 = 0;
/* super sonic senser */
sss_l = t4_1 - sss_l_old ;
sss_c = t4_2 - sss_c_old ;
sss_r = t4_3 - sss_r_old ;
if((sss_l != 0)&&(flag2 != 1)) /* sss_left ON */
{
sss_l_old = t4_1;
if((sss_l > 25) || (sss_l < -25)){durty_l = -20;flag1 = 1;f = 1;}
if(f == 1)
{
durty_l = -20;
if((sss_l > 30) || (sss_l < -30)) f=0;
}
}
if((sss_r != 0)&&(flag2 != 1)) /* sss_right ON */
{
sss_r_old = t4_3 ;
if((sss_r > 25) || (sss_r < -25)){durty_r = -20;flag1 = 1;f = 1;}
if(f == 1)
{
durty_r = -20;
if((sss_r > 30) || (sss_r < -30))f=0;
}
}
if((sss_c != 0)&&(flag2 != 1)) /* sss_center ON */
{
sss_c_old = t4_2 ;
if((sss_c > 25) || (sss_c < -25)){durty_r = durty_l = 80; flag1 = 1;f=1;}
if(f == 1)
{
durty_l = durty_r = 80;
if((sss_c > 30) || (sss_c < -30))f=0;
}
}
/* infrared ray */
if((t3_1 != 0)&&(flag2 != 1)) /* ir ON */
{
flag1 = 1;
if(t3_1 == 8 ) /* ir right ON */
{durty_r = 0 ;}
if(t3_1 == 128 ) /* ir left ON */
{durty_l = 0 ;}
if(t3_1 == 16 ) /* ir center_right ON */
{durty_r = 10 ;}
if(t3_1 == 64 ) /* ir center_left ON */
{durty_l = 10 ;}
if(t3_1 == 32 ) /* ir center ON */
{durty_r = durty_l = 80 ;}
}
/* touch data */
if(((t2_3 != 0) && (flag1 == 0))||(flag2 == 1)) /* if touch ON and other sencer OFF */
{
if(t2_3 != 0) {touch = t2_3;lre_r = lre_l = 0;}
if(lre_r < 2500) /* if touch ON , back 10[cm] */
{
durty_r = -60;
durty_l = -60;
flag2 = 1; /* This is flag of touch mode */
}
if(lre_r > 2500)flag3 = 1; /* This is back 10[cm] end flag*/
if((touch == 1)&&(flag3 ==1 )) /* center ON and back end*/
{
if(lre_r < 9000) /* turn +180 */
{
durty_r = -60;
durty_l = 60;
flag4 = 1; /* center ON flag */
}
}
if(((touch == 2)||(touch == 3))&&(flag3 == 1)) /* right or center ON and back end */
{
if(lre_r < 5000) /* turn -90 */
{
durty_r = 60;
durty_l = -60;
flag4 = 2; /* right and center ON flag */
}
}
if(((touch == 4)||(touch == 5))&&(flag3 == 1)) /* left or center ON and back end */
{
if(lre_r < 5000) /* turn +90 */
{
durty_r = -60;
durty_l = 60;
flag4 = 3; /* left and center ON flag */
}
}
if((lre_r > 9000)&&(flag4 == 1))
{
flag1 = 0;
flag2 = 0;
flag3 = 0;
flag4 = 0;
lre_r = lre_l = 0;
} /* touch mode end */
if((lre_r > 5000)&&(flag4 == 2))
{
flag1 = 0;
flag2 = 0;
flag3 = 0;
flag4 = 0;
lre_r = lre_l = 0;
} /* touch mode end */
if((lre_r > 5000)&&(flag4 == 3))
{
flag1 = 0;
flag2 = 0;
flag3 = 0;
flag4 = 0;
lre_r = lre_l = 0;
} /* touch mode end */
}
if(((durty_r != sys9(1,1))||(durty_l != sys9(1,2)))&&(flag2 != 1))
{
while(count < 1000)
{
count++;
sys8(1,1,durty_r);
sys8(1,2,durty_l);
pwm_data(durty_r,-durty_l);
}
count=0;
}
sys8(1,1,durty_r); /* write durty_r */
sys8(1,2,durty_l); /* write durty_l */
if(lre_r == 0)lr_r = 0;
if(lre_l == 0)lr_l = 0;
lre_r = lr_r;
lre_l = lr_l;
}
}
void intt00(a,b)
int a,b;
{
}
void move() /* for test */
{ /* front - back - turn left 360[deg] - turn right 360[deg] - stop */
static int j = 0;
if(j == 0)
{
duty_r = duty_l = 30;
if((re_r >= 30) || (re_l >= 30))
{
stop();
re_r = re_l = 0;
lre_r = lre_l = 0;
duty_r = duty_l = -30;
j++;
}
}
else if(j == 1)
{
duty_r = duty_l = -30;
if((re_r <= -30) || (re_l <= -30))
{
stop();
re_r = re_l = 0;
lre_r = lre_l = 0;
duty_r = 30; duty_l = -30;
j++;
}
}
else if(j == 2)
{
duty_r = 30; duty_l = -30;
if((re_r >= 78) || (re_l <= -78)) /* turn right 360[deg] */
{
stop();
re_r = re_l = 0;
lre_r = lre_l = 0;
duty_r = -30; duty_l = 30;
j++;
}
}
else if(j == 3)
{
duty_r = -30; duty_l = 30;
if((re_r <= -78) || (re_l >= 78)) /* turn left 360[deg] */
{
re_r = re_l = 0;
lre_r = lre_l = 0;
duty_r = duty_l = 0;
j++;
}
}
else if(j >= 4)
{
stop();
duty_r = duty_l = 0;
j = 0;
}
}
void re_test() /* for test turn 360[deg] */
{
long re_r_test;
static int j = 0;
int k;
if((j % 2) == 0){
duty_r = 25; duty_l = -25;
j++;
}
if((j % 2) == 1){
duty_r = -25; duty_l = 25;
j++;
}
if((re_r >= 78) || (re_r <= -78)){
stop();
re_r_test = lre_r / 100;
sys8(2, 4, re_r_test);
lre_r = lre_l = 0;
for(k = 0; k < 30000; k++){}
}
}
void stop() /* for test */
{
int k1, k2;
duty_r = duty_l = 0;
pwm_data(duty_r, -duty_l);
for(k1 = 0; k1 < 30000; k1++){
for(k2 = 0; k2 < 10; k2++){}
}
}