subr.c

这是Mc9sdg128的舵机调试程序,适合进行智能小汽车的调试.

#include <hidef.h>              /* common defines and macros */
#include <mc9s12dg128.h>       /* derivative information */
#include "SubR.h"
#include "SYS.h"   
/*****************variables define ***********************/
extern unsigned char AD[9];						 //AD结果值					                
extern unsigned char  rudder, speed,stop,i,buf;
extern char lastposition,position,positionbuf,pola;
extern unsigned int  time;
unsigned int timeset;
char po_level;
#pragma CODE_SEG DEFAULT
void GetADValues()
{
    while(!ATD0STAT0_SCF);
    AD[0] = ATD0DR0L;
    AD[1] = ATD0DR1L;
    AD[2] = ATD0DR2L;
    AD[3] = ATD0DR3L;
    AD[4] = ATD0DR4L;
    AD[5] = ATD0DR5L;
    AD[6] = ATD0DR6L;
    AD[7] = ATD0DR7L;       
}
void CalcuPos()
{
   //buf=PORTB & 0x05;
   // if(buf==0x00)po_level=2;
   //if(buf==0x01)po_level=3;
   //if(buf==0x04)po_level=4;
   // if(buf==0x05)po_level=5;
   
   if(PORTB_BIT0 == 0)po_level=3;
   if(PORTB_BIT0 == 1)po_level=4;
    
    lastposition=position;
    positionbuf=127;

    for(i=0;i<8;i++)
    {
        if((AD[i]<GATE)&&(AD[i+1]<GATE))
        {
            positionbuf=(AD[i]-AD[i+1])/(MAX/po_level)+(i*2-6)*po_level;
            break;
        }
        else if(AD[i]<GATE)
        {
            positionbuf=(i*2-7)*po_level;
            break;
        } 
    }
    if(positionbuf !=127)
    {
        if(positionbuf>0)
            pola=1;
       else pola=-1;
    }     
    if(PORTA_BIT4 ^ PORTA_BIT5)
    positionbuf=pola*42;
    if(PORTA_BIT2 ^ PORTA_BIT3)
    positionbuf=pola*(29+po_level*2);
    if(PORTA_BIT0 ^ PORTA_BIT1)
    positionbuf=pola*(16+po_level*4);     
    if(!(PORTA_BIT0 | PORTA_BIT1))
    positionbuf=127;        
    if(positionbuf!=127)
        position=positionbuf; 
 
    /*  ad8--PA6  ad9--PA4     ad10--PA2        ad11--PA0
       ad12--PA1  ad13--PA3   ad14--PA5        ad15--PA7   
         8	9	10	11	7	6	5	4	3	2	1	0	12	13	14	15
    */
}
void CtrlSpeed()
{  
     #if BEFORE
     if((Mabs(lastposition)<5) && (Mabs(position)>4))
     {
        SpeedSet(0);
        StopSet(0);
        for(i=0;i<100;i++)
        delay_5ms();
     }
     else if(Mabs(position)<5)
     {
        speed=187;
        SpeedSet(speed);
        StopSet(speed);
     }
     else
     {
        speed=181+Mabs(position)/3;
        StopSet(speed);
        SpeedSet(speed);
     }
    //version 5 code
     /*
    //if(Mabs(position)<10)
    if(Mabs(position)<3)
    {
        speed=120-Mabs(position)*4 ;
        SpeedSet(speed);
    }
    else
    {
        //speed=93+2*Mabs(position);
        speed=105+Mabs(position)/2;
        SpeedSet(speed);
    }                                                                                                       
    */ //version 4 code 
    #endif
    #if !BEFORE

    buf=Mabs(position);
     if(Max(Mabs(lastposition),9)<buf)
     {
        SpeedSet(0);
        StopSet(0);
        
        if(PORTB_BIT1 == 0)
        {
            for(i=0;i<2+buf/2;i++)
                delay_5ms();
        }
        else
        {
            for(i=0;i<24-buf/2;i++)
                delay_5ms();
        }
        StopSet(200);
     }
    
    if((PORTB & 0x50) == 0x00)
        timeset=0x75-buf/4;
    if((PORTB & 0x50) == 0x10)
        timeset=0x75-buf/3;
    if((PORTB & 0x50) ==0x40)
        timeset=0x70-buf/4;
    if((PORTB & 0x50)== 0x50)
        timeset=0x70-buf/3;
    
    /*if((PORTB & 0x50) == 0x00)
        timeset=0x75-buf/8;
    if((PORTB & 0x50) == 0x10)
        timeset=0x75-buf/4;
    if((PORTB & 0x50) ==0x40)
        timeset=0x75-buf/2;
    if((PORTB & 0x50)== 0x50)
        timeset=0x75; 
   */
 
    if(timeset <time)
        {
            if(speed<200)
            speed++;
        }
    else 
        {
            if(speed>120)
            speed--; 
        }
        SpeedSet(speed);  
    #endif
}
void CtrlRudder()
{
    if(positionbuf!=127)
    RudderSet(213-position);
    else RudderSet(213-pola*42);
}
char Mabs(char s)
 {
    if(s>=0)
    return s;
    else
    return 0-s;
 }
char Max(char a,char b)
 {
      if(a>b)return a;
      return b;
 }
 void delay_5ms(void)//5 ms delay based on an 8MHz clock
 {
    int i;
    for(i=0;i<50;i++)
    {
        delay_100us();
    }
 }
void delay_100us(void)//100us delay based on an 8MHz clock
 {
    int i;
    for(i=0;i<50;i++)
    {
        asm("nop");
    }
 }