悬挂系统.c

通过利用两台步进电机实现悬挂物体运动轨迹的控制

 #include <reg52.h>
 #include <math.h>
 #define h 115
 #define a 15
 #define b 95
 #define R 25

   sbit P0_0=P0^0;
   sbit P0_1=P0^1;
   sbit P0_2=P0^2;
   sbit P0_3=P0^3;
   sbit P0_4=P0^4;
   sbit m1=P3^6;
   sbit m2=P3^7;




  unsigned char code mov0[]={0x55,0x99,0xaa,0x66};/*M1和M2 正转  */
   unsigned char code mov1[]={0x65,0xa9,0x9a,0x56};/*M1正转和M2反转*/
   unsigned char code mov2[]={0x56,0x9a,0xa9,0x65};/*M1 反转M2正转*/
   unsigned char code mov3[]={0x66,0xaa,0x99,0x55};/*M1和M2 反转*/

  
   unsigned char code dispi[8]={0x00,0x10,0x20,0x30,0x40,0x50,0x60,0x70};
   unsigned char disp[8]={0,0,0,0,0,0,0,0};
   

   int set_x=40,set_y=40;
   float y[2];
   int x[2];
   float dlx,dly;
   
   int n1,n2,sg1,sg2;

    
   int j0=0,j1=0;
 
  
 int modle=0,gn_buf,go,i0,j0;

void dot_to_dot(void);
void draw_circle(void);
void disp_2(void);
void disp_1(void);

  
void delay(void)
    {unsigned char i,j;
	 for(i=0;i<10;i++)
	 for (j=0;j<249;j++);
     }
   
void suan(unsigned char q)
	{ float dlx,dly;
	 dlx=sqrt((h-y[q+1])*(h-y[q+1])+(a+x[q+1])*(a+x[q+1]))-sqrt((h-y[q])*(h-y[q])+(a+x[q])*(a+x[q]));
         dly=sqrt((h-y[q+1])*(h-y[q+1])+(b-x[q+1])*(b-x[q+1]))-sqrt((h-y[q])*(h-y[q])+(b-x[q])*(b-x[q]));


      if(dly<0)sg1=0;
	  else sg1=1;
	  n1=fabs(dly)*295/11.13;
      
      if(dlx<0)sg2=0;
	  else sg2=1;
	  n2=fabs(dlx)*295/11.13;
     }

void keyscan(void)
     {if (P0_0==0)
       {delay();
          if (P0_0==0)
           while(!P0_0)disp_1();
            gn_buf++;
           if(gn_buf==5)gn_buf=0;
         }
       
     if (P0_1==0)
        {delay();
           if (P0_1==0)
             while(!P0_1)disp_1();
              set_x++;
             if(set_x>=80)set_x=0;
          }
      
     if (P0_2==0)
        {delay();
          if (P0_2==0)
            while(!P0_2)disp_1();
            set_y++;
            if(set_y>=100)set_y=0;
         }
     
     if (P0_3==0)
        {delay();
         if (P0_3==0)
          while(!P0_3)disp_1();
         if(go){go=0; m1=0; m2=0;}
		 else go=1;
         }
     if (P0_4==0)
        {delay();
         if(P0_4==0)
         while(!P0_4)disp_1();
         modle++;
         if(modle==4)modle=0;
         }

    }

void disp_1(void)
	 {disp[0]=set_x%10;
	  disp[1]=set_x/10;
	  disp[2]=set_y%10;
	  disp[3]=set_y/10;
          disp[4]=modle;
          disp[6]=go;
          disp[7]=gn_buf;
	  P1=disp[i0]+dispi[i0];
	  i0++;
	  if(i0==8)i0=0;
	 
	 }
void main(void)
    {
     TMOD=0x11;
     TH0=(65536-5000)/256;
     TL0=(65536-5000)%256;
     TH1=(65536-5000)/256;
     TL1=(65536-5000)%256;
     ET1=1;
     ET0=1;
     EA=1;

     while(1)
       {keyscan();
        disp_1();
         if(go)
          {
		    if(gn_buf==0) // //手动调零
             {TR0=1;
			  TR1=0;
			  m1=1;
			  m2=1;
              }
            
		  
		  else if(gn_buf==1) //画弧线
      	      {TR0=1;
			   TR1=0;
			   m1=1;
			   m2=0;
		       }
	      else if(gn_buf==2)
              {dot_to_dot();
               /*  TR1=0;
				 x[0]=0;
				 y[0]=0;
				 x[1]=set_x;
				 y[1]=set_y;
				 suan(0);
				 TR1=1;
				 m1=1;
				 m2=1;
				 while(n1||n2)*/

                gn_buf=4; }

        else if(gn_buf==3)
              {  draw_circle(); gn_buf=4; }
            }
		else {TR0=0;TR1=0;}
      }
    }

