ls.c

使用MC9S12DG128芯片

#include "includes.h"

INT8U StartFlag = OFF;

INT8U ReachBalance = OFF;

INT32S CarLocationtemp = 0;

/*void LsSecondSub(void) {
  
   
   INT8U  CarDirect;
   
   INT16U AngleTemp1;
   
   Movement(4);
   
   while(1) {
   
     if (StartFlag == ON) {
      
     StartFlag = OFF;
     
     CarDirect = SCA_Angle(&AngleTemp1);
     
     if (CarDirect == CarBack) {Movement(-2);} 
     
     else Movement(2);
     
     
     }
     
     
     if (ReachBalance == ON) break;
  }
   
   ReachBalance = OFF;
   
   PORTB = 0xFB;
   
   FOREVER();
      

}    

void Movement(INT8S Step,INT8S Speed) {

  INT32U Temp = CarLocation;

  if(Step >= 0) {
    
     for(;Step>0;Step--) {
      
         MDirect = Front;
         
         Temp = Temp + 3;
         
         
     
         while(CarLocation < Temp){SmartMoto(Speed);};
     
         StopMoto();
  
         Dly5sec();
         
         Temp = CarLocation;
     }
  
  }
  
  else {
  
     for(;Step<0;Step++){
           
      MDirect = Back;
  
      Temp = Temp - 3;
      
      
     
      while(CarLocation > Temp){SmartMoto(Speed);};
     
      StopMoto();
  
      Dly5sec();
      
      Temp = CarLocation;
      
     }
    
  }
     
    
  
} */

void ExtSecond(void) {
  
}          


void Movement(INT8S Step,INT8S Speed)
{
   //INT16U CarLocationtemp;
   
   //CarLocationtemp  = CarLocation;
   
   if(Step>0)
   {
     CarLocationtemp  = CarLocation+Step;
     
     MDirect  = Front;
     
     SmartMoto(Speed);
     
     while(CarLocationtemp >= CarLocation)
     {
       SmartMoto(Speed);
     }
   
     StopMoto();
     
   }
   else if(Step<0)
   {
     CarLocationtemp  = CarLocation-(-Step);
     
     MDirect  = Back;
     
     SmartMoto(Speed);
     
     while(CarLocationtemp <= CarLocation)
     {
       SmartMoto(Speed);
     }
   
     StopMoto();
     
   }
}


void StopMoto(void)
{
  if (MDirect==Front)
  {
    StopPWM7();
    Moto(-7);
    AngleFlag = OFF;
    TimeCount3 = 800;
    while(CarStop==ON) {
     
        if (AngleFlag == ON) break;
      
    }
    StopPWM6();
  }
  else
  {
    StopPWM6();
    Moto(7);
    AngleFlag = OFF;
    TimeCount3 = 800;
    while(CarStop==ON) {
     
        if (AngleFlag == ON) break;
      
    }
    StopPWM7();
  }
  
  StopPWM7();
  StopPWM6();
  
}  


INT8U CheckBalance(void)
{
  INT16S CDeg,Cha,Max,Min;
  
  INT16S DataAngle[10];
  
  INT8U i;
  
  PORTB = ~PORTB;
  
  for (i=0;i<10;i++)
  {
      
  Delay(30);
  
  DataAngle[i] = ReadADC(0);
  
  }
  
  Max = DataAngle[0];
    
  for (i=1;i<10;i++)
  {
    if (DataAngle[i] >= Max) Max = DataAngle[i];
  }
  
  Min = DataAngle[0];
  
  for (i=1;i<10;i++)
  {
    if (DataAngle[i] <= Min) Min = DataAngle[i];
  }
    
  CDeg  = (Max + Min)/2 -  BalanceAngle;
  
  
  if(CDeg>=-10 && CDeg <=10)
  {
    return TRUE;
  }
  else
  {
    return FALSE;
  }
}