mmotor
Programm mit Ablauf
Dependencies: EPOS2 mbed-rtos mbed
Fork of
Projekt_Kugelbahn
by 
mmotor
Diff: main.cpp
- Revision:
- 13:cc43e546f310
- Parent:
- 12:9e90d7a5a8c6
- Child:
- 14:bee23de5dff0
--- a/main.cpp Tue Oct 18 14:16:09 2016 +0000
+++ b/main.cpp Wed Oct 19 10:02:02 2016 +0000
@@ -9,24 +9,7 @@
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 counter_s = false;
-int counter_s2 = false;
-*/
DigitalIn usv_ready(p23);
InterruptIn buffering(p24);
DigitalIn replace_battery(p25);
@@ -36,6 +19,9 @@
DigitalOut led3(LED3);
DigitalOut led4(LED4);
+bool epos1_homing = false;
+bool epos3_homing = false;
+
bool node1_homed = false;
bool node2_homed = false;
bool node3_homed = false;
@@ -47,18 +33,23 @@
bool Power_ein = true;
bool node5_grundposition = false;
-int homing = false;
-int ready = false;
-int ausfahren = false;
-int einfahren = false;
-int usv = false;
+bool ausfahren = false;
+bool einfahren = false;
+bool usv = false;
int current_step = 1; //HOMING
int ausfahren_step = 1;
int einfahren_step = 1;
int usv_step = 1;
-int ErrorState;
+int ErrorState1;
+int ErrorState2;
+int ErrorState3;
+int ErrorState4;
+int ErrorState5;
+int ErrorState6;
+int ErrorState7;
+float speed = 1.2;
-void interrupt_usv(){
+void interrupt_usv(){ //Funktion, wird aufgeruffen durch den interrupt des USV's
current_step = 5;
wait_ms(10);
}
@@ -109,64 +100,30 @@
led3 = 0;
led4 = 0;
MyEpos5.SetDigOut(4, 1);
-
-/* usv_ready.mode(PullUp);
- buffering.mode(PullUp);
- replace_battery.mode(PullUp);
-*/
- //************** Initialisierung **************//
-
- /* HOMING METHODEN:
- Homing Method 7 :Home Switch Positive Speed & Index
- Homing Method 11:Home Switch Negative Speed & Index
- Homing Method 23: Home Switch Positive Speed
- Homing Method 27: Home Switch Negative Speed
- Homing Method -3: Current Threshold Positive Speed
- Homing Method -4: Current Threshold Negative Speed
-
- */
- //Motortype 10: EC-Motor
-
-/* MyEpos1.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
- MyEpos1.SetHomingPar(-4, 100, 100, 1000); //Set Homing Parameter (homing mode, speed, acceleration, offset) #EINGESTELLT
- MyEpos1.Reset();
-
- MyEpos2.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
- MyEpos2.SetHomingPar(27, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)
- MyEpos2.Reset();
-
- MyEpos3.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
- MyEpos3.SetHomingPar(-3, 100, 100, 1000); //Set Homing Parameter (homing mode, speed, acceleration, offset) #EINGESTELLT
- MyEpos3.Reset();
-
- MyEpos4.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
- MyEpos4.SetHomingPar(23, 100, 100, 8000); //Set Homing Parameter (homing mode, speed, acceleration, offset)#EINGESTELLT
- MyEpos4.Reset();
-
- MyEpos5.SetPar(10,4430, 7, 3500, 38); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
- MyEpos5.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)#EINGESTELLT
- MyEpos5.Reset();
-
- MyEpos6.SetPar(10,3440, 1, 12000, 3.8); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)#EINGESTELLT
- MyEpos6.SetHomingPar(11, 100, 100, -7000); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)#EINGESTELLT
- MyEpos6.Reset();
-
- MyEpos7.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
- MyEpos7.SetHomingPar(23, 100, 100, 0); //Set Homing Parameter (homing mode, speed, acceleration, offset)
- MyEpos7.Reset();
-*/
-
+
while(1){
if(MyEpos1.GetError() != 0 or MyEpos2.GetError() != 0 or MyEpos3.GetError() != 0 or MyEpos4.GetError() != 0 or MyEpos5.GetError() != 0 /*or MyEpos6.GetError() != 0 or MyEpos7.GetError() != 0*/){
current_step = 7; //FEHLER
- ErrorState = MyEpos5.GetError();
+ ErrorState1 = MyEpos1.GetError();
+ ErrorState2 = MyEpos2.GetError();
+ ErrorState3 = MyEpos3.GetError();
+ ErrorState4 = MyEpos4.GetError();
+ ErrorState5 = MyEpos5.GetError();
wait(0.1);
- pc.printf("ErrorState: %d\r\n", ErrorState);
+ pc.printf("ErrorState Panels/Kopf: Panel Aussen Linkss=%d, Panel Innen Links=%d, Panel Aussen Rechts=%d, Panel Innen Rechts=%d, Kopf=%d\r\n", ErrorState1, ErrorState2, ErrorState3, ErrorState4, ErrorState5);
}
- switch (current_step){
+/* if(MyEpos6.GetError() != 0 or MyEpos7.GetError() != 0){
+ current_step = 76; //FEHLER
+ ErrorState6 = MyEpos6.GetError();
+ ErrorState7 = MyEpos7.GetError();
+ wait(0.1);
+ pc.printf("Bohrgestell/Bohrer ErrorState: Bohrgestell=%d, Bohrer=%d\r\n", ErrorState6, ErrorState7);
+ }
+
+*/ switch (current_step){
case 1://HOMING:
led1 = 1;
@@ -191,6 +148,9 @@
MyEpos5.MoveAbsolute(-60000,800,100,100); //Kopf
pc.printf("Fahren auf -60000\n");
while(MyEpos5.TargetReached()!=1){
+ if(MyEpos5.GetError() == 1){
+ break;
+ }
}
pc.printf("Target reached\n");
node5_homed = true;
@@ -198,17 +158,38 @@
}
if (node1_homed == false){
- pc.printf("Homing1\n");
- MyEpos1.Homing(); //Solarpanel_Aussen_Links
- node1_homed = true;
+ if(epos1_homing == false){
+ MyEpos1.MoveVelocity(-400, 100, 100); //Fährt auf Endschalter vor Beginn mit Stromhoming
+ epos1_homing = true;
+ }
+/* if(MyEpos1.GetError() != 0){
+ current_step = 7;
+ break;
+ }
+*/ if(MyEpos1.GetDigIn(4) == 1){
+ pc.printf("Homing1\n");
+ MyEpos1.Homing(); //Solarpanel_Aussen_Links
+ node1_homed = true;
+ break;
+ }
break;
}
if (node3_homed == false){
- pc.printf("Homing3\n");
- MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
- node3_homed = true;
+ if (epos3_homing == false){
+ MyEpos3.MoveVelocity(400, 100, 100); //Fährt auf Endschalter vor Beginn mit Stromhoming
+ epos3_homing = true;
+ }
+/* if(MyEpos3.GetError() != 0){
+ current_step = 7;
+ break;
+ }
+*/ if(MyEpos3.GetDigIn(4) == 1){
+ pc.printf("Homing3\n");
+ MyEpos3.Homing(); //Solarpanel_Aussen_Rechts
+ node3_homed = true;
+ }
break;
}
@@ -220,7 +201,10 @@
break;
}
- if (node1_homed == true and node2_homed == false){
+ if (node1_homed == true and node2_homed == false){
+ if (node5_grundposition == false){
+ MyEpos5.MoveAbsolute(-60000,800,100,100); //Kopf
+ }
pc.printf("Homing2\n");
MyEpos2.Homing(); //Solarpanel_Innen_Links
node2_homed = true;
@@ -244,10 +228,13 @@
break;
}
- if (node2_homed==1 and node4_homed==1 and node5_grundposition == false){
- MyEpos5.MoveAbsolute(0,800,100,100); //Kopf
+ if (node2_homed == true and node4_homed == true and node5_grundposition == false){
+ MyEpos5.MoveAbsolute(500,800*speed,100,100); //Kopf
wait(0.1);
while(MyEpos5.TargetReached() != 1){
+ if(MyEpos5.GetError() == 1){
+ break;
+ }
wait(0.1);
}
node5_grundposition = true;
@@ -256,8 +243,8 @@
if (node1_homed == true and node2_homed == true and node3_homed == true and node4_homed == true and node5_homed == true and node6_homed == true and node7_homed == true){
pc.printf("Homing beendet\n");
- node5_grundposition == false;
- Power_ein == true;
+ node5_grundposition = false;
+ Power_ein = true;
MyEpos1.Power(0);
MyEpos2.Power(0);
MyEpos3.Power(0);
@@ -266,9 +253,6 @@
MyEpos5.Power(0);
current_step = 2;//BEREIT;
}
-
-
-
led1 = 0;
@@ -288,138 +272,21 @@
MyEpos4.Power(1);
MyEpos5.Power(1);
einfahren = false;
- current_step = 6;
+ ausfahren_step = 1;
+ current_step = 3;
wait(0.5);
}
wait_ms(10);
led2 = 0;
break;
-
-
- case 3://START_AUSFAHREN:
-
- break;
-
-
- case 5://USV:
- pc.printf("USV\n");
- led4 = 1;
- MyEpos5.SetDigOut(4,1);
- switch (usv_step){
- case 1:
-// wait(0.1);
-// if (node1_homed == true){
-// MyEpos1.MoveAbsolute(0,500,500,500); //Panel_Aussen_Links
- MyEpos1.MoveAbsolute(2000,500,500,500); //Panel_Aussen_Links
-// wait(0.1);
-// }
-// if (node3_homed == true){
-// MyEpos3.MoveAbsolute(0,500,500,500); //Panel_Aussen_Rechts
- MyEpos3.MoveAbsolute(-2000,500,500,500); //Panel_Aussen_Rechts
- wait(0.1);
-// }
-/* if (node7_homed == true){
- MyEpos7.MoveAbsolute(0,500,500,500); // Bohrer
- wait(0.1);
- }
- */// wait(0.1);
-// usv_step = 2;
- if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
- usv_step = 2;
- wait(0.1);
- }
- break;
-
- case 2:
-// if (MyEpos1.TargetReached()==1){
-// wait(1);
- MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
- MyEpos2.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
-// wait_ms(10);
-// }
-// if (MyEpos3.TargetReached()==1){
-// wait(1);
- MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Links
- MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
-// wait_ms(10);
-// }
-// wait(0.1);
-/* if (node4_homed == true and MyEpos3.TargetReached()==1){
- wait(1);
- MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
- wait_ms(10);
- }
- if (node6_homed == true and MyEpos7.TargetReached()==1){
- wait(1);
- MyEpos6.MoveAbsolute(0,500,500,500); // Bohrer
- wait_ms(10);
- }
-*/ wait(0.1);
- if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1 /*and MyEpos6.TargetReached()==1*/){
- usv_step = 3;
- wait(0.1);
- }
- break;
-
- case 3:
- MyEpos5.MoveAbsolute(0,800,500,500); //Kopf
- wait(0.1);
- if (MyEpos5.TargetReached()==1){
- usv_step = 4;
- wait(0.1);
- }
- break;
-
- case 4:
- /*while (MyEpos5.TargetReached()==0){
- wait_ms(10);
- }*/
-/* MyEpos1.Power(0);
- MyEpos2.Power(0);
- MyEpos3.Power(0);
- MyEpos4.Power(0);
- MyEpos5.Power(0);
- MyEpos6.Power(0);
- MyEpos7.Power(0);
- usv_step = 5;
-*/ MyEpos1.Power(0);
- MyEpos2.Power(0);
- MyEpos3.Power(0);
- MyEpos4.Power(0);
- MyEpos5.Power(0);
- MyEpos6.Power(0);
- MyEpos7.Power(0);
- usv_step = 5;
- wait(0.1);
- break;
-
- case 5:
- if(buffering == 1){
- led3 = 1;
- } else {
- led3 = 0;
- current_step=1;
- usv_step=1;
- ausfahren_step=1;
- einfahren_step=1;
- }
- /*if (buffering == 0){
- current_step = 1;
- usv_step = 1;
- }*/
- wait(1);
- break;
- }
- led4 = 0;
- break;
-
- case 6://Test
+
+ case 3://ABLAUF
// pc.printf("Läuft...\n");
led3 = 1;
if (einfahren == false){
switch (ausfahren_step){
case 1:
- MyEpos5.MoveAbsolute(-150000,800,100,100); //Kopf
+ MyEpos5.MoveAbsolute(-150000,800*speed,100*speed,100*speed); //Kopf
ausfahren_step = 2;
wait(1);
break;
@@ -428,17 +295,17 @@
if (MyEpos5.TargetReached()==1){
MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
- MyEpos2.MoveAbsolute(310000,500,100,100); //Panel Innen Links
- MyEpos4.MoveAbsolute(-310000,500,100,100); //Panel Innen Rechts
+ MyEpos2.MoveAbsolute(310000,500*speed,100*speed,100*speed); //Panel Innen Links
+ MyEpos4.MoveAbsolute(-310000,500*speed,100*speed,100*speed); //Panel Innen Rechts
ausfahren_step = 3;
wait(1);
}
break;
case 3:
- if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1){
- MyEpos1.MoveAbsolute(318000,500,100,100); //Panel Aussen Links
- MyEpos3.MoveAbsolute(-318000,500,100,100); //Panel Aussen Rechts
+ if (MyEpos2.GetActualPos() > 280000 and MyEpos4.GetActualPos() < -280000){
+ MyEpos1.MoveAbsolute(318000,500*speed,100*speed,100*speed); //Panel Aussen Links
+ MyEpos3.MoveAbsolute(-318000,500*speed,100*speed,100*speed); //Panel Aussen Rechts
ausfahren_step = 4;
wait(1);
@@ -452,6 +319,8 @@
wait(1);
}
break;
+
+
}//switch (ausfahren_step)
}//if (einfahren == false)
@@ -460,21 +329,18 @@
ausfahren_step = 1;
switch (einfahren_step){
case 1:
-/* MyEpos1.MoveAbsolute(1000,500,100,100); //Panel Aussen Links
- MyEpos3.MoveAbsolute(-1000,500,100,100); //Panel Aussen Rechts
-*/ MyEpos1.MoveAbsolute(2000,500,100,100); //Panel Aussen Links
- MyEpos3.MoveAbsolute(-2000,500,100,100); //Panel Aussen Rechts
+ MyEpos1.MoveAbsolute(4000,500*speed,100*speed,100*speed); //Panel Aussen Links
+ MyEpos3.MoveAbsolute(-4000,500*speed,100*speed,100*speed); //Panel Aussen Rechts
einfahren_step = 2;
wait(0.1);
break;
case 2:
- if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
-// wait(0.1);
+ if (MyEpos1.GetActualPos() < 20000 and MyEpos3.GetActualPos() > -20000){
MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
- MyEpos2.MoveAbsolute(0,500,100,100); //Panel Innen Links
- MyEpos4.MoveAbsolute(0,500,100,100); //Panel Innen Rechts
+ MyEpos2.MoveAbsolute(0,500*speed,100*speed,100*speed); //Panel Innen Links
+ MyEpos4.MoveAbsolute(0,500*speed,100*speed,100*speed); //Panel Innen Rechts
einfahren_step = 3;
wait(0.1);
}
@@ -482,8 +348,7 @@
case 3:
if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1){
- MyEpos5.MoveAbsolute(0,500,100,100); //Kopf
-// wait(1);
+ MyEpos5.MoveAbsolute(1000,500*speed,100*speed,100*speed); //Kopf
MyEpos1.Power(0);
MyEpos2.Power(0);
MyEpos3.Power(0);
@@ -505,11 +370,104 @@
}//switch(einfahren_step)
}//if einfahren == true)
led3 = 0;
+ break;
+
+ case 5://USV:
+ pc.printf("USV\n");
+ led4 = 1;
+ MyEpos5.SetDigOut(4,1);
+ switch (usv_step){
+ case 0:
+ MyEpos1.Power(1);
+ MyEpos2.Power(1);
+ MyEpos3.Power(1);
+ MyEpos4.Power(1);
+ MyEpos5.Power(1);
+ MyEpos6.Power(1);
+ MyEpos7.Power(1);
+ usv_step = 1;
+ break;
+
+ case 1:
+ MyEpos1.MoveAbsolute(2000,500,500,500); //Panel_Aussen_Links
+ MyEpos3.MoveAbsolute(-2000,500,500,500); //Panel_Aussen_Rechts
+ wait(0.1);
+
+ if (MyEpos1.TargetReached()==1 and MyEpos3.TargetReached()==1){
+ usv_step = 2;
+ wait(0.1);
+ }
+ break;
+
+ case 2:
+
+ MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
+ MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Links
+
+ MyEpos2.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
+ MyEpos4.MoveAbsolute(0,500,500,500); //Panel_Innen_Links
+ wait(0.1);
+
+ if (MyEpos2.TargetReached()==1 and MyEpos4.TargetReached()==1 /*and MyEpos6.TargetReached()==1*/){
+ usv_step = 3;
+ wait(0.1);
+ }
+ break;
+
+ case 3:
+ MyEpos5.MoveAbsolute(0,800,500,500); //Kopf
+ wait(0.1);
+ if (MyEpos5.TargetReached()==1){
+ usv_step = 4;
+ wait(0.1);
+ }
+ break;
+
+ case 4:
+ MyEpos1.Power(0);
+ MyEpos2.Power(0);
+ MyEpos3.Power(0);
+ MyEpos4.Power(0);
+ MyEpos5.Power(0);
+ MyEpos6.Power(0);
+ MyEpos7.Power(0);
+ usv_step = 5;
+ wait(0.1);
+ break;
+
+ case 5:
+ if(buffering == 1){
+ led3 = 1;
+ } else {
+ led3 = 0;
+ Power_ein = true;
+ current_step=1;
+ usv_step=0;
+ ausfahren_step=1;
+ einfahren_step=1;
+ }
+ wait(1);
+ break;
+ }//switch(usv_step)
+ led4 = 0;
+ break;
+
+ case 6:
+ pc.printf("Fehler RESET Bohrer\n");
+ MyEpos5.MoveVelocity(0, 500, 500);
+ MyEpos6.MoveVelocity(0, 500, 500);
+ wait(3);
+ MyEpos5.Reset();
+ MyEpos5.Power(1);
+ MyEpos6.Reset();
+ MyEpos6.Power(1);
+ //node1_homed = false;
+ //node2_homed = false;
+ current_step = 3;
break;
case 7:
- pc.printf("Fehler RESET\n");
-// if(MyEpos5.GetDigIn(3) == 1){
+ pc.printf("Fehler RESET\n");
MyEpos1.MoveVelocity(0, 500, 500);
MyEpos2.MoveVelocity(0, 500, 500);
MyEpos3.MoveVelocity(0, 500, 500);
@@ -526,26 +484,28 @@
MyEpos4.Power(1);
MyEpos5.Reset();
MyEpos5.Power(1);
+ epos1_homing = false;
+ epos3_homing = false;
node1_homed = false;
node2_homed = false;
node3_homed = false;
node4_homed = false;
node5_homed = false;
+ Power_ein = true;
+ if(MyEpos2.GetActualPos() < 100000 or MyEpos4.GetActualPos() > -100000){
+ MyEpos1.MoveAbsolute(0,500,100,100); //Panel Aussen Links
+ MyEpos3.MoveAbsolute(0,500,100,100); //Panel Aussen Rechts
+ MyEpos2.MoveAbsolute(0,500,100,100); //Panel Innen Links
+ MyEpos4.MoveAbsolute(0,500,100,100); //Panel Innen Rechts
+ while (MyEpos2.TargetReached() != 1 or MyEpos4.TargetReached() != 1){
+ if(MyEpos2.GetError() == 1 or MyEpos4.GetError() == 1){
+ break;
+ }
+ }
+ }
current_step = 1;
wait(0.1);
-// }
-
-// MyEpos5.MoveVelocity(0,500,500);
- wait(0.1);
-
-/* MyEpos1.Reset();
- MyEpos2.Reset();
- MyEpos3.Reset();
- MyEpos4.Reset();
- MyEpos5.Reset();
- wait(0.5);
- */
-
+
break;
}//switch (current_step)
}//while