c

通过插补计算实现在51单片机上控制X-Y机械手臂绘图的功能

#include <math.h>
#define uint unsigned int
#define uchar unsigned char
int disp[8];

int para[10],ptr=0;

void modeproc(),keyproc();
int xm,ym,xe,ye,xx=0,yy=0,fm,xo,yo,x2,y2,shun=0,ni=1,i;


void dingwei()
{
 while(xx!=xm||yy!=ym)
     {
         if(xx<xm)
            {
                xx++;

                printf("XX:%d,YY:%d,F:%d----- ",xx,yy,fm);;;
            }
         if(xx>xm)
             {
                xx--;

                printf("XX:%d,YY:%d,F:%d----- ",xx,yy,fm);;;
            }
         if(yy<ym)
            {
                yy++;
               printf("XX:%d,YY:%d,F:%d----- ",xx,yy,fm);;;
            }
         if(yy>ym)
             {
                yy--;
               printf("XX:%d,YY:%d,F:%d----- ",xx,yy,fm);;;
            }

     }
}

void mode_zx()
{
    int xiangxian;
    ptr=0;
     xm=para[0];
     ym=para[1];
     dingwei();
     xm=0;
     ym=0;
     xe=para[2]-para[0];
     ye=para[3]-para[1];

     xx=para[2];
     yy=para[3];
     fm=0;    
          if((ye>ym)&&(xe>xm))
     xiangxian=1;
         if((ye>ym)&&(xe<xm))
     {xiangxian=2;}
          if((ye<ym)&&(xe<xm))
     {xiangxian=3;}
         if((ye<ym)&&(xe>xm))
     {xiangxian=4;}



        if(xe==0)
        {if(ye>=0)
             {while(ym!=ye)
                 {
                    ym++;
                    printf("X:%d,Y:%d,F:%d----- ",xm,ym,fm);;;;
                }
            }
         if(ye<0)
             {
                while(ym!=ye)
                    {
                        ym--;
                        printf("X:%d,Y:%d,F:%d----- ",xm,ym,fm);;;;
                    }
            }
         }


       if(ye==0)
        {if(xe>=0)
             {while(xm!=xe)
                 {
                    xm++;
                    printf("X:%d,Y:%d,F:%d----- ",xm,ym,fm);;;;
                }
            }
         if(xe<0)
             {
                while(xm!=xe)
                    {
                        xm--;
                        printf("X:%d,Y:%d,F:%d----- ",xm,ym,fm);;;
                    }
            }
         }

          while(xm!=xe||ym!=ye)
     {
          if(fm>=0)
         {
             if(xiangxian==2||xiangxian==3)
                {
                    xm--;
                    printf("X:%d,Y:%d,F:%d----- ",xm,ym,fm);;;;
                }

            if(xiangxian==1||xiangxian==4)
                {
                    xm++;
                    printf("X:%d,Y:%d,F:%d----- ",xm,ym,fm);;;;

                }

                    fm=fm-abs(ye);
        
         }

          if(fm<0)
         {
             if(xiangxian==1||xiangxian==2)
                {
                    ym++;
                    printf("X:%d,Y:%d,F:%d----- ",xm,ym,fm);;;;
                }

            if(xiangxian==3||xiangxian==4)
                {
                    ym--;
                    printf("X:%d,Y:%d,F:%d----- ",xm,ym,fm);;;;

                }

                    fm=fm+abs(xe);
        
         }
         }

    
}

