#include "m9402.h"
#include
/*#include "io.h"*/

k_mode_judge()
{
 extern unsigned char  pwm_mode;
 extern unsigned int mmi_iivseg;

 static int junban;

 int sw_data;
 int mmi_mod;
 int shouhai;
 void turn();		/*往復運�*/
 void vertical();	/*垂柱ｕ�*/
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 ;
 

   sw_data = inportb(PBDR0) & 0x0f;/*ＰＩＴ０ｂaポートｉ�４ｂｉｔｂｂ奄買Xイッチ*/

   switch(sw_data){
        case 0x08: turn();
                   mmi_mod=0000;            
	      break;

	case 0x09: pwm_mode=0xd0;
	           mmi_mod=0001;    	      
	      break;

	case 0x0a: pwm_mode=0x90;
	           mmi_mod=0002;    	      
	      break;

	case 0x0b: vertical();
	           mmi_mod=0003;    	          
	      break;

	case 0x0c: pwm_mode=0xf0;
	           mmi_mod=0004;    	      
	      break;

	case 0x0d: pwm_mode=0xe0; 
	           mmi_mod=0005;    	      
	      break;

	case 0x0e: pwm_mode=0xb0;
	           mmi_mod=0006;    	       
	      break;

	case 0x0f: pwm_mode=0xa0;
	           mmi_mod=0007;    
	      break;
        default:   pwm_mode=0x00;
                   mmi_mod=8989;

		          }
  if(sys9(0,3)>=500)
   {
    sys8(0,3,0);
    switch(junban){
      case 1: junban=2;
              mmi_iivseg=mmi_mod;
              break;
      case 2: mmi_iivseg=(1000+(unsigned int)abs(rot_position_i.y));
              junban=3;
              break;
      case 3: mmi_iivseg=(2000+(unsigned int)rot_position_i.x);
              junban=4;
              break;
      case 4: mmi_iivseg=(3000+(unsigned int)rot_position_i.theta);
              junban=1;
              break;
      default:junban=1;
              break;
                   }
    }

 
}



void turn() 
{ 
  extern unsigned char pwm_mode;
  static int pb_old;
  int pb;
  static int ts_old;
  int ts;
  int p_m;

	 pb = (inportb(PBDR0) & 0x80);/*data of PB*/
         ts = (inportb(EDRH)  & 0x80);/*data of SF*/
     if(((pb == 0x80) && (pb_old==0x00))||((ts == 0x80) &&(ts_old==0x00)))	/*if button is pushed*/
	  {
           p_m=pwm_mode;
	   switch(p_m){
	         case 0x68: pwm_mode=0x00;
		       break;
                 case 0x00: pwm_mode=0x48;
                       break;
		 case 0x48: pwm_mode=0x20;
		       break;
                 case 0x20: pwm_mode=0x68;
                       break;
		 default:   pwm_mode=0x68;
		       break;
			           }
  	  }
       pb_old=pb;
       ts_old=ts;
}


void vertical()
 {
  extern unsigned char pwm_mode;
  extern int initial[MAXTSK];
  extern int inportb();
  extern struct pos{
   int x ;           /* cm */
   int y ;           /* cm */
   int v ;           /* cm/sec */
   int theta ;       /* deg */
   int thetav ;      /* deg/sec */
   int way ;         /* cm */
                  }
                   rot_position_i;
  int th;
  int ts;
  static int th_old;
  static int ts_o;
  static int vflag;
if(initial[8]!=0)
  {
   vflag=1;
   th_old=(rot_position_i.theta % 90);
   ts_o= 0;
  }

  ts = (inportb(EDRH)  & 0x80);/*data of SF*/

 if((ts==0x80)&&(ts_o==0x00))
    vflag=0;

     th = (rot_position_i.theta % 90);
  if(vflag==0)
   {
    if(((th-th_old)<=40)&&((th-th_old)>=-40))
      	   pwm_mode=0x62;
       else 
          {
           vflag=1;
           pwm_mode=0x00 ;
          }
    }
  initial[8]=0;
  ts_o=ts;
  th_old=th;
 }