hyj.c

使用MC9S12DG128芯片

#include "includes.h"

INT16U BanlanceAngel;

INT16U BoardAngle;

void SecondSub(void)      
{
  
  CurrentTime = 0;

  
/********************************************/
  for(;;)
  {
                                               
  Balance();
  
  if(TRUE==CheckBalance())break;
  }
  
 PORTB = 0xAA;
 
 Delay(500);

/*********************************************/  
  
   
  
  DisplayTDF(2);
  
  //PORTB= 0b01100110;
  
  //Moto(35);
  
  //FOREVER();
  
}
  
void ThirdSub (void)
{
  
  CurrentTime = 0;
  
  StartHW;
  
  MDirect = Front;
  
  CarLocation=158;      //消除积累误差.
  
  SmartMoto(25);
  
  while(CarLocation<=230)
  {
    Hwsam();
    SmartMoto(0);
  }
  
  StopMoto();
  
  DisplayTDF(3);
  
}

void FourthSub (void)
{
  CurrentTime = 0;
  
  //Dly5sec();
  
  Delay(500);
  
  StartHW;
  
  MDirect = Back;
  
  SmartMoto(-30);

  while(CarLocation>80)
  {
    RHwsam();
    //SmartMoto(0);

  }
   
  StopMoto();
  
  DisplayTDF(4);

  for(;;);                        //第一部分结束 
   
}

  

void Dly5sec(void)
{
  
  INT32U Dytime;
  
  for(Dytime=0;Dytime<=100000;Dytime++);
  
}

void Delay(INT16U dytime) {
  
  AngleFlag = OFF;
  
  TimeCount3 = 4*dytime;
  
  while(AngleFlag==OFF) {
  };
}



INT8U RHwsam(void)
{
  INT8U Hwrd;
  
  INT8S Hwcode;
  
  DDRH  = 0x0f;
  
  _NOP_;
  
  _NOP_;
  
  Hwrd  = PTIH;
  
  Hwrd  = Hwrd & 0b01100000;  //onle connect two redray;
  
  switch(Hwrd)
  {
    case 0b10000000:
    
      Hwcode  = -6;         
      
      Servo2deg(2000);
      
      break;
      
    case 0b11000000:
    
      Hwcode  = -4;
      
      Servo2deg(1000);
      
      break;
      
    case 0b01000000:
    
      Hwcode  = -2;
      
      Servo2deg(200);
      
      break;
      
    case 0b01100000:
    
      Hwcode  = 0;
      
      Servo2deg(0);
      
      break;
      
    case 0b00100000:
    
      Hwcode  = 2;
      
      Servo2deg(-200);
      
      break;
      
    case 0b00110000:
    
      Hwcode  = 4;
      
      Servo2deg(-1000);
      
      break;
      
    case 0b00010000:
    
      Hwcode  = 6;
      
      Servo2deg(-2000);
      
      break;
      
    default:
    
      Hwcode  = 127;
  }
  
    return (Hwcode);
  
} 


void ExtFirst(void) 
{
  
  INT8U LcdExt1[] = {"the process of The Second Sub "};
  
  INT8U LcdExt2[] = {"have go up to the black line  "};
  
  INT8U LcdExt3[] = {"the first has found           "};
  
  INT8U LcdExt4[] = {"the second has found          "}; 
    
    
  LcdWriteAll(LcdExt1);  
  
  
  StartHW;
  
  
  MDirect = Front;
  
  
  //SmartMoto(20);
  
  //Moto(30);
  
  DDRB=0xFF;
  
  while(CarLocation <= 291)  /////////////////////////////
  {
    
    
    HwsamSearch();                  //采样红外值并且控制舵机转向
    SmartMoto(25);
    //PORTB = PTIH;
    
    Delay(5);
    
    
    //PORTB = PTIH;
  
  };
  
  StopMoto();
  
 Servo2deg(0);

 
  LcdWriteAll(LcdExt2);
  
  for(;;)
  {
  Hwsam();
                                               
  Balance();
  
  if(TRUE==CheckBalance())break;
  }
  
 PORTB = 0xAA;
 

 StopMoto();
  
LcdWriteAll(LcdExt3); 

for(;;)
{
  if(FALSE  == CheckBalance())break; 
  
}

PORTB = 0x0F;

  for(;;)
  {
  Hwsam();
                                               
  Balance();
  
  if(TRUE==CheckBalance())break;
  } 
  
  PORTB = 0xAA;
  
  LcdWriteAll(LcdExt4);
  
  Delay(300);
  
      //结束
  
}



  

