File content as of revision 0:aaf583a75b97:

#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;
bool start = true;
bool counter_s =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);
                    
                }  
   //}        
}  


void Motor2(EPOS2 MyEpos2){
     pc.printf("Motor2\n");
     
       
                 
                    wait_ms(100) ;          
                    MyEpos2. MoveVelocity(1000, 80, 1000);         
                    wait(0.1); 
                    
                   
                
}  
int main()                                      ///////// Main
{
restart:                 
        //************** Initialisierung  **************//
        counter_s=false;
        pc.baud(9600);
        pc.printf("Fehlerfall %d\n", counter) ;
        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)
    
        //RobotControl robotControl(&MyEpos, &MyEpos2);
        //robotControl.start();

        
        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);
        
        
        pc.printf("Initialisation abgeschlossen\n");
           
        while(1){ 
    
         if      (MyEpos.GetError()==true || MyEpos2.GetError()==true && counter<3) {
                pc.printf("FEHLER!!!\n");
               
         if     (counter_s==false){
                counter++;
                counter_s=true;
        }
                goto restart;
               
        }     
       
        if(counter>2){
           MyEpos.Power(0);
           MyEpos2.Power(0);
        }
           
            Motor2(MyEpos2);   
            Motor1(MyEpos); 
            
           // pc.printf("%d\n", MyEpos.GetError()); 
        }//while
}//main