#include"m9402.h"
int t_k_flag;
int s_flag;
void mode_judge()
{
extern int initial[MAXTSK];
extern int mode_num;
static int ss_ts_o;
int ss_ts;
void ss_s();
void ir_s();
void ir_c();
void kaihi();
void t_kaihi();
extern int initial[MAXTSK];
if(initial[0]==10)
{
t_k_flag=1;
ss_ts_o=sys9(3,1);
initial[0]=1;
}
ss_ts=sys9(3,1);
if((ss_ts!=0)&&(ss_ts_o==0))
{
initial[0]=5;
t_k_flag=0;
}
ss_ts_o=ss_ts;
if(t_k_flag==0)
t_kaihi();
else
{
if(sys9(1,1)!=0)
mode_num=3;
switch(mode_num)
{
case 1: ss_s();
break;
case 2: ir_s();
break;
case 3: ir_c();
break;
case 4: kaihi();
break;
default:mode_num=1;
break;
}
}
}
unsigned long ir_s_count;
void ss_s()
{
extern unsigned char pwm_mode;
extern unsigned int mmi_iivseg;
extern int initial[MAXTSK];
extern int mode_num;
extern int s_flag;
extern unsigned long Sysclk;
extern struct pos
{
int x ; /* cm */
int y ; /* cm */
int v ; /* cm/sec */
int theta ; /* degree */
int thetav ; /* degree/sec */
int way ; /* cm */
}
rot_position_i ;
static unsigned char s_pwm_mode;
static unsigned int s_mmi;
static int th_o;
int x,y,t;
int th;
mmi_iivseg=1000;
if(initial[0]==1)
{
th_o=rot_position_i.theta % 90;
}
initial[0]=0;
x=rot_position_i.x;
y=rot_position_i.y;
t=rot_position_i.theta;
if( ((t % 90)<=5)||((t%90)>=85))
pwm_mode=0x60;
else{
if((t % 90)>=45 )
pwm_mode=0xb0;
else if((t % 90)<=45)
pwm_mode=0xf0;
}
if((sys9(2,5)&0x08)!=0)/*if left or right sss is hit*/
{
if((sys9(2,5)&0x04)==0x04)
{
s_pwm_mode=0xa8;
s_mmi=100;
s_flag=0;
}
else if((sys9(2,5)&0x01)==0x01)
{
s_mmi=1;
s_pwm_mode=0xe8;
s_flag=0;
}
else
{
s_mmi=10;
s_pwm_mode=0x68;
s_flag=1;
}
}
th = (rot_position_i.theta % 90);
if(s_flag==0)
{
if(((th-th_o)<=40)&&((th-th_o)>=-40))
{
pwm_mode=s_pwm_mode;
mmi_iivseg+=s_mmi;
}
else
s_flag=1;
}
else if((sys9(2,2)<=40)&&(sys9(2,2)!=0))/*if forward sss is 30cm and not overflow*/
{
if( ((absl(t-270)<=35)&&((y-0)<=60)) ||
((absl(t-180)<=35)&&((x-0)<=60)) ||
((absl(t-90)<=35)&&((250-y)<=60))||
(((t<=35)||(t>=325))&&((250-x)<=60)) )
{
mode_num=4; /* if wall, go to kaihi*/
s_flag=0;
initial[0]=2;
}
else
{
mode_num=2;
ir_s_count=Sysclk;
initial[0]=3;
}
}
th_o=th;
}
void ir_s()
{
extern unsigned char pwm_mode;
extern unsigned int mmi_iivseg;
extern unsigned long ir_s_count;
extern unsigned long Sysclk;
static unsigned char ir_pwm_mode;
static int ir_th_o;
static unsigned long ir_s_mask_count;
static int ir_mmi;
static int ir_s_c;
static int ir_r_p_th_o;
int it_r_p_th;
int d;
int th;
if(initial[0]==3)
{
ir_th_o=(rot_position_i.theta % 90);
ir_r_p_th_o=rot_position_i.theta;
ir_s_mask_count=0;
}
initial[0]=0;
th=(rot_position_i.theta % 90);
mmi_iivseg=2000;
if(Sysclk<(ir_s_count+7000))
{
if((sys9(2,5)!=0)&&(ir_s_mask_count<=Sysclk))
{
if((sys9(2,5)&0x04)!=0)
{
ir_pwm_mode=0x62;
ir_mmi=1;
}
if((sys9(2,5)&0x01)!=0)
{
ir_pwm_mode=0x63;
ir_mmi=100;
}
if((sys9(2,5)&0x02)!=0)
{
ir_pwm_mode=0x00;
ir_mmi=10;
}
s_flag=0;
}
else
pwm_mode=0;
if(s_flag==0)
{
if(((th-ir_th_o)<=40)&&((th-ir_th_o)>=-40))
{
pwm_mode=ir_pwm_mode;
mmi_iivseg+=ir_mmi;
ir_s_count=(Sysclk-3500);
}
else if((absl(ir_r_p_th_o-th)>=330)||(absl(ir_r_p_th_o-th)<=40))
{
ir_r_p_th_o=rot_position_i.theta;
}
else
{
ir_s_mask_count=(Sysclk+750);
ir_s_count=(Sysclk-3500);
s_flag=1;
}
}
}
else
{
mode_num=4;
initial[0]=2;
s_flag=0;
}
ir_th_o=th;
}
void ir_c()
{
int ir_dt;
extern int mode_num;
static unsigned long ir_c_count;
extern unsigned long Sysclk;
extern unsigned char pwm_mode;
extern struct pos
{
int x ; /* cm */
int y ; /* cm */
int v ; /* cm/sec */
int theta ; /* degree */
int thetav ; /* degree/sec */
int way ; /* cm */
}
rot_position_i ;
int d;
mmi_iivseg=3000;
ir_dt=sys9(1,1);
if(ir_dt!=0)
{
if((ir_dt&0x38)==0x38)
pwm_mode=0x60;
else if((ir_dt&0xe0)!=0)
pwm_mode=0xe8;
else if((ir_dt&0x0e)!=0)
pwm_mode=0xa8;
ir_c_count=Sysclk;
}
else if((ir_c_count+3000)= 85))
{
mode_num=1;
s_flag=1;
initial[0]=1;
}
else
pwm_mode=0x62;
}
d = (ir_dt & 0xc0);
switch (d) {
case 0x40: mmi_iivseg += 1;
break;
case 0x80: mmi_iivseg += 2;
break;
case 0xc0: mmi_iivseg += 3;
break;
}
d = (ir_dt & 0x06);
switch (d) {
case 0x04: mmi_iivseg += 100;
break;
case 0x02: mmi_iivseg += 200;
break;
case 0x06: mmi_iivseg += 300;
break;
}
d = (ir_dt & 0x38);
switch (d) {
case 0x10: mmi_iivseg += 10;
break;
case 0x20: mmi_iivseg += 20;
break;
case 0x08: mmi_iivseg += 30;
break;
case 0x30: mmi_iivseg += 40;
break;
case 0x18: mmi_iivseg += 50;
break;
case 0x38: mmi_iivseg += 60;
break;
}
}
void kaihi()
{
extern unsigned char pwm_mode;
extern unsigned int mmi_iivseg;
extern struct pos
{
int x ; /* cm */
int y ; /* cm */
int v ; /* cm/sec */
int theta ; /* degree */
int thetav ; /* degree/sec */
int way ; /* cm */
}
rot_position_i ;
static int o_th1;
static int o_th;
static int o_th2;
static int k_flag;
mmi_iivseg=4000;
if(initial[0]==2)
{
o_th=(rot_position_i.theta+180)%360;
o_th1=o_th;
o_th2=o_th;
k_flag=1;
if((o_th+10)>=360)
{
o_th1-=360;
k_flag=0;
}
if((o_th-40)<=-1)
{
o_th2+=360;
k_flag=0;
}
}
initial[0]=0;
if(s_flag==0)
{
if(k_flag==0)
{
if(((o_th1+10)rot_position_i.theta))
pwm_mode=0xa8;
else
{
initial[0]=1;
mode_num=1;
s_flag=1;
}
}
if(k_flag==1)
{
if(((o_th1+20)rot_position_i.theta))
pwm_mode=0xa8;
else
{
initial[0]=1;
mode_num=1;
s_flag=1;
}
}
}
}
void t_kaihi()
{
extern struct pos
{
int x ; /* cm */
int y ; /* cm */
int v ; /* cm/sec */
int theta ; /* degree */
int thetav ; /* degree/sec */
int way ; /* cm */
}
rot_position_i ;
extern unsigned char pwm_mode;
extern unsigned int mmi_iivseg;
extern int t_k_flag;
static unsigned int t_k_way;
mmi_iivseg=5000;
if(initial[0]==5)
t_k_way=rot_position_i.way;
initial[0]=0;
if((t_k_way+10)>rot_position_i.way)
pwm_mode=0x40;
else
t_k_flag=1;
}
ŠJ”‹K–ñ‚É–ß‚é