vehicle.c

dsp2812小车控制

  }

}



void Stop()//直线加减速
//假设变速前都是直线运动
{
  //unsigned int cKey;
  //while(LeftIndex!=Maxstep)
  //{
  //  cKey=(LeftIndex<Maxstep)?0:1;
  //  StopCpuTimer0();
  //  LeftMotor(cKey,1);
  //  RightMotor(cKey,1);
  //  StartCpuTimer0();
  //}
    StopCpuTimer0();
    EvaRegs.T2CMPR=0x0F000;
    EvaRegs.CMPR3=0x0F000;
    EvaRegs.CMPR2=0x0F000;
    EvaRegs.CMPR1=0x0F000;
    StartCpuTimer0();
    RightIndex=7;
    LeftIndex=7;
  //0x0F000;
  //0x0F000;
  //0x0F000;
}

void LinearMotion(unsigned int cKey)//直线加减速
//假设变速前都是直线运动
//{
//  StopCpuTimer0();
//  LeftMotor(cKey,1);
//  RightMotor(cKey,1);
//  StartCpuTimer0();
//}
//获取当前速度
{
    unsigned int temp,temp1,IndexStep=1;//int ChangOrNot=0;
    StopCpuTimer0();  
    
  if((LeftIndex>=IndexStep)&(cKey==1))
  {
    LeftIndex-=IndexStep;
    //ChangOrNot=1;
  }
  else if((LeftIndex<=(2*Maxstep-IndexStep))&(cKey==0))//-IndexStep
  {
    LeftIndex+=IndexStep;  
    //ChangOrNot=1;
  }
  //if (ChangOrNot==1)
  { 
    temp=a[LeftIndex];
    temp1=a[2*Maxstep-LeftIndex];
    if (LeftIndex>Maxstep) 
    {
    	EvaRegs.CMPR3=temp; 
    	EvaRegs.T2CMPR=temp1+ajust_num;  
    }
    else if (LeftIndex<Maxstep)
    {
    	EvaRegs.CMPR3=temp+ajust_num; 
    	EvaRegs.T2CMPR=temp1;  
    }
    else
    {
    	EvaRegs.CMPR3=temp; 
    	EvaRegs.T2CMPR=temp1;  
    }
    EvaRegs.CMPR1=temp;
    EvaRegs.CMPR2=temp1;
  }
  
    RightIndex=LeftIndex;
    StartCpuTimer0();
  
}//end






//void LinearMotion2(unsigned int TafgetKey)//直线加减速
////假设变速前都是直线运动
//{
//  unsigned int cKey;
//  while(TafgetKey!=LeftIndex)
//  {
//    cKey=(TafgetKey>LeftIndex)?0:1;
//    StopCpuTimer0();
//    LeftMotor(cKey,1);
//    RightMotor(cKey,1);
//    StartCpuTimer0();
//    if(SciaRegs.SCIFFRX.bit.RXFIFST!=0)
//        break;
//  }
//}


void LeftMotor(unsigned int cKey,unsigned int IndexStep)//,unsigned int Index
{
//获取当前速度
    unsigned int temp;
  if((LeftIndex>=IndexStep)&(cKey==1))
  {
    LeftIndex-=IndexStep;
  }
  else if((LeftIndex<(2*Maxstep))&(cKey==0))//-IndexStep
  {
    LeftIndex+=IndexStep;
  }

  
  EvaRegs.CMPR1=a[LeftIndex];//EvbRegs.CMPR4=
  temp=2*Maxstep-LeftIndex;
  EvaRegs.CMPR2=a[temp];//EvbRegs.CMPR5=
  
}//end


void RightMotor(unsigned int cKey,unsigned int IndexStep)//,unsigned int Index
{
//获取当前速度
    unsigned int temp;
  if((RightIndex>=IndexStep)&(cKey==1))
  {
    RightIndex-=IndexStep;
  }
  else if((RightIndex<(2*Maxstep))&(cKey==0))//-IndexStep
  {
    RightIndex+=IndexStep;
  }

  
    EvaRegs.CMPR3=a[RightIndex];//EvbRegs.CMPR6=
    temp=2*Maxstep-LeftIndex;
    EvaRegs.T2CMPR=a[temp];//EvbRegs.T4CMPR=
  
}//end


