threeaxes.c

三軸伺服馬達控制程式利用軟體DDA方式讀回目前位置然後以取樣時間計算出下一點位置取誤差後利用DA將命令電壓送出做三軸控制

//This program is for 3 axes pcb
#define DDA_TIME 0.001   
#define CONST 0x800   
#define DA1LO 0x8060
#define DA1HI 0x8061
#define DA2LO 0x8062
#define DA2HI 0x8063
#define DA3LO 0x8064
#define DA3HI 0x8065
#define ENC10 0x8000  //Cnt1 Lo
#define ENC11 0x8001  //Cnt1 Mid
#define ENC12 0x8002  //Cnt1 Hi
#define ENC13 0x8003  //Cnt1 Status
#define ENC20 0x8020  //Cnt2 Lo
#define ENC21 0x8021  //Cnt2 Mid
#define ENC22 0x8022  //Cnt2 Hi
#define ENC23 0x8023  //Cnt2 Status
#define ENC30 0x8040  //Cnt3 Lo
#define ENC31 0x8041  //Cnt3 Mid
#define ENC32 0x8042  //Cnt3 Hi
#define ENC33 0x8043  //Cnt2 Status  
#define BAUD9600 64 
#define CR 0x0d    
#define ESC 0x1b  
#define CMDADR 0x1f
#define RUNADR 0x20
 
#include <io8515.h>  
#include "Variable.h"
#include "EEprom.h"   
#include <ina90.h>   
#include "Pos.h" 
#include "Interrupt.h"
#include "InitIO.h"   

