File content as of revision 1:c53aafa72b36:

#include "mbed.h"
#include "EPOS2.h"
//#include "RobotControl.h"

//***********************************************************************************************************************************//
//*** Global_Var  *******************************************************************************************************************//
//***********************************************************************************************************************************//

Serial pc(USBTX, USBRX);             // (tx, rx)


CAN can(p9, p10);                    // Can Pin def


int velocity = 3000;
int acceleration = 50000; 
int deceleration = 50000;

int anzahl_zyklen = 1000000;
int absolvierte_zyklen = 0;  
int ActualPos = 0;
int DemandPos = 0;
int ActualCurrent = 0;
int DemandCurrent = 0;
int counter=0;
int counter2=0;
bool start = true;
int current_State = 2;
int counter_s = false;
int counter_s2 = false;


  
void Motor1 (EPOS2 MyEpos){
        pc.printf("Motor1\n");
        
       
        MyEpos.SetDigOut(3, 0);        
        wait(2);
       
      
       
                
   // while(1){    
 
   
            //  pc.printf("start\n"); 
                MyEpos.SetDigOut(3, 1);                                                     //(1=Enable, 0=Disable)
                MyEpos.MoveAbsolute(500,20,500,deceleration); 
                MyEpos.SetDigOut(3, 0);                                   //(TargetPosition, PosSpeed, acceleration, deceleration)
                wait(1);
            // pc.printf("%d\n", MyEpos.GetError()); 
                
            while (MyEpos.TargetReached() == 0) {
                    ActualPos = MyEpos.GetActualPos();
                    DemandPos = MyEpos.GetDemandPos();
                    ActualCurrent = MyEpos.GetActualCurrentAveraged();
                   DemandCurrent = MyEpos.GetDemandCurrent();
                    //wait_ms(100); 
                    MyEpos.SetDigOut(3, 0);                         
            }  // pc.printf("%10.d,%10.d,%10.hd,%10.hd\n", ActualPos, DemandPos, ActualCurrent, DemandCurrent);
                 
            
                wait(0.5);
           
                MyEpos.SetDigOut(3, 1); 
                //wait_ms(100);
                MyEpos.MoveAbsolute(0,3000,3000,3000);
                MyEpos.SetDigOut(3, 0); 
               wait(1);
                
    
                while (MyEpos.TargetReached() == 0) {
                    ActualPos = MyEpos.GetActualPos();
                    DemandPos = MyEpos.GetDemandPos();
                    ActualCurrent = MyEpos.GetActualCurrent();
                    DemandCurrent = MyEpos.GetDemandCurrent();
                    //wait_ms(100);    
                    MyEpos.SetDigOut(3, 0);                       
                    //pc.printf("%10.d,%10.d,%10.hd,%10.hd\n", ActualPos, DemandPos, ActualCurrent, DemandCurrent);
                    
                }  
                    counter=0;
   //}        
}  


void Motor2(EPOS2 MyEpos2){
     pc.printf("Motor2\n");
     
       
                 
                    wait_ms(100) ;          
                    MyEpos2. MoveVelocity(1000, 80, 1000);         
                    wait(0.1); 
                    
                   
              
}  
int main()                                      ///////// Main
{
        pc.baud(9600);
        
        wait(2);                                    //Wartezeit bis Epos2 70/10 aufgestartet ist
        
        pc.printf("Initialisation CAN\n");
        
        can.frequency(1000000);                     //Define Can baud in bit/s
    
        CANopen canOpen(&can, 0.001);               //Define CanOpen Network(can function, periode of the CANopen driver in sec)
        canOpen.start();                            //Start defined CanOpen Network
    
        EPOS2 MyEpos(&canOpen, 1);
        EPOS2 MyEpos2(&canOpen, 2);                  //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)                 
       
        
        
        pc.printf("Initialisation abgeschlossen\n");
           
        while(1){ 
    
         
       
        //if(counter>2){
       //    MyEpos.Power(0);
       //    MyEpos2.Power(0);
       // }
           
     
            
   
    switch (current_State){
            
        case 1://STATE_NORMAL:
        
            if  (MyEpos.GetError()==true && counter<3) {
                pc.printf("FEHLER-Node1!!!\n");
               
                current_State = 3;//ERROR;
                
         }       
         if    (MyEpos2.GetError()==true&& counter2<3) {
                pc.printf("FEHLER-Node2!!!\n");
               
                current_State = 5;//ERROR;
                
         }       
         
         
          if (counter2>2){
                    MyEpos2.Power(0);
                    MyEpos.Power(0);
            }
            
             if (counter>2){
                    MyEpos2.Power(0);
                    MyEpos.Power(0);
            }
            Motor2(MyEpos2);   
            Motor1(MyEpos);  
            break;     
        case 2:// INITIALISIERUNG:
        
        
         //************** Initialisierung  **************//
        counter_s=false;
        counter_s2=false;
    
        

        pc.printf("Fehlerfall %d\n", counter) ;
        pc.printf("Fehlerfall2 %d\n", counter2) ;
        MyEpos.SetPar(10,2660, 1, 3100, 2.76);        //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
        MyEpos.SetHomingPar(23, 10, 10, 0);           //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)                                        
        MyEpos.Reset();                               //Reset the EPOS2
        wait(2);
        MyEpos2.SetPar(10,936, 8, 1000, 29.8);        //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
        MyEpos2.SetHomingPar(23, 10, 10, 0);          //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)                                        
        MyEpos2.Reset();                              //Reset the EPOS2
        wait(2);                                    
        pc.printf("Homing\n");                      
        
        MyEpos.Power(1);  
        pc.printf("power\n");
        MyEpos2.Power(1);                             //Enable the Epos for Homing
        pc.printf("Power2\n");
        wait(2);
        MyEpos.Homing(); 
        
                
         }                                            //do Homing and wait untill homing is done // MyEpos.Power(1); 
        wait_ms(100);
        current_State = 1;//STATE_NORMAL;
        break;  
        case 3://ERROR1:
        
        
               
            if     (counter_s==false){
                    counter++;
                    counter_s=true;
            
                    current_State = 2;// INITIALISIERUNG;
                    } 
            
            break;  
        
        case 4: //STATE_OFF:
        
        MyEpos.Power(0); 
        MyEpos2.Power(0); 
            
        break;  
        
        case 5://ERROR2
            if      (counter_s2==false){
                    counter2++;
                    counter_s2=true;
            
                    current_State = 2;// INITIALISIERUNG;
                    }
           
                    break;
 }         
          
          
           // pc.printf("%d\n", MyEpos.GetError()); 
        }//while
}//main