void timer0(void) interrupt 1 using 0
	{TH0=(65536-5000)/256;
         TL0=(65536-5000)%256;
	  if(gn_buf==0)
      {
	  if (modle==0)
	    P2=mov0[j0];
	  if (modle==1)
	    P2=mov1[j0];
          if (modle==2)
	    P2=mov2[j0];
	  if (modle==3)
	    P2=mov3[j0];}
	  if(gn_buf==1)
	    P2=mov0[j0];

	  j0++;
	  if (j0==4)
	  {j0=0;}
	 }
void timer1(void) interrupt 3 using 1
    {TH1=(65536-5000)/256;
     TL1=(65536-5000)%256;
	 

     if(sg1==0&&sg2==0)P2=mov0[j1];
     if(sg1==0&&sg2==1)P2=mov1[j1];
     if(sg1==1&&sg2==0)P2=mov2[j1];
     if(sg1==1&&sg2==1)P2=mov3[j1];
     j1++;
     if (j1==4)
       {j1=0;
         if(n1>0)n1--;
         if(n2>0)n2--;
         }
     if (n1==0)m1=0;
     if (n2==0)m2=0;
 	  
	 
      }
void dot_to_dot(void)//（0，0）到 (set_x,set_y)移?
	  
    	{       TR1=0;
		TR0=0;  
		x[0]=0,y[0]=0;x[1]=0;
		while(x[1]!=set_x) //if it is not  finished  ,then go on
		{  
   	         x[1]=x[1]+2;
	    	 y[1]=(float)(x[1])*set_y/set_x;
		 suan(0);
           	 x[0]=x[1],y[0]=y[1];
		 TR1=1;
		 m1=1;
               m2=1;
             while(n1||n2)  disp_2();  //等待电机执行完毕
                 TR1=0;
		  }
	          m1=0;
			  m2=0;
              TR1=0;
		      TR0=0; 	 
	         
	  }
void draw_circle(void)
    	  {   TR1=0;
	      TR0=0;
	      x[0]=0,y[0]=0;
	      x[1]=x[0],y[1]=y[0];
	
	      while(x[1]!=set_x-R) // 移动到水平作图点
	       {  
   	          x[1]=x[1]+1;
		  y[1]=(float)(x[1])*set_y/(set_x-R);
		  suan(0);
            	  x[0]=x[1],y[0]=y[1];
			   TR1=1;
			   m1=1;
               m2=1;
             while(n1||n2)  disp_1();  //等待电机执行完毕
             TR1=0;    
		  }
		
         while(x[1]!=(R+set_x) )//作上半周
		 {
		  x[1]=x[1]+2;
		  y[1]=set_y+sqrt(R*R-(set_x-x[1])*(set_x-x[1]));
		 suan(0);
		 x[0]=x[1];y[0]=y[1];
		       TR1=1;
			   m1=1;
               m2=1;
             while(n1||n2)  disp_2();  //等待电机执行完?
			 TR1=0;
		 }
          
         while(x[1]!=(set_x-R) )//作下半周
	  		 {
		  x[1]=x[1]-2;
		  y[1]=set_y-sqrt(R*R-(x[1]-set_x)*(x[1]-set_x));
		 suan(0);
		 x[0]=x[1];y[0]=y[1];
		       TR1=1;
			   m1=1;
               m2=1;
             while(n1||n2)  disp_2();  //等待电机执行完?
			 TR1=0;
		 }
	   }
	   	 void disp_2(void)
	 {disp[0]=set_x%10;
	  disp[1]=set_x/10;
	  disp[2]=set_y%10;
	  disp[3]=set_y/10;
      disp[4]=(int)x[0]%10;
      disp[5]=(int)x[0]/10;
      disp[6]=(int)y[0]%10;
      disp[7]=(int)y[0]/10;
	  P1=disp[i0]+dispi[i0];
	  i0++;
	  if(i0==8)i0=0;
	 
	 }