INT8U HwsamSearch(void)
{
  INT8U Hwrd;
  
  INT8S Hwcode;
  
  DDRH  = 0x0f;
  
  _NOP_;
  
  _NOP_;
  
  Hwrd  = PTIH;
  
  Hwrd  = Hwrd & 0b11110000;  //onle connect two redray;
  
  switch(Hwrd)
  {
    case 0b10000000:
    
      Hwcode  = -6;         
      
      Servo2deg(-3000);
      
      break;
      
    case 0b11000000:
    
      Hwcode  = -4;
      
      Servo2deg(-2000);
      
      break;
      
    case 0b01000000:
    
      Hwcode  = -2;
      
      Servo2deg(-250);
      
      break;
      
    case 0b01100000:
    
      Hwcode  = 0;
      
      Servo2deg(0);
      
      break;
      
    case 0b00100000:
    
      Hwcode  = 2;
      
      Servo2deg(250);
      
      break;
      
    case 0b00110000:
    
      Hwcode  = 4;
      
      Servo2deg(2000);
      
      break;
      
    case 0b00010000:
    
      Hwcode  = 6;
      
      Servo2deg(3000);
      
      break;
      
 //   case 0b00000000:
    
//      Hwcode  = 50;
      
  //    StopMoto();
      
    //  break;
      
    default:
    
      Hwcode  = 127;
  }
  
    return (Hwcode);
  
}








  
void Balance (void)
{
    INT16S Dec1,Dec2,Dec1Max,Dec2Max;
    
    INT16S Delta;
    
    Delay(10);       //延时0.1S
    
    
    
    
    Dec1  = ReadADC(0);
    
    Delta = Dec1 - BalanceAngle;
    
    Delay(100);
      
    while(Delta<=-10 || Delta>=10)
    
    {
      
      if (Delta>0) Movement(2,49);
      
      else if (Delta<0) Movement(-2,-49);    //
      
      else return ;
      
      Delay(100);

      Dec1  = ReadADC(0);
    
      Delta = Dec1 - BalanceAngle;
      
      
    }
        
    
    
    
    
    
    
    
  //  BoardAngle  = ReadADC(0);    
    
/*    do
    {
      Movement(2,49);
      
      Delay(10);
    
      Dec2  = ReadADC(0);
    }
    
    while((Dec2-BoardAngle>=-10) && (Dec2-BoardAngle<=10));
    
    PORTB = 0b11111110;
    
                  
CB: do
    {
      Delay(20);
      
      Dec1  = Dec2;
      
      Dec2  = ReadADC(0);
      
      Delta = Dec2-Dec1;
    }
    
    while(Delta<-1 || Delta>1);
    
    Dec1Max = Dec2;
    
    PORTB = 0b11111100;
    
    do
    {
      Delay(20);
      
      Dec1  = Dec2;
      
      Dec2  = ReadADC(0);
      
      Delta = Dec2-Dec1;
    }
    
    while(Delta<-1 || Delta>1);
    
    Dec2Max = Dec2;
    
    Delta=(Dec1Max+Dec2Max)/2;
    
    PORTB = 0b11111000;
    
    ////////////
    
    Delta=Delta-BalanceAngle;
    
    if(Delta>0)          //后退
    {
      Movement(2,49);
      
      
      goto CB;
    }
    else if(Delta<0)   //前进
    {
      
      Movement(-2,-49);
      goto CB;
    }
    else 
    {
      //CheckBalance();
      DDRB  = 0xFF;
      PORTB = 0b01100110;
      
      FOREVER();
      
    }
  */

}