void mode_yh(int fangxiang)
{
    int xiangxian,flag;

    xm=para[0];
    ym=para[1];
     dingwei();
     xm=para[0]-para[4];
     ym=para[1]-para[5];
     xe=para[2]-para[4];
     ye=para[3]-para[5];
     xo=0;
     yo=0;

     xx=para[2];
     yy=para[3];
     fm=0;
     if(xm==xe&ym==ye)
             flag=1;
      while(xm!=xe||ym!=ye||flag)
     {     flag=0;
          if((ym-yo)>0&&(xm-xo)>0)
     xiangxian=1;
         if((ym-yo)>0&&(xm-xo)<0)
     xiangxian=2;    
          if((ym-yo)<0&&(xm-xo)<0)
     xiangxian=3;
         if((ym-yo)<0&&(xm-xo)>0)
     xiangxian=4;    
         if(ym==yo&&(xm-xo)>0)
     xiangxian=5;
         if(xm==xo&&(ym-yo)>0)
     xiangxian=6;
         if(ym==yo&&(xm-xo)<0)
     xiangxian=7;
         if(xm==xo&&(ym-yo)<0)
     xiangxian=8;

      
          if(fm>=0)
         {
             if((fangxiang==1&&xiangxian==1)||(fangxiang==0&&xiangxian==4)||(xiangxian==5))
                {   fm=fm-2*abs(xm)+1;
                    xm--;
                    printf("X:%d,Y:%d,F:%d----- ",xm,ym,fm);;;;

                }

            if((xiangxian==3&&fangxiang==1)||(xiangxian==2&&fangxiang==0)||(xiangxian==7))
                {   fm=fm-2*abs(xm)+1;
                    xm++;
                    printf("X:%d,Y:%d,F:%d----- ",xm,ym,fm);;;;


                }
            if((fangxiang==0&&xiangxian==1)||(fangxiang==1&&xiangxian==2)||(xiangxian==6))
                {fm=fm-2*abs(ym)+1;
                    ym--;
                    printf("X:%d,Y:%d,F:%d----- ",xm,ym,fm);;;;

                }

            if((xiangxian==3&&fangxiang==0)||(xiangxian==4&&fangxiang==1)||(xiangxian==8))
                {fm=fm-2*abs(ym)+1;
                    ym++;
                    printf("X:%d,Y:%d,F:%d----- ",xm,ym,fm);;;;


                }
                
        
         }

         else
         {
                 if((fangxiang==0&&(xiangxian==3||xiangxian==8))||(fangxiang==1&&(xiangxian==2||xiangxian==6)))
                {fm=fm+2*abs(xm)+1;
                    xm--;
                    printf("X:%d,Y:%d,F:%d----- ",xm,ym,fm);;;;

                }

            if(((xiangxian==1||xiangxian==6)&&fangxiang==0)||((xiangxian==4||xiangxian==8)&&fangxiang==1))
                {fm=fm+2*abs(xm)+1;
                    xm++;
                    printf("X:%d,Y:%d,F:%d----- ",xm,ym,fm);;;;


                }
            if((fangxiang==1&&(xiangxian==3||xiangxian==7))||(fangxiang==0&&(xiangxian==4||xiangxian==5)))
                {fm=fm+2*abs(ym)+1;
                    ym--;
                    printf("X:%d,Y:%d,F:%d----- ",xm,ym,fm);;;;

                }

            if(((xiangxian==1||xiangxian==5)&&fangxiang==1)||((xiangxian==2||xiangxian==7)&&fangxiang==0))
                {fm=fm+2*abs(ym)+1;
                    ym++;
                    printf("X:%d,Y:%d,F:%d----- ",xm,ym,fm);;;;


                }
        
         }
         }

}

void fuwei()
{
    xm=0;
    ym=0;
    dingwei();
}








void main()
{ 
    para[0]=300;
    para[1]=100;
    para[2]=1300;
    para[3]=100;  mode_zx();   getch();

    para[0]=1300;
    para[1]=100;
    para[2]=1300;
    para[3]=200;   mode_zx();      getch();

    para[0]=1300;
    para[1]=200;
    para[2]=1200;
    para[3]=200;    mode_zx();    getch();

    para[0]=1200;
    para[1]=200;
    para[2]=1100;
    para[3]=400;  mode_zx();       getch();

    para[0]=1100;
    para[1]=400;
    para[2]=500;
    para[3]=400;    mode_zx();      getch();

    para[0]=500;
    para[1]=400;
    para[2]=400;
    para[3]=200;      mode_zx();     getch();

    para[0]=400;
    para[1]=200;
    para[2]=300;
    para[3]=200;     mode_zx();       getch();

    para[0]=300;
    para[1]=200;
    para[2]=300;
    para[3]=100;   mode_zx();        getch();

    para[0]=400;
    para[1]=100;
    para[2]=400;
    para[3]=100;
    para[4]=550;
    para[5]=100;    mode_yh(0);       getch();

    para[0]=900;
    para[1]=100;
    para[2]=900;
    para[3]=100;
    para[4]=1050;
    para[5]=100;     mode_yh(0);       getch();


 getch();
}