#include <reg51.h>
#include <math.h>

#define POS1 218//(223-5)	//測試頭下落至磁環高度時的位置
#define POS2 51		//測試頭移至磁環正中心時的位置
#define POS3 142//(137+5)	//測試頭上升至磁環高度時的位置

#define TIME1 10	//脈沖之間的延時
#define TIME2 4
#define TIME3 3
#define TIME4 16
#define TIME5 19
#define TIME6 23
#define TIME7 28
#define TIME8 35

#define NUM  9	//兩齒輪的比例

#define HIGH 1
#define LOW  0

sbit PULSE = P3^7;
sbit SENS  = P3^0;
sbit LED = P3^1;

unsigned char pos;	//測試頭位置

void delay( unsigned char time )
{
	unsigned char i = time;
	while( --i );
}

void delays( )
{
	unsigned char i = 20;
	while( --i )
		delay( 255 );
}
//*********************************************************************
void step( )
{
	
/*************************************************
小撥盤前進一格（45度），
為防止出錯，發出的脈沖數最多可移動2格
轉動過程中檢查光耦以確定是否到位
檢查測試頭位置以確定撥盤是否還可以繼續轉動
64細分，轉動45度需1600個脈沖
分2步發送脈沖，第一步直接發送160個脈沖
第二步邊監測邊發脈沖，最多發3200個脈沖
*************************************************/
	unsigned int i, j;
	LED = HIGH;
	//第一步
	for ( i = 1500; i > 0; i -- )
	{
		for ( j = NUM; j > 0; j -- )
		{

			PULSE = HIGH;
			delay( TIME3 );
			PULSE = LOW;
			delay( TIME3 );
		}
	}

	//第二步
	for ( i = 20000; i > 0; i -- )
	{
		pos = P1;
        
	   //	if ( ( SENS == 0 ) || (( pos >= POS1 ) || ( pos < POS3 )) )
		//	break;
	//	for ( j = NUM; j > 0; j -- )
	//	{
	       if((SENS==0)||(pos>0&&pos<140)||(pos>216&&pos<255))  break;
			PULSE = HIGH;
			delay( TIME2 );
			PULSE = LOW;
			delay( TIME2);
		
	//	}
	}
/*
	//3
	for ( i = 160; i > 0; i -- )
	{
		pos = P1;
//		if ( ( SENS == 0 ) || (( pos >= POS2 ) && ( pos < POS3 )) )
		if ( ( SENS == 0 ) || (( pos >= POS1 ) || ( pos < POS3 )) )
//		if( SENS == 0 )
			break;
		for ( j = NUM; j > 0; j -- )
		{
		    if(SENS==0)   break;
			PULSE = HIGH;
			delay( TIME3 );
			PULSE = LOW;
			delay( TIME3 );
		}
	}
*/
/*	
	//防止丟步,補差
	for ( i = 160; i > 0; i -- )
	{
		pos = P1;
            //		if ( ( SENS == 0 ) || (( pos >= POS2 ) && ( pos < POS3 )) )
		if ( ( SENS == 0 ) || (( pos >= POS1 ) || ( pos < POS3 )) )
            //		if( SENS == 0 )
			break;
		for ( j = NUM; j > 0; j -- )
		{
		   if(SENS==0)  break;
			PULSE = HIGH;
			delay( TIME3 );
			PULSE = LOW;
			delay( TIME3 );
		}
	}
*/	
	LED = LOW;

}
//*********************************************************************************

/*
void test( )
{
	unsigned int i;
	while( 1 )
	{

			step( );
	}
}
*/
void init( )
{
	unsigned char i;
	pos = P1;
	while( ( pos == 0xff ) || ( pos == 0x00) )
	{
		pos = P1;
	}
	pos = P1;	
	i = pos;
	//判斷馬達是否上電
	while( pos == i )
	{
		delay( 25 );
		i = P1;
	}
	//空轉一周
	pos = P1;
	i = pos;
	while( pos == i )
	{
		delay( 25 );
		i = P1;
	}
	delays( );

	while( pos != i )
	{
		i = P1;
	}
	

}

void main( )
{
  //	test( );
    delays();
	delays();
	init( );
	pos = P1;
/*	while ( ( pos <= POS2 ) || ( pos >= POS3 ) )
	{
   //			delay( 25 );
		pos = P1;
	}   
	pos = P1;
	while ( ( pos <= POS2 ) || ( pos >= POS3 ) )
	{
   //			delay( 25 );
		pos = P1;
	}
*/
	while ( 1 )
	{
		//等待
		pos = P1;
       if((pos>=0)&&(pos<=140))              //from 216~255  and 0~140 moto can't move
          {
            pos=P1;
          }
       else if (( pos>=216 )&&( pos<=255 ))
           {
             pos=P1;
            }
       else if (( pos>140)&&(pos<216))
            {
                char w1;
                w1 = P1;
                delay(TIME1);
                pos = P1;
               if( (pos>w1 ) && ( pos < 160))   //限定一個比較窗口，注意160這個值不能太大，不然電機走不完測頭就下來了。
                   {
                          step();
                   }
               else if  (pos >196)
                        step();
			   while((pos>140)&&(pos<224))        //手搖時反轉也能步進一格
			   {
			      pos = P1;
				  delay(TIME1);
				}

             } 
                  
     }


}