main(void)
{           
  unsigned char Cmd,Data;  
  int i,j,k;
  unsigned int Acc,JogSpeed;  
  long XPos,YPos,ZPos,Spd; 
  float AccStep;
  InitHardware();  
  InitVar(); 
  PORTC&=0x7f;
  PORTC|=0x80; 
  _SEI();
  i=0;j=0;k=0;
  XPos=0,YPos=0,ZPos=0;
        
  while (1) {   
    if (Start==1) { 
      Cmd=EERead(CMDADR);
      switch (Cmd) { 
        case 0://Motion    
          //PORTD|=0x20;  
          Data=EERead(0);
          Acc=0;
          Acc+=Data; 
          Acc<<=8; 
          Data=EERead(1);
          Acc+=Data; 
           
          Data=EERead(0x0b);
          XPos=0;
          XPos+=Data; 
          XPos<<=8; 
          Data=EERead(0x0c);
          XPos+=Data;
          XPos<<=8;   
          Data=EERead(0x0d);
          XPos+=Data;          
          
          Data=EERead(0x0e);
          YPos=0;
          YPos+=Data; 
          YPos<<=8; 
          Data=EERead(0x0f);
          YPos+=Data;
          YPos<<=8;   
          Data=EERead(0x10);
          YPos+=Data;     
          
          Data=EERead(0x11);
          ZPos=0;
          ZPos+=Data; 
          ZPos<<=8; 
          Data=EERead(0x12);
          ZPos+=Data;
          ZPos<<=8;   
          Data=EERead(0x13);
          ZPos+=Data;     
          
          Data=EERead(0x14);
          Spd=0;
          Spd+=Data; 
          Spd<<=8; 
          Data=EERead(0x15);
          Spd+=Data;
          Spd<<=8;   
          Data=EERead(0x16);
          Spd+=Data;  
           
          PosMove(XPos,YPos,ZPos,Spd,Acc/1000.0);
          Start=0;  
          EEWrite(RUNADR,Start);  
          //PORTD&=0xdf;  
          break;
        case 1: //X Jog
          Data=EERead(0);
          Acc=0;
          Acc+=Data; 
          Acc<<=8; 
          Data=EERead(1);
          Acc+=Data; 
            
          Data=EERead(0x17);
          if ((Data<=100) && (Data>0)) JogSpeed=20*Data;
          else JogSpeed=100;
          
          Data=EERead(0x1d);
          JogSec=0;
          JogSec+=Data; 
          JogSec<<=8; 
          Data=EERead(0x1e);
          JogSec+=Data;   
           
          AccStep=(float)Acc/(float)JogSpeed;
          
          AccMSec=AccStep;
          AccStart=1;
          for (i=0;i<JogSpeed;i++) { 
            DAError1=i;  
            while (AccMSec!=0);   
            AccMSec=AccStep;
          }          
          AccStart=0;
          
          JogStart=1;
          while (JogSec!=0);
          JogStart=0; 
           
          AccMSec=AccStep; 
          AccStart=1;
          for (i=JogSpeed;i>0;i--) { 
            DAError1=i;  
            while (AccMSec!=0);
            AccMSec=AccStep;
          }                      
          DAError1=0;
          AccStart=0;
          Start=0;  
          EEWrite(RUNADR,Start);    
          break;   
        case 2: //Y Jog 
          Data=EERead(0);
          Acc=0;
          Acc+=Data; 
          Acc<<=8; 
          Data=EERead(1);
          Acc+=Data; 
            
          Data=EERead(0x18);   
          if ((Data<=100) && (Data>0)) JogSpeed=20*Data;
          else JogSpeed=20;  
           
          Data=EERead(0x1d);
          JogSec=0;
          JogSec+=Data; 
          JogSec<<=8; 
          Data=EERead(0x1e);
          JogSec+=Data;   
                    
          AccStep=(float)Acc/(float)JogSpeed;
          
          AccMSec=AccStep;
          AccStart=1;
          for (i=0;i<JogSpeed;i++) { 
            DAError1=i;     //ゼㄓ璶эΘDAError2
            while (AccMSec!=0);   
            AccMSec=AccStep;
          }          
          AccStart=0;
          
          JogStart=1;
          while (JogSec!=0);
          JogStart=0; 
           
          AccMSec=AccStep; 
          AccStart=1;
          for (i=JogSpeed;i>0;i--) { 
            DAError1=i;     //ゼㄓ璶эΘDAError2 
            while (AccMSec!=0);
            AccMSec=AccStep;
          }                      
          DAError1=0;       //ゼㄓ璶эΘDAError2
          AccStart=0;
          Start=0;  
          EEWrite(RUNADR,Start);    
          break;    
        case 3: //Z Jog 
          Data=EERead(0);
          Acc=0;
          Acc+=Data; 
          Acc<<=8; 
          Data=EERead(1);
          Acc+=Data; 
            
          Data=EERead(0x19); 
          if ((Data<=100) && (Data>0)) JogSpeed=20*Data;
          else JogSpeed=100;  
                    
          Data=EERead(0x1d);
          JogSec=0;
          JogSec+=Data; 
          JogSec<<=8; 
          Data=EERead(0x1e);
          JogSec+=Data;   
           
          AccStep=(float)Acc/(float)JogSpeed;
          
          AccMSec=AccStep;
          AccStart=1;
          for (i=0;i<JogSpeed;i++) { 
            DAError1=i;     //ゼㄓ璶эΘDAError3
            while (AccMSec!=0);   
            AccMSec=AccStep;
          }          
          AccStart=0;
          
          JogStart=1;
          while (JogSec!=0);
          JogStart=0; 
           
          AccMSec=AccStep; 
          AccStart=1;
          for (i=JogSpeed;i>0;i--) { 
            DAError1=i;     //ゼㄓ璶эΘDAError3 
            while (AccMSec!=0);
            AccMSec=AccStep;
          }                      
          DAError1=0;       //ゼㄓ璶эΘDAError3
          AccStart=0;
          Start=0;  
          EEWrite(RUNADR,Start);    
          break;
        case 4: //X Home
          Data=EERead(0);
          Acc=0;
          Acc+=Data; 
          Acc<<=8; 
          Data=EERead(1);
          Acc+=Data; 
            
          Data=EERead(0x1a);
          if ((Data<=100) && (Data>0)) JogSpeed=20*Data;
          else JogSpeed=10;
          
          AccStep=(float)Acc/(float)JogSpeed;
          
          AccMSec=AccStep;
          AccStart=1;
          for (i=0;i<JogSpeed;i++) { 
            DAError1=-i;  
            while (AccMSec!=0);   
            AccMSec=AccStep;
          }          
          AccStart=0;
          
          while (Start==1);  
          DAError1=0;  
          Start=0;  
          EEWrite(RUNADR,Start);    
          break;  
        case 5: //Y Home
          Data=EERead(0);
          Acc=0;
          Acc+=Data; 
          Acc<<=8; 
          Data=EERead(1);
          Acc+=Data; 
            
          Data=EERead(0x1b);
          if ((Data<=100) && (Data>0)) JogSpeed=20*Data;
          else JogSpeed=10;
          
          AccStep=(float)Acc/(float)JogSpeed;
          
          AccMSec=AccStep;
          AccStart=1;
          for (i=0;i<JogSpeed;i++) { 
            DAError1=-i;    //ゼㄓ璶эΘDAError2  
            while (AccMSec!=0);   
            AccMSec=AccStep;
          }          
          AccStart=0;
          
          while (Start==1);         
          DAError1=0;   
          Start=0;  
          EEWrite(RUNADR,Start);    
          break;          
        case 6: //Z Home
          Data=EERead(0);
          Acc=0;
          Acc+=Data; 
          Acc<<=8; 
          Data=EERead(1);
          Acc+=Data; 
            
          Data=EERead(0x1c);
          if ((Data<=100) && (Data>0)) JogSpeed=20*Data;
          else JogSpeed=10;
          
          AccStep=(float)Acc/(float)JogSpeed;
          
          AccMSec=AccStep;
          AccStart=1;
          for (i=0;i<JogSpeed;i++) { 
            DAError1=-i;    //ゼㄓ璶эΘDAError3  
            while (AccMSec!=0);   
            AccMSec=AccStep;
          }          
          AccStart=0;
          
          while (Start==1);         
          DAError1=0;   
          Start=0;  
          EEWrite(RUNADR,Start);    
          break;                          
        default:   
          Start=0;  
          EEWrite(RUNADR,Start);    
          break;
      }
    }
  }   
}           
          
       
    //PORTA=i; 
    
    /*
    PosMove(500000.0,0.0,0.0,100000.0,0.5);  
    for (j=0;j<100;j++){
      for (i=0;i<30000;i++);  
    }
    PosMove(0.0,0.0,0.0,50000.0,0.5);  
    for (j=0;j<100;j++){
      for (i=0;i<30000;i++);  
    } 
  }
}     
    */  
       

/*  玻ネ亏睛猧
    k=j;
    k=k+CONST;
    PORTA=k;
    PORTC=k>>8;  
    j--;
    if (j<-2048) j=0;
*/