#include"m9402.h"
int t_k_flag;
int s_flag;
void p_mode_judge()
{
extern int initial[MAXTSK];
extern int mode_num;
static int ss_ts_o;
int ss_ts;
extern void ss_s();
extern void p_ir_s();
extern void ir_c();
extern void kaihi();
extern 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: p_ir_s();
            break;
    case 3: ir_c();
            break;
    case 4: kaihi();
            break;
    default:mode_num=1;
            break;
   }
 }
}

unsigned long ir_s_count;

void p_ir_s()
{
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;
 static int p_ir_th_o;
 static int p_ir_mmi;
 static int p_ir_s_c;
 static int p_rot_posi_th;
 static char p_flag;
 int th;

if(initial[0]==3)
 {
  p_ir_th_o=(rot_position_i.theta % 90);
  p_flag=1;
  p_rot_posi_th=rot_position_i.theta;
  mmi_iivseg=1111;
 }
initial[0]=0;

th=(rot_position_i.theta % 90);
  if(p_flag==1)
   {
    if(((th-p_ir_th_o)<=40)&&((th-p_ir_th_o)>=-40))
          {
               pwm_mode=0x62;
           }
       else if(absl(p_rot_posi_th-rot_position_i.theta)<=20)
          initial[0]=3;
          
          else
             p_flag=2;
   }
  if(p_flag==2)
   {
    mmi_iivseg=2222;
    pwm_mode=0xf8;
   }
 p_ir_th_o=th;
}