2 Achsen Motorsteuerung mit Fehlerauswertung
Dependencies: EPOS2 mbed-rtos mbed
Revision 1:c53aafa72b36, committed 2016-07-28
- Comitter:
- ODEM
- Date:
- Thu Jul 28 11:50:16 2016 +0000
- Parent:
- 0:aaf583a75b97
- Commit message:
- Fehlerauswertung ; 2 Achsen Motorsteuerung
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Jul 28 06:24:33 2016 +0000 +++ b/main.cpp Thu Jul 28 11:50:16 2016 +0000 @@ -23,8 +23,11 @@ int ActualCurrent = 0; int DemandCurrent = 0; int counter=0; +int counter2=0; bool start = true; -bool counter_s =false; +int current_State = 2; +int counter_s = false; +int counter_s2 = false; @@ -41,12 +44,12 @@ // while(1){ - // pc.printf("start\n"); + // 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()); + // pc.printf("%d\n", MyEpos.GetError()); while (MyEpos.TargetReached() == 0) { ActualPos = MyEpos.GetActualPos(); @@ -77,6 +80,7 @@ //pc.printf("%10.d,%10.d,%10.hd,%10.hd\n", ActualPos, DemandPos, ActualCurrent, DemandCurrent); } + counter=0; //} } @@ -91,15 +95,12 @@ 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"); @@ -110,54 +111,119 @@ 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); + 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 (MyEpos.GetError()==true || MyEpos2.GetError()==true && counter<3) { - pc.printf("FEHLER!!!\n"); + + + //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"); - if (counter_s==false){ - counter++; - counter_s=true; - } - goto restart; + current_State = 3;//ERROR; + + } + if (MyEpos2.GetError()==true&& counter2<3) { + pc.printf("FEHLER-Node2!!!\n"); - } - - if(counter>2){ - MyEpos.Power(0); - MyEpos2.Power(0); - } + 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; + } - Motor2(MyEpos2); - Motor1(MyEpos); - + break; + } + + // pc.printf("%d\n", MyEpos.GetError()); }//while }//main \ No newline at end of file