main.cpp

00001 #include "mbed.h"
00002 #include "EPOS2.h"
00003 //#include "RobotControl.h"
00004 
00005 //***********************************************************************************************************************************//
00006 //*** Global_Var  *******************************************************************************************************************//
00007 //***********************************************************************************************************************************//
00008 
00009 Serial pc(USBTX, USBRX);             // (tx, rx)
00010 
00011 
00012 CAN can(p9, p10);                    // Can Pin def
00013 
00014 
00015 int velocity = 3000;
00016 int acceleration = 50000; 
00017 int deceleration = 50000;
00018 
00019 int anzahl_zyklen = 1000000;
00020 int absolvierte_zyklen = 0;  
00021 int ActualPos = 0;
00022 int DemandPos = 0;
00023 int ActualCurrent = 0;
00024 int DemandCurrent = 0;
00025 int counter=0;
00026 int counter2=0;
00027 bool start = true;
00028 int current_State = 2;
00029 int counter_s = false;
00030 int counter_s2 = false;
00031 
00032 
00033   
00034 void Motor1 (EPOS2 MyEpos){
00035         pc.printf("Motor1\n");
00036         
00037        
00038         MyEpos.SetDigOut(3, 0);        
00039         wait(2);
00040        
00041       
00042        
00043                 
00044    // while(1){    
00045  
00046    
00047             //  pc.printf("start\n"); 
00048                 MyEpos.SetDigOut(3, 1);                                                     //(1=Enable, 0=Disable)
00049                 MyEpos.MoveAbsolute(500,20,500,deceleration); 
00050                 MyEpos.SetDigOut(3, 0);                                   //(TargetPosition, PosSpeed, acceleration, deceleration)
00051                 wait(1);
00052             // pc.printf("%d\n", MyEpos.GetError()); 
00053                 
00054             while (MyEpos.TargetReached() == 0) {
00055                     ActualPos = MyEpos.GetActualPos();
00056                     DemandPos = MyEpos.GetDemandPos();
00057                     ActualCurrent = MyEpos.GetActualCurrentAveraged();
00058                    DemandCurrent = MyEpos.GetDemandCurrent();
00059                     //wait_ms(100); 
00060                     MyEpos.SetDigOut(3, 0);                         
00061             }  // pc.printf("%10.d,%10.d,%10.hd,%10.hd\n", ActualPos, DemandPos, ActualCurrent, DemandCurrent);
00062                  
00063             
00064                 wait(0.5);
00065            
00066                 MyEpos.SetDigOut(3, 1); 
00067                 //wait_ms(100);
00068                 MyEpos.MoveAbsolute(0,3000,3000,3000);
00069                 MyEpos.SetDigOut(3, 0); 
00070                wait(1);
00071                 
00072     
00073                 while (MyEpos.TargetReached() == 0) {
00074                     ActualPos = MyEpos.GetActualPos();
00075                     DemandPos = MyEpos.GetDemandPos();
00076                     ActualCurrent = MyEpos.GetActualCurrent();
00077                     DemandCurrent = MyEpos.GetDemandCurrent();
00078                     //wait_ms(100);    
00079                     MyEpos.SetDigOut(3, 0);                       
00080                     //pc.printf("%10.d,%10.d,%10.hd,%10.hd\n", ActualPos, DemandPos, ActualCurrent, DemandCurrent);
00081                     
00082                 }  
00083                     counter=0;
00084    //}        
00085 }  
00086 
00087 
00088 void Motor2(EPOS2 MyEpos2){
00089      pc.printf("Motor2\n");
00090      
00091        
00092                  
00093                     wait_ms(100) ;          
00094                     MyEpos2. MoveVelocity(1000, 80, 1000);         
00095                     wait(0.1); 
00096                     
00097                    
00098               
00099 }  
00100 int main()                                      ///////// Main
00101 {
00102         pc.baud(9600);
00103         
00104         wait(2);                                    //Wartezeit bis Epos2 70/10 aufgestartet ist
00105         
00106         pc.printf("Initialisation CAN\n");
00107         
00108         can.frequency(1000000);                     //Define Can baud in bit/s
00109     
00110         CANopen canOpen(&can, 0.001);               //Define CanOpen Network(can function, periode of the CANopen driver in sec)
00111         canOpen.start();                            //Start defined CanOpen Network
00112     
00113         EPOS2 MyEpos(&canOpen, 1);
00114         EPOS2 MyEpos2(&canOpen, 2);                  //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)                 
00115        
00116         
00117         
00118         pc.printf("Initialisation abgeschlossen\n");
00119            
00120         while(1){ 
00121     
00122          
00123        
00124         //if(counter>2){
00125        //    MyEpos.Power(0);
00126        //    MyEpos2.Power(0);
00127        // }
00128            
00129      
00130             
00131    
00132     switch (current_State){
00133             
00134         case 1://STATE_NORMAL:
00135         
00136             if  (MyEpos.GetError()==true && counter<3) {
00137                 pc.printf("FEHLER-Node1!!!\n");
00138                
00139                 current_State = 3;//ERROR;
00140                 
00141          }       
00142          if    (MyEpos2.GetError()==true&& counter2<3) {
00143                 pc.printf("FEHLER-Node2!!!\n");
00144                
00145                 current_State = 5;//ERROR;
00146                 
00147          }       
00148          
00149          
00150           if (counter2>2){
00151                     MyEpos2.Power(0);
00152                     MyEpos.Power(0);
00153             }
00154             
00155              if (counter>2){
00156                     MyEpos2.Power(0);
00157                     MyEpos.Power(0);
00158             }
00159             Motor2(MyEpos2);   
00160             Motor1(MyEpos);  
00161             break;     
00162         case 2:// INITIALISIERUNG:
00163         
00164         
00165          //************** Initialisierung  **************//
00166         counter_s=false;
00167         counter_s2=false;
00168     
00169         
00170 
00171         pc.printf("Fehlerfall %d\n", counter) ;
00172         pc.printf("Fehlerfall2 %d\n", counter2) ;
00173         MyEpos.SetPar(10,2660, 1, 3100, 2.76);        //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
00174         MyEpos.SetHomingPar(23, 10, 10, 0);           //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)                                        
00175         MyEpos.Reset();                               //Reset the EPOS2
00176         wait(2);
00177         MyEpos2.SetPar(10,936, 8, 1000, 29.8);        //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
00178         MyEpos2.SetHomingPar(23, 10, 10, 0);          //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)                                        
00179         MyEpos2.Reset();                              //Reset the EPOS2
00180         wait(2);                                    
00181         pc.printf("Homing\n");                      
00182         
00183         MyEpos.Power(1);  
00184         pc.printf("power\n");
00185         MyEpos2.Power(1);                             //Enable the Epos for Homing
00186         pc.printf("Power2\n");
00187         wait(2);
00188         MyEpos.Homing(); 
00189         
00190                 
00191          }                                            //do Homing and wait untill homing is done // MyEpos.Power(1); 
00192         wait_ms(100);
00193         current_State = 1;//STATE_NORMAL;
00194         break;  
00195         case 3://ERROR1:
00196         
00197         
00198                
00199             if     (counter_s==false){
00200                     counter++;
00201                     counter_s=true;
00202             
00203                     current_State = 2;// INITIALISIERUNG;
00204                     } 
00205             
00206             break;  
00207         
00208         case 4: //STATE_OFF:
00209         
00210         MyEpos.Power(0); 
00211         MyEpos2.Power(0); 
00212             
00213         break;  
00214         
00215         case 5://ERROR2
00216             if      (counter_s2==false){
00217                     counter2++;
00218                     counter_s2=true;
00219             
00220                     current_State = 2;// INITIALISIERUNG;
00221                     }
00222            
00223                     break;
00224  }         
00225           
00226           
00227            // pc.printf("%d\n", MyEpos.GetError()); 
00228         }//while
00229 }//main