Programm mit Ablauf
Dependencies: EPOS2 mbed-rtos mbed
Fork of Projekt_Kugelbahn by
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