void init_eva()
{

//EVAConfigureT1PWM,T2PWM,PWM1-PWM6
//Initalizethetimers
//InitalizeEVATimer1
EvaRegs.T1PR=0xF000;//Timer1period
EvaRegs.T1CMPR=0xF000;//Timer1compare
EvaRegs.T1CNT=0x0000;//Timer1counter
//TMODE=continuousup/down
//Timerenable
//Timercompareenable
EvaRegs.T1CON.all=0x1042;


//InitalizeEVATimer2
EvaRegs.T2PR=0xF000;//Timer2period
EvaRegs.T2CMPR=0xF000;//Timer2compare
EvaRegs.T2CNT=0x0000;//Timer2counter
//TMODE=continuousup/down
//Timerenable
//Timercompareenable
EvaRegs.T2CON.all=0x1042;


//SetupT1PWMandT2PWM
//DriveT1/T2PWMbycomparelogic
EvaRegs.GPTCONA.bit.TCMPOE=1;
//PolarityofGPTimer1Compare=Activelow/1
EvaRegs.GPTCONA.bit.T1PIN=1;
//PolarityofGPTimer2Compare=Activehigh/2
EvaRegs.GPTCONA.bit.T2PIN=1;

//EnablecompareforPWM1-PWM6
EvaRegs.CMPR1=0xF000;
EvaRegs.CMPR2=0xF000;
EvaRegs.CMPR3=0xF000;

//Compareactioncontrol.Actionthattakesplace
//onacmpareevent
//outputpin1CMPR1-activehigh
//outputpin2CMPR1-activelow
//outputpin3CMPR2-activehigh
//outputpin4CMPR2-activelow
//outputpin5CMPR3-activehigh
//outputpin6CMPR3-activelow
EvaRegs.ACTRA.all=0x0666;
EvaRegs.DBTCONA.all=0x0000;//Disabledeadband
EvaRegs.COMCONA.all=0xA600;
}

void init_evb()
{
//InitializeEVBTimer3
EvbRegs.T3PR=0xFFFF;//Timer3period
//EvbRegs.T3CMPR=0xF000;//Timer3compare
EvbRegs.T3CNT=0x0000;//Timer3counter
//TMODE=continuousup/down
//Timerenable
//TimercompareDISable
EvbRegs.T3CON.all=0x1540; 
    EvbRegs.CAPCONB.all = 0x0000;		// reset the capture units
//捕获控制寄存器
EvbRegs.CAPCONB.bit.CAP5EDGE=2;
EvbRegs.CAPCONB.bit.CAP4EDGE=2;//检测下降沿
EvbRegs.CAPCONB.bit.CAP45TSEL=1;//Timer3
EvbRegs.CAPCONB.bit.CAP45EN=1;//使能捕获

//CAPFIFOB为空
EvbRegs.CAPFIFOB.all=0;
//清空捕获中断标志位
EvbRegs.EVBIFRC.bit.CAP4INT=1;
EvbRegs.EVBIFRC.bit.CAP5INT=1;
//使能捕获中断
EvbRegs.EVBIMRC.bit.CAP4INT=1;
EvbRegs.EVBIMRC.bit.CAP5INT=1;

	PieCtrlRegs.PIEIER5.bit.INTx5 = 1;	// Enable CAPINT4 in PIE group 5
	PieCtrlRegs.PIEIER5.bit.INTx6 = 1;	// Enable CAPINT5 in PIE group 5	
	//IER |= 0x0010;						// Enable INT5 in IER to enable PIE group 5
}

void Delay(unsigned int nDelay)
{
  int ii,jj;
  for(ii=0;ii<nDelay;ii++)
  {
    for(jj=0;jj<50;jj++);//
  }
}

void Swap(unsigned int *a,unsigned int *b)
{
  unsigned int temp;
  temp=*a;
  *a=*b;
  *b=temp;
}


unsigned int capnum1;
unsigned int capnum2;
unsigned int pinlvf1[5]={0,0,0,0,0};
int fnum1=0;
interrupt void CAPINT4_ISR(void) // EV-A捕获中断子程序
{
    unsigned int temp;//unsigned int
    EvbRegs.EVBIFRC.bit.CAP4INT=1;// 清捕获中断1标志位
    //EvbRegs.EVBIFRC.bit.CAP5INT=1;// 清捕获中断1标志位
    capnum1=EvbRegs.CAP4FIFO;
    capnum2=EvbRegs.CAP4FIFO;
    if(capnum2 > capnum1)
        temp = capnum2-capnum1;
    else
        temp = capnum2+(0xFFFF-capnum1);
        pinlvf1[fnum1++] =temp-0x4000;
    if(fnum1>4) fnum1=0;//取5个数值后回零
    //f1= temp;//f= 150M/64/temp     2343750.0/(float)
    PieCtrlRegs.PIEACK.bit.ACK4 = 1;//开外设中断应答
}


