Diff: main.cpp

Revision:

0:aaf583a75b97

Child:

1:c53aafa72b36

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jul 28 06:24:33 2016 +0000
@@ -0,0 +1,163 @@
+#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
\ No newline at end of file