unsigned int pinlvf2[5]={0,0,0,0,0};
int fnum2=0;
interrupt void CAPINT5_ISR(void) // EV-A捕获中断子程序
{
    unsigned int temp;
    EvbRegs.EVBIFRC.bit.CAP5INT=1;// 清捕获中断1标志位
    //EvbRegs.EVBIFRC.bit.CAP5INT=1;// 清捕获中断1标志位
    capnum1=EvbRegs.CAP5FIFO;
    capnum2=EvbRegs.CAP5FIFO;
    if(capnum2 > capnum1)
        temp = capnum2-capnum1;
    else
        temp = capnum2+(0xFFFF-capnum1);
        pinlvf2[fnum2++] =temp-0x4000;
    if(fnum2>4) fnum2=0;//取5个数值后回零
    //f2= temp;//f= 150M/64/temp     2343750.0/(float)
    PieCtrlRegs.PIEACK.bit.ACK4 = 1;//开外设中断应答
}
unsigned int mean(unsigned int* a)//double
{
    unsigned int Max_pinlvf=0,i,j;
//    long sum,temp;
//    sum=a[0]+a[1]+a[2]+a[3]+a[4];
//    temp=(sum&0x0FFFF)/5;
//    for (i=1;i<4;i++)
//    {
//        if ( abs(a[i]-temp) > abs(a[Max_pinlvf]-temp) )
//        Max_pinlvf=i;
//    }
//    temp=sum-pinlvf1[Max_pinlvf];
//    temp=(temp&0x0FFFF)>>2;
//    return temp&0x0FFFF;   
    
    for (i=0;i<4;i++)
        for (j=i;j<4;j++)
        {
            if ( a[i]>a[j] ) 
            {
                Max_pinlvf=a[i];
                a[i]=a[j];
                a[j]=Max_pinlvf;
            }
        }
    return  a[2];
}


void adjust_speed(void)
{
    unsigned int Frequency_Left_Wheel,Frequency_Right_Wheel,temp;
    Frequency_Left_Wheel=mean(pinlvf2);
    Frequency_Right_Wheel=mean(pinlvf1);
    if ( Frequency_Left_Wheel>Frequency_Right_Wheel )
    {
        temp=Frequency_Left_Wheel-Frequency_Right_Wheel;
        if (temp>0x50)
        {
            if( ajust_num>=5 )
            ajust_num-=0x1;
            LinearMotion(zhuangtai-1);
        }
    }
    else
    {
        temp=Frequency_Right_Wheel-Frequency_Left_Wheel;
        if (temp>0x50)
        {
            if( ajust_num<=0x3000 )
            ajust_num+=0x1;
            LinearMotion(zhuangtai-1);
        }
    }
}




void vehicle_self_control(void)
{   
    if(ScibRegs.SCIFFRX.bit.RXFIFST!=0)
    {
        ReadDistance();
        if (distance<Mindistance)
            mov_key=0;             
        else
        	mov_key=1<<5;
    }
    //A13|=GpioDataRegs.GPADAT.bit.GPIOA13;
    //A13|=GpioDataRegs.GPADAT.bit.GPIOA13;
//mov_key=mov_key|(GpioDataRegs.GPADAT.bit.GPIOA14<<4)|(GpioDataRegs.GPADAT.bit.GPIOA15<<3)|(GpioDataRegs.GPFDAT.bit.GPIOF3<<2)|(A13<<1)|GpioDataRegs.GPFDAT.bit.GPIOF2;
    mov_key=mov_key|(GpioDataRegs.GPADAT.bit.GPIOA14<<4)|(GpioDataRegs.GPADAT.bit.GPIOA15<<3)|(GpioDataRegs.GPFDAT.bit.GPIOF3<<2)|(GpioDataRegs.GPFDAT.bit.GPIOF2<<1)|GpioDataRegs.GPADAT.bit.GPIOA13;
    switch (mov_key)
    {
        /*case 32: case 33: case 34: case 35: case 36: case 37: case 38: case 40: case 41: case 48: case 49: case 56: case 57: case 62: case 63: 
        zhuangtai=1;
        LinearMotion(0);
        
        break;
        case 1: case 3: case 5: case 7: case 25: case 27:  case 39: 
        LinearMotion(1);
        zhuangtai=2;
        break;

        case 2: case 6: case 8: case 10: case 18: case 22: case 26: case 50: case 51: case 54: case 55:  
        zhuangtai=3;
        TurnAround(1,100);
        break;
        
        case 4: case 12: case 14: case 16: case 20: case 24: case 28: case 29: case 30: case 31: case 44: case 45: case 46: case 47:
        zhuangtai=4;
        TurnAround(0,100);
        break;
        
        case 61: case 60: case 43: case 42:
        zhuangtai=1;
        LinearMotion(0);
        Delay(400);
        zhuangtai=4;
        TurnAround(0,200);
        break;
        
        case 59: case 58: case 53: case 52:
        zhuangtai=1;
        LinearMotion(0);
        Delay(400);
        zhuangtai=3;
        TurnAround(1,200);
        break;
        
        case 17: case 19: case 21: case 23:
        zhuangtai=2;
        LinearMotion(1);
        Delay(400);
        zhuangtai=4;
        TurnAround(0,400);
        break;
        
        case 9: case 11: case 13: case 15:
        zhuangtai=2;
        LinearMotion(1);
        Delay(400);
        zhuangtai=3;
        TurnAround(1,400);
        break;
        */
        /*
        case 32: case 33: case 34: case 36: case 38: case 56: case 57: case 62: case 63: // 前
        zhuangtai=1;
        LinearMotion(0);
        
        break;
        case 1://后 
        LinearMotion(1);
        zhuangtai=2;
        break;

        case 2: case 6: case 16: case 18: case 20: case 22: case 28: case 54: case 55:  //左
        zhuangtai=3;
        TurnAround(1,100);
        break;
        
        case 4: case 8: case 10: case 12: case 14: case 24: case 26: case 30: case 31: case 46: case 47://右
        zhuangtai=4;
        TurnAround(0,100);
        break;
        
        case 40: case 41: case 42: case 43:  case 44: case 45: case 60: case 61: //右前
        zhuangtai=1;
        LinearMotion(0);
        Delay(400);
        zhuangtai=4;
        TurnAround(0,200);
        break;
        
        case 48: case 49: case 50: case 51: case 52: case 53: case 58: case 59://左前
        zhuangtai=1;
        LinearMotion(0);
        Delay(400);
        zhuangtai=3;
        TurnAround(1,200);
        break;
        
        case 3: case 7: case 9: case 11: case 15: case 19: case 25: case 27: case 35: case 39://后右
        zhuangtai=2;
        LinearMotion(1);
        Delay(400);
        zhuangtai=4;
        TurnAround(0,400);
        break;
        
        case 5: case 13: case 17: case 21: case 23: case 29: case 37://后左
        zhuangtai=2;
        LinearMotion(1);
        Delay(400);
        zhuangtai=3;
        TurnAround(1,400);
        break;
        */
        case 32: case 33: case 34: case 36: case 38: case 56: case 57: case 62: case 63: // 前
        zhuangtai=1;
        LinearMotion(0);
        
        break;
        case 1: case 9: case 18://后 
        LinearMotion(1);
        zhuangtai=2;
        break;

        case 2: case 6: case 8: case 10: case 16: case 22: case 26: case 54: case 55:  //左
        zhuangtai=3;
        TurnAround(1,100);
        break;
        
        case 4: case 12: case 14: case 20: case 24: case 28: case 29: case 30: case 31: case 46: case 47://右
        zhuangtai=4;
        TurnAround(0,100);
        break;
        
        case 40: case 41: case 42: case 43:  case 44: case 45: case 60: case 61: //右前
        zhuangtai=1;
        LinearMotion(0);
        Delay(200);
        zhuangtai=4;
        TurnAround(0,200);
        break;
        
        case 48: case 49: case 50: case 51: case 52: case 53: case 58: case 59://左前
        zhuangtai=1;
        LinearMotion(0);
        Delay(200);
        zhuangtai=3;
        TurnAround(1,200);
        break;
        
        case 5: case 7: case 13: case 15: case 21: case 25: case 37: case 39://后右
        zhuangtai=2;
        LinearMotion(1);
        Delay(300);
        zhuangtai=4;
        TurnAround(0,400);
        break;
        
        case 3: case 11: case 17: case 19: case 23: case 27: case 35://后左
        zhuangtai=2;
        LinearMotion(1);
        Delay(300);
        zhuangtai=3;
        TurnAround(1,400);
        break;
        case 0:
        zhuangtai=0;Stop();
        break;
    }

}

/*
void vehicle_self_control(void)
{
              if(ScibRegs.SCIFFRX.bit.RXFIFST!=0)
              ReadDistance();
              
              if (GpioDataRegs.GPADAT.bit.GPIOA15==1)         //     !C
              {
                  if ( (distance>Mindistance) && (GpioDataRegs.GPADAT.bit.GPIOA14==1) )           //  AB
                  {
                      //Stop();
                      zhuangtai=1;LinearMotion(0);
                  }
                  else 
                  {
                    Stop();
                    if ( (GpioDataRegs.GPADAT.bit.GPIOA15==1)&&( (GpioDataRegs.GPADAT.bit.GPIOA14==0)||(distance<=Mindistance) )  )
                    zhuangtai=4;TurnAround(0,0x3000);
                  }
              
              }
              else                                            //     C
              {
                  if (GpioDataRegs.GPADAT.bit.GPIOA14==1)             //    B
                  {
                      Stop();
                      zhuangtai=3;TurnAround(1,0x3000);
                  }
                  else if (GpioDataRegs.GPADAT.bit.GPIOA14==0)      //   !B
                 {
                      //Stop();
                      zhuangtai=2;LinearMotion(1);
                  }
              }
} */
//===========================================================================
//Nomore.
//